qcsrc/server/pathlib/main.qc

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