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