qcsrc/server/pathlib/main.qc

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