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     node.medium = pointcontents(where);
  59 #if DEBUGPATHING
  60     pathlib_showsquare(where, 1 ,15);
  61 #endif
  62     ++pathlib_made_cnt;
  63     ++pathlib_open_cnt;
  64
  65     return node;
  66 }
  67
  68 float pathlib_makenode_adaptive(entity parent,vector start, vector to, vector goal,float cost)
  69 {
  70     entity node;
  71     float h,g,f,doedge = 0;
  72     vector where;
  73
  74     ++pathlib_searched_cnt;
  75
  76     if(inwater(parent.origin))
  77     {
  78         LOG_TRACE("FromWater");
  79         pathlib_expandnode = pathlib_expandnode_box;
  80         pathlib_movenode   = pathlib_swimnode;
  81     }
  82     else
  83     {
  84         if(inwater(to))
  85         {
  86             LOG_TRACE("ToWater");
  87             pathlib_expandnode = pathlib_expandnode_box;
  88             pathlib_movenode   = pathlib_walknode;
  89         }
  90         else
  91         {
  92             LOG_TRACE("LandToLoand");
  93             //if(edge_check(parent.origin))
  94             //    return 0;
  95
  96             pathlib_expandnode = pathlib_expandnode_star;
  97             pathlib_movenode   = pathlib_walknode;
  98             doedge = 1;
  99         }
 100     }
 101
 102     node = pathlib_nodeatpoint(to);
 103     if(node)
 104     {
 105         LOG_TRACE("NodeAtPoint");
 106         ++pathlib_merge_cnt;
 107
 108         if(node.owner == openlist)
 109         {
 110             h = pathlib_heuristic(node.origin,goal);
 111             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 1;
 130     }
 131
 132     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_TRACE("pathlib_movenode_goodnode = 0");
 139         return 0;
 140     }
 141
 142     //pathlib_showsquare(where, 1 ,30);
 143
 144     if(pathlib_nodeatpoint(where))
 145     {
 146         LOG_TRACE("NAP WHERE :",vtos(where));
 147         LOG_TRACE("not NAP TO:",vtos(to));
 148         LOG_TRACE("NAP-NNAP:",ftos(vlen(to-where)));
 149         return 0;
 150     }
 151
 152
 153     if(doedge)
 154         if (!tile_check(parent, where))
 155         {
 156             LOG_TRACE("tile_check fail");
 157 #if DEBUGPATHING
 158             pathlib_showsquare(where, 0 ,30);
 159 #endif
 160             return 0;
 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 1;
 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
 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     entity node;
 274
 275     node = findfloat(NULL,is_path_node, true);
 276     while(node)
 277     {
 278         /*
 279         node.owner = openlist;
 280         node.pathlib_node_g = 0;
 281         node.pathlib_node_h = 0;
 282         node.pathlib_node_f = 0;
 283         node.path_prev = NULL;
 284         */
 285
 286         dumpnode(node);
 287         node = findfloat(node,is_path_node, true);
 288     }
 289
 290     if(openlist)
 291         delete(openlist);
 292
 293     if(closedlist)
 294         delete(closedlist);
 295
 296     openlist       = NULL;
 297     closedlist     = NULL;
 298
 299 }
 300
 301 float Cosine_Interpolate(float a, float b, float x)
 302 {
 303     float ft,f;
 304
 305         ft = x * 3.1415927;
 306         f = (1 - cos(ft)) * 0.5;
 307
 308         return  a*(1-f) + b*f;
 309 }
 310
 311 float buildpath_nodefilter_directional(vector n,vector c,vector p)
 312 {
 313     vector d1,d2;
 314
 315     d2 = normalize(p - c);
 316     d1 = normalize(c - n);
 317
 318     if(vdist(d1 - d2, <, 0.25))
 319     {
 320         //mark_error(c,30);
 321         return 1;
 322     }
 323     //mark_info(c,30);
 324     return 0;
 325 }
 326
 327 float buildpath_nodefilter_moveskip(entity this, vector n,vector c,vector p)
 328 {
 329     pathlib_walknode(this, p, n, 1);
 330     if(pathlib_movenode_goodnode)
 331         return 1;
 332
 333     return 0;
 334 }
 335
 336 float buildpath_nodefilter_none(vector n,vector c,vector p)
 337 {
 338     return 0;
 339 }
 340
 341 entity path_build(entity next, vector where, entity prev, entity start)
 342 {
 343     entity path;
 344
 345     if(prev && next)
 346         if(buildpath_nodefilter)
 347             if(buildpath_nodefilter(next.origin,where,prev.origin))
 348                 return next;
 349
 350
 351     path           = spawn();
 352     path.owner     = start;
 353     path.path_next = next;
 354
 355     setorigin(path, where);
 356
 357     if(!next)
 358         path.classname = "path_end";
 359     else
 360     {
 361         if(!prev)
 362             path.classname = "path_start";
 363         else
 364             path.classname = "path_node";
 365     }
 366
 367     return path;
 368 }
 369
 370 entity pathlib_astar(entity this, vector from,vector to)
 371 {
 372     entity path, start, end, open, n, ln;
 373     float ptime, ftime, ctime;
 374
 375     ptime = gettime(GETTIME_REALTIME);
 376     pathlib_starttime = ptime;
 377
 378     pathlib_cleanup();
 379
 380     // Select water<->land capable node make/link
 381     pathlib_makenode     = pathlib_makenode_adaptive;
 382
 383     // Select XYZ cost estimate
 384     //pathlib_heuristic    = pathlib_h_diagonal3;
 385     pathlib_heuristic    = pathlib_h_diagonal;
 386
 387     // Select distance + waterfactor cost
 388     pathlib_cost         = pathlib_g_euclidean_water;
 389
 390     // Select star expander
 391     pathlib_expandnode   = pathlib_expandnode_star;
 392
 393     // Select walk simulation movement test
 394     pathlib_movenode     = pathlib_walknode;
 395
 396     // Filter final nodes by direction
 397     buildpath_nodefilter = buildpath_nodefilter_directional;
 398
 399     // Filter tiles with cross pattern
 400     tile_check = tile_check_cross;
 401
 402     // If the start is in water we need diffrent settings
 403     if(inwater(from))
 404     {
 405         // Select volumetric node expaner
 406         pathlib_expandnode = pathlib_expandnode_box;
 407
 408         // Water movement test
 409         pathlib_movenode   = pathlib_swimnode;
 410     }
 411
 412     if (!openlist)
 413         openlist       = spawn();
 414
 415     if (!closedlist)
 416         closedlist     = spawn();
 417
 418     pathlib_closed_cnt       = 0;
 419     pathlib_open_cnt         = 0;
 420     pathlib_made_cnt         = 0;
 421     pathlib_merge_cnt        = 0;
 422     pathlib_searched_cnt     = 0;
 423     pathlib_bestopen_searched = 0;
 424     pathlib_bestcash_hits    = 0;
 425     pathlib_bestcash_saved   = 0;
 426
 427     pathlib_gridsize       = 128;
 428     pathlib_movecost       = pathlib_gridsize;
 429     pathlib_movecost_diag  = vlen(('1 1 0' * pathlib_gridsize));
 430     pathlib_movecost_waterfactor = 25000000;
 431     pathlib_foundgoal      = 0;
 432
 433     movenode_boxmax   = this.maxs * 1.1;
 434     movenode_boxmin   = this.mins * 1.1;
 435
 436     movenode_stepsize = pathlib_gridsize * 0.25;
 437
 438     tile_check_size = pathlib_gridsize * 0.5;
 439     tile_check_up   = '0 0 2' * pathlib_gridsize;
 440     tile_check_up   = '0 0 128';
 441     tile_check_down = '0 0 3' * pathlib_gridsize;
 442     tile_check_down = '0 0 256';
 443
 444     //movenode_stepup   = '0 0 128';
 445     movenode_stepup   = '0 0 36';
 446     movenode_maxdrop  = '0 0 512';
 447     //movenode_maxdrop  = '0 0 512';
 448     movenode_boxup    = '0 0 72';
 449
 450     from.x = fsnap(from.x, pathlib_gridsize);
 451     from.y = fsnap(from.y, pathlib_gridsize);
 452     //from_z += 32;
 453
 454     to.x = fsnap(to.x, pathlib_gridsize);
 455     to.y = fsnap(to.y, pathlib_gridsize);
 456     //to_z += 32;
 457
 458     LOG_TRACE("AStar init");
 459     path = pathlib_mknode(from, NULL);
 460     pathlib_close_node(path, to);
 461     if(pathlib_foundgoal)
 462     {
 463         LOG_TRACE("AStar: Goal found on first node!");
 464
 465         open           = new(path_end);
 466         open.owner     = open;
 467         setorigin(open, path.origin);
 468
 469         pathlib_cleanup();
 470
 471         return open;
 472     }
 473
 474     if(pathlib_expandnode(path, from, to) <= 0)
 475     {
 476         LOG_TRACE("AStar path fail.");
 477         pathlib_cleanup();
 478
 479         return NULL;
 480     }
 481
 482     best_open_node = pathlib_getbestopen();
 483     n = best_open_node;
 484     pathlib_close_node(best_open_node, to);
 485     if(inwater(n.origin))
 486         pathlib_expandnode_box(n, from, to);
 487     else
 488         pathlib_expandnode_star(n, from, to);
 489
 490     while(pathlib_open_cnt)
 491     {
 492         if((gettime(GETTIME_REALTIME) - pathlib_starttime) > pathlib_maxtime)
 493         {
 494             LOG_TRACE("Path took to long to compute!");
 495             LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt));
 496             LOG_TRACE("Nodes -    open: ", ftos(pathlib_open_cnt));
 497             LOG_TRACE("Nodes -  merged: ", ftos(pathlib_merge_cnt));
 498             LOG_TRACE("Nodes -  closed: ", ftos(pathlib_closed_cnt));
 499
 500             pathlib_cleanup();
 501             return NULL;
 502         }
 503
 504         best_open_node = pathlib_getbestopen();
 505         n = best_open_node;
 506         pathlib_close_node(best_open_node,to);
 507
 508         if(inwater(n.origin))
 509             pathlib_expandnode_box(n,from,to);
 510         else
 511             pathlib_expandnode(n,from,to);
 512
 513         if(pathlib_foundgoal)
 514         {
 515             LOG_TRACE("Target found. Rebuilding and filtering path...");
 516             ftime = gettime(GETTIME_REALTIME);
 517             ptime = ftime - ptime;
 518
 519             start = path_build(NULL,path.origin,NULL,NULL);
 520             end   = path_build(NULL,goal_node.origin,NULL,start);
 521             ln    = end;
 522
 523             open = goal_node;
 524             for(open = goal_node; open.path_prev != path; open = open.path_prev)
 525             {
 526                 n    = path_build(ln,open.origin,open.path_prev,start);
 527                 ln.path_prev = n;
 528                 ln = n;
 529             }
 530             start.path_next = n;
 531             n.path_prev = start;
 532             ftime = gettime(GETTIME_REALTIME) - ftime;
 533
 534             ctime = gettime(GETTIME_REALTIME);
 535             pathlib_cleanup();
 536             ctime = gettime(GETTIME_REALTIME) - ctime;
 537
 538
 539 #if DEBUGPATHING
 540             pathlib_showpath2(start);
 541
 542             LOG_TRACE("Time used -      pathfinding: ", ftos(ptime));
 543             LOG_TRACE("Time used - rebuild & filter: ", ftos(ftime));
 544             LOG_TRACE("Time used -          cleanup: ", ftos(ctime));
 545             LOG_TRACE("Time used -            total: ", ftos(ptime + ftime + ctime));
 546             LOG_TRACE("Time used -         # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)));
 547             LOG_TRACE("Nodes -         created: ", ftos(pathlib_made_cnt));
 548             LOG_TRACE("Nodes -            open: ", ftos(pathlib_open_cnt));
 549             LOG_TRACE("Nodes -          merged: ", ftos(pathlib_merge_cnt));
 550             LOG_TRACE("Nodes -          closed: ", ftos(pathlib_closed_cnt));
 551             LOG_TRACE("Nodes -        searched: ", ftos(pathlib_searched_cnt));
 552             LOG_TRACE("Nodes bestopen searched: ", ftos(pathlib_bestopen_searched));
 553             LOG_TRACE("Nodes bestcash -   hits: ", ftos(pathlib_bestcash_hits));
 554             LOG_TRACE("Nodes bestcash -   save: ", ftos(pathlib_bestcash_saved));
 555             LOG_TRACE("AStar done.");
 556 #endif
 557             return start;
 558         }
 559     }
 560
 561     LOG_TRACE("A* Faild to find a path! Try a smaller gridsize.");
 562
 563     pathlib_cleanup();
 564
 565     return NULL;
 566 }