qcsrc/server/pathlib/main.qc

   1 #include "main.qh"
   2
   3 #include <server/defs.qh>
   4 #include <server/miscfunctions.qh>
   5 #include "pathlib.qh"
   6 #include "utility.qh"
   7 #include <common/turrets/util.qh>
   8 #include "../command/common.qh"
   9
  10 void pathlib_deletepath(entity start)
  11 {
  12     if(!start)
  13         return;
  14
  15     FOREACH_ENTITY_ENT(owner, start,
  16     {
  17         setthink(it, SUB_Remove);
  18         it.nextthink = time;
  19     });
  20 }
  21
  22 const float PATHLIB_NODEEXPIRE = 20; // 0.05
  23
  24 void dumpnode(entity n)
  25 {
  26     if(n.is_path_node)
  27         IL_REMOVE(g_pathlib_nodes, n);
  28     n.is_path_node = false;
  29     setthink(n, SUB_Remove);
  30     n.nextthink    = time;
  31 }
  32
  33 #if DEBUGPATHING
  34 #include "debug.qh"
  35 #endif
  36
  37
  38 entity pathlib_mknode(vector where,entity parent)
  39 {
  40     entity node = pathlib_nodeatpoint(where);
  41     if(node)
  42     {
  43         #ifdef TURRET_DEBUG
  44         mark_error(where, 60);
  45         #endif
  46         return node;
  47     }
  48
  49     node = spawn();
  50
  51     setthink(node, SUB_Remove);
  52     node.nextthink    = time + PATHLIB_NODEEXPIRE;
  53     IL_PUSH(g_pathlib_nodes, node);
  54     node.is_path_node = true;
  55     node.owner        = openlist;
  56     node.path_prev    = parent;
  57
  58     setsize(node, '0 0 0', '0 0 0');
  59
  60     setorigin(node, where);
  61 #if DEBUGPATHING
  62     pathlib_showsquare(where, 1 ,15);
  63 #endif
  64     ++pathlib_made_cnt;
  65     ++pathlib_open_cnt;
  66
  67     return node;
  68 }
  69
  70 bool pathlib_makenode_adaptive(entity parent,vector start, vector to, vector goal,float cost)
  71 {
  72     bool dodge = false;
  73     float f, h, g;
  74
  75     ++pathlib_searched_cnt;
  76
  77     if(inwater(parent.origin))
  78     {
  79         LOG_DEBUG("FromWater");
  80         pathlib_expandnode = pathlib_expandnode_box;
  81         pathlib_movenode   = pathlib_swimnode;
  82     }
  83     else
  84     {
  85         if(inwater(to))
  86         {
  87             LOG_DEBUG("ToWater");
  88             pathlib_expandnode = pathlib_expandnode_box;
  89             pathlib_movenode   = pathlib_walknode;
  90         }
  91         else
  92         {
  93             LOG_DEBUG("LandToLoand");
  94             //if(edge_check(parent.origin))
  95             //    return 0;
  96
  97             pathlib_expandnode = pathlib_expandnode_star;
  98             pathlib_movenode   = pathlib_walknode;
  99             dodge = true;
 100         }
 101     }
 102
 103     entity node = pathlib_nodeatpoint(to);
 104     if(node)
 105     {
 106         LOG_DEBUG("NodeAtPoint");
 107         ++pathlib_merge_cnt;
 108
 109         if(node.owner == openlist)
 110         {
 111             h = pathlib_heuristic(node.origin,goal);
 112             float g = pathlib_cost(parent,node.origin,cost);
 113             f = g + h;
 114
 115             if(node.pathlib_node_g > g)
 116             {
 117                 node.pathlib_node_h = h;
 118                 node.pathlib_node_g = g;
 119                 node.pathlib_node_f = f;
 120
 121                 node.path_prev = parent;
 122             }
 123
 124             if (!best_open_node)
 125                 best_open_node = node;
 126             else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
 127                 best_open_node = node;
 128         }
 129
 130         return true;
 131     }
 132
 133     vector where = pathlib_movenode(parent, parent.origin, to, 0);
 134
 135     if (!pathlib_movenode_goodnode)
 136     {
 137         //pathlib_showsquare(where, 0 ,30);
 138         //pathlib_showsquare(parent.origin, 1 ,30);
 139         LOG_DEBUG("pathlib_movenode_goodnode = 0");
 140         return false;
 141     }
 142
 143     //pathlib_showsquare(where, 1 ,30);
 144
 145     if(pathlib_nodeatpoint(where))
 146     {
 147         LOG_DEBUG("NAP WHERE :",vtos(where));
 148         LOG_DEBUG("not NAP TO:",vtos(to));
 149         LOG_DEBUG("NAP-NNAP:",ftos(vlen(to-where)));
 150         return false;
 151     }
 152
 153
 154     if(dodge)
 155         if (!tile_check(parent, where))
 156         {
 157             LOG_DEBUG("tile_check fail");
 158 #if DEBUGPATHING
 159             pathlib_showsquare(where, 0 ,30);
 160 #endif
 161             return false;
 162         }
 163
 164
 165     h = pathlib_heuristic(where,goal);
 166     g = pathlib_cost(parent,where,cost);
 167     f = g + h;
 168
 169     /*
 170     node = findradius(where,pathlib_gridsize * 0.5);
 171     while(node)
 172     {
 173         if(node.is_path_node == true)
 174         {
 175             ++pathlib_merge_cnt;
 176             if(node.owner == openlist)
 177             {
 178                 if(node.pathlib_node_g > g)
 179                 {
 180                     //pathlib_movenode(node, where,node.origin,0);
 181                     //if(pathlib_movenode_goodnode)
 182                     //{
 183                         //mark_error(node.origin + '0 0 128',30);
 184                         node.pathlib_node_h = h;
 185                         node.pathlib_node_g = g;
 186                         node.pathlib_node_f = f;
 187                         node.path_prev = parent;
 188                     //}
 189                 }
 190
 191                 if (!best_open_node)
 192                     best_open_node = node;
 193                 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
 194                     best_open_node = node;
 195             }
 196
 197             return 1;
 198         }
 199         node = node.chain;
 200     }
 201     */
 202
 203     node = pathlib_mknode(where, parent);
 204     node.pathlib_node_h = h;
 205     node.pathlib_node_g = g;
 206     node.pathlib_node_f = f;
 207
 208     if (!best_open_node)
 209         best_open_node = node;
 210     else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
 211         best_open_node = node;
 212
 213     return true;
 214 }
 215
 216 entity pathlib_getbestopen()
 217 {
 218     if(best_open_node)
 219     {
 220         ++pathlib_bestcash_hits;
 221         pathlib_bestcash_saved += pathlib_open_cnt;
 222
 223         return best_open_node;
 224     }
 225
 226     entity bestnode = NULL;
 227     FOREACH_ENTITY_ENT(owner, openlist,
 228     {
 229         ++pathlib_bestopen_searched;
 230
 231         if(!bestnode || it.pathlib_node_f < bestnode.pathlib_node_f)
 232             bestnode = it;
 233     });
 234
 235     return bestnode;
 236 }
 237
 238 void pathlib_close_node(entity node,vector goal)
 239 {
 240     if(node.owner == closedlist)
 241     {
 242         LOG_TRACE("Pathlib: Tried to close a closed node!");
 243         return;
 244     }
 245
 246     if(node == best_open_node)
 247         best_open_node = NULL;
 248
 249     ++pathlib_closed_cnt;
 250     --pathlib_open_cnt;
 251
 252     node.owner = closedlist;
 253
 254     if(vdist(node.origin - goal, <=, pathlib_gridsize))
 255     {
 256         vector goalmove;
 257
 258         goalmove = pathlib_walknode(node, node.origin, goal, 1);
 259         if(pathlib_movenode_goodnode)
 260         {
 261             goal_node         = node;
 262             pathlib_foundgoal = true;
 263         }
 264     }
 265 }
 266
 267 void pathlib_cleanup()
 268 {
 269     best_open_node = NULL;
 270
 271     //return;
 272
 273     IL_EACH(g_pathlib_nodes, it.is_path_node,
 274     {
 275         dumpnode(it);
 276     });
 277
 278     IL_CLEAR(g_pathlib_nodes);
 279
 280     if(openlist)
 281         delete(openlist);
 282
 283     if(closedlist)
 284         delete(closedlist);
 285
 286     openlist = NULL;
 287     closedlist = NULL;
 288 }
 289
 290 float Cosine_Interpolate(float a, float b, float c)
 291 {
 292         float ft = c * 3.1415927;
 293         float f = (1 - cos(ft)) * 0.5;
 294
 295         return a*(1-f) + b*f;
 296 }
 297
 298 bool buildpath_nodefilter_directional(vector n,vector c,vector p)
 299 {
 300     vector d2 = normalize(p - c);
 301     vector d1 = normalize(c - n);
 302
 303     if(vdist(d1 - d2, <, 0.25))
 304     {
 305         //mark_error(c,30);
 306         return true;
 307     }
 308     //mark_info(c,30);
 309     return false;
 310 }
 311
 312 bool buildpath_nodefilter_moveskip(entity this, vector n,vector c,vector p)
 313 {
 314     pathlib_walknode(this, p, n, 1);
 315     if(pathlib_movenode_goodnode)
 316         return true;
 317
 318     return false;
 319 }
 320
 321 bool buildpath_nodefilter_none(vector n,vector c,vector p)
 322 {
 323     return false;
 324 }
 325
 326 entity path_build(entity next, vector where, entity prev, entity start)
 327 {
 328     if(prev && next)
 329         if(buildpath_nodefilter)
 330             if(buildpath_nodefilter(next.origin,where,prev.origin))
 331                 return next;
 332
 333
 334     entity path    = spawn();
 335     path.owner     = start;
 336     path.path_next = next;
 337
 338     setorigin(path, where);
 339
 340     if(!next)
 341         path.classname = "path_end";
 342     else
 343     {
 344         if(!prev)
 345             path.classname = "path_start";
 346         else
 347             path.classname = "path_node";
 348     }
 349
 350     return path;
 351 }
 352
 353 entity pathlib_astar(entity this, vector from, vector to)
 354 {
 355         entity open;
 356     float ptime = gettime(GETTIME_REALTIME);
 357     pathlib_starttime = ptime;
 358
 359     pathlib_cleanup();
 360
 361     // Select water<->land capable node make/link
 362     pathlib_makenode     = pathlib_makenode_adaptive;
 363
 364     // Select XYZ cost estimate
 365     //pathlib_heuristic    = pathlib_h_diagonal3;
 366     pathlib_heuristic    = pathlib_h_diagonal;
 367
 368     // Select distance + waterfactor cost
 369     pathlib_cost         = pathlib_g_euclidean_water;
 370
 371     // Select star expander
 372     pathlib_expandnode   = pathlib_expandnode_star;
 373
 374     // Select walk simulation movement test
 375     pathlib_movenode     = pathlib_walknode;
 376
 377     // Filter final nodes by direction
 378     buildpath_nodefilter = buildpath_nodefilter_directional;
 379
 380     // Filter tiles with cross pattern
 381     tile_check = tile_check_cross;
 382
 383     // If the start is in water we need diffrent settings
 384     if(inwater(from))
 385     {
 386         // Select volumetric node expaner
 387         pathlib_expandnode = pathlib_expandnode_box;
 388
 389         // Water movement test
 390         pathlib_movenode   = pathlib_swimnode;
 391     }
 392
 393     if (!openlist)
 394         openlist       = spawn();
 395
 396     if (!closedlist)
 397         closedlist     = spawn();
 398
 399     pathlib_closed_cnt       = 0;
 400     pathlib_open_cnt         = 0;
 401     pathlib_made_cnt         = 0;
 402     pathlib_merge_cnt        = 0;
 403     pathlib_searched_cnt     = 0;
 404     pathlib_bestopen_searched = 0;
 405     pathlib_bestcash_hits    = 0;
 406     pathlib_bestcash_saved   = 0;
 407
 408     pathlib_gridsize       = 128;
 409     pathlib_movecost       = pathlib_gridsize;
 410     pathlib_movecost_diag  = vlen(('1 1 0' * pathlib_gridsize));
 411     pathlib_movecost_waterfactor = 25000000;
 412     pathlib_foundgoal      = 0;
 413
 414     movenode_boxmax   = this.maxs * 1.1;
 415     movenode_boxmin   = this.mins * 1.1;
 416
 417     movenode_stepsize = pathlib_gridsize * 0.25;
 418
 419     tile_check_size = pathlib_gridsize * 0.5;
 420     tile_check_up   = '0 0 2' * pathlib_gridsize;
 421     tile_check_up   = '0 0 128';
 422     tile_check_down = '0 0 3' * pathlib_gridsize;
 423     tile_check_down = '0 0 256';
 424
 425     //movenode_stepup   = '0 0 128';
 426     movenode_stepup   = '0 0 36';
 427     movenode_maxdrop  = '0 0 512';
 428     //movenode_maxdrop  = '0 0 512';
 429     movenode_boxup    = '0 0 72';
 430
 431     from.x = fsnap(from.x, pathlib_gridsize);
 432     from.y = fsnap(from.y, pathlib_gridsize);
 433     //from.z += 32;
 434
 435     to.x = fsnap(to.x, pathlib_gridsize);
 436     to.y = fsnap(to.y, pathlib_gridsize);
 437     //to.z += 32;
 438
 439     LOG_DEBUG("AStar init");
 440     entity path = pathlib_mknode(from, NULL);
 441     pathlib_close_node(path, to);
 442     if(pathlib_foundgoal)
 443     {
 444         LOG_DEBUG("AStar: Goal found on first node!");
 445
 446         open           = new(path_end);
 447         open.owner     = open;
 448         setorigin(open, path.origin);
 449
 450         pathlib_cleanup();
 451
 452         return open;
 453     }
 454
 455     if(pathlib_expandnode(path, from, to) <= 0)
 456     {
 457         LOG_TRACE("AStar path fail.");
 458         pathlib_cleanup();
 459
 460         return NULL;
 461     }
 462
 463     best_open_node = pathlib_getbestopen();
 464     entity n = best_open_node;
 465     pathlib_close_node(best_open_node, to);
 466     if(inwater(n.origin))
 467         pathlib_expandnode_box(n, from, to);
 468     else
 469         pathlib_expandnode_star(n, from, to);
 470
 471     while(pathlib_open_cnt)
 472     {
 473         if((gettime(GETTIME_REALTIME) - pathlib_starttime) > pathlib_maxtime)
 474         {
 475             LOG_TRACE("Path took too long to compute!");
 476             LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt));
 477             LOG_TRACE("Nodes -    open: ", ftos(pathlib_open_cnt));
 478             LOG_TRACE("Nodes -  merged: ", ftos(pathlib_merge_cnt));
 479             LOG_TRACE("Nodes -  closed: ", ftos(pathlib_closed_cnt));
 480
 481             pathlib_cleanup();
 482             return NULL;
 483         }
 484
 485         best_open_node = pathlib_getbestopen();
 486         n = best_open_node;
 487         pathlib_close_node(best_open_node,to);
 488
 489         if(inwater(n.origin))
 490             pathlib_expandnode_box(n,from,to);
 491         else
 492             pathlib_expandnode(n,from,to);
 493
 494         if(pathlib_foundgoal)
 495         {
 496             LOG_DEBUG("Target found. Rebuilding and filtering path...");
 497             float ftime = gettime(GETTIME_REALTIME);
 498             ptime = ftime - ptime;
 499
 500             entity start = path_build(NULL,path.origin,NULL,NULL);
 501             entity end   = path_build(NULL,goal_node.origin,NULL,start);
 502             entity ln    = end;
 503
 504             open = goal_node;
 505             for(open = goal_node; open.path_prev != path; open = open.path_prev)
 506             {
 507                 n    = path_build(ln,open.origin,open.path_prev,start);
 508                 ln.path_prev = n;
 509                 ln = n;
 510             }
 511             start.path_next = n;
 512             n.path_prev = start;
 513             ftime = gettime(GETTIME_REALTIME) - ftime;
 514
 515             float ctime = gettime(GETTIME_REALTIME);
 516             pathlib_cleanup();
 517             ctime = gettime(GETTIME_REALTIME) - ctime;
 518
 519
 520 #if DEBUGPATHING
 521             pathlib_showpath2(start);
 522
 523             LOG_TRACE("Time used -      pathfinding: ", ftos(ptime));
 524             LOG_TRACE("Time used - rebuild & filter: ", ftos(ftime));
 525             LOG_TRACE("Time used -          cleanup: ", ftos(ctime));
 526             LOG_TRACE("Time used -            total: ", ftos(ptime + ftime + ctime));
 527             LOG_TRACE("Time used -         # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)));
 528             LOG_TRACE("Nodes -         created: ", ftos(pathlib_made_cnt));
 529             LOG_TRACE("Nodes -            open: ", ftos(pathlib_open_cnt));
 530             LOG_TRACE("Nodes -          merged: ", ftos(pathlib_merge_cnt));
 531             LOG_TRACE("Nodes -          closed: ", ftos(pathlib_closed_cnt));
 532             LOG_TRACE("Nodes -        searched: ", ftos(pathlib_searched_cnt));
 533             LOG_TRACE("Nodes bestopen searched: ", ftos(pathlib_bestopen_searched));
 534             LOG_TRACE("Nodes bestcash -   hits: ", ftos(pathlib_bestcash_hits));
 535             LOG_TRACE("Nodes bestcash -   save: ", ftos(pathlib_bestcash_saved));
 536             LOG_TRACE("AStar done.");
 537 #endif
 538             return start;
 539         }
 540     }
 541
 542     LOG_TRACE("A* Faild to find a path! Try a smaller gridsize.");
 543
 544     pathlib_cleanup();
 545
 546     return NULL;
 547 }