qcsrc/server/bot/lib/pathlib/main.qc

   1 #include "pathlib.qh"
   2 #include "utility.qh"
   3
   4 void pathlib_deletepath(entity start)
   5 {
   6     entity e;
   7
   8     e = findchainentity(owner, start);
   9     while(e)
  10     {
  11         e.think = SUB_Remove;
  12         e.nextthink = time;
  13         e = e.chain;
  14     }
  15 }
  16
  17 //#define PATHLIB_NODEEXPIRE 0.05
  18 const float PATHLIB_NODEEXPIRE = 20;
  19
  20 void dumpnode(entity n)
  21 {
  22     n.is_path_node = false;
  23     n.think        = SUB_Remove;
  24     n.nextthink    = time;
  25 }
  26
  27 entity pathlib_mknode(vector where,entity parent)
  28 {
  29     entity node;
  30
  31     node = pathlib_nodeatpoint(where);
  32     if(node)
  33     {
  34         #ifdef TURRET_DEBUG
  35         mark_error(where, 60);
  36         #endif
  37         return node;
  38     }
  39
  40     node = spawn();
  41
  42     node.think        = SUB_Remove;
  43     node.nextthink    = time + PATHLIB_NODEEXPIRE;
  44     node.is_path_node = true;
  45     node.owner        = openlist;
  46     node.path_prev    = parent;
  47
  48
  49     setsize(node, '0 0 0', '0 0 0');
  50
  51     setorigin(node, where);
  52     node.medium = pointcontents(where);
  53     pathlib_showsquare(where, 1 ,15);
  54
  55     ++pathlib_made_cnt;
  56     ++pathlib_open_cnt;
  57
  58     return node;
  59 }
  60
  61 float pathlib_makenode_adaptive(entity parent,vector start, vector to, vector goal,float cost)
  62 {
  63     entity node;
  64     float h,g,f,doedge = 0;
  65     vector where;
  66
  67     ++pathlib_searched_cnt;
  68
  69     if(inwater(parent.origin))
  70     {
  71         LOG_TRACE("FromWater\n");
  72         pathlib_expandnode = pathlib_expandnode_box;
  73         pathlib_movenode   = pathlib_swimnode;
  74     }
  75     else
  76     {
  77         if(inwater(to))
  78         {
  79             LOG_TRACE("ToWater\n");
  80             pathlib_expandnode = pathlib_expandnode_box;
  81             pathlib_movenode   = pathlib_walknode;
  82         }
  83         else
  84         {
  85             LOG_TRACE("LandToLoand\n");
  86             //if(edge_check(parent.origin))
  87             //    return 0;
  88
  89             pathlib_expandnode = pathlib_expandnode_star;
  90             pathlib_movenode   = pathlib_walknode;
  91             doedge = 1;
  92         }
  93     }
  94
  95     node = pathlib_nodeatpoint(to);
  96     if(node)
  97     {
  98         LOG_TRACE("NodeAtPoint\n");
  99         ++pathlib_merge_cnt;
 100
 101         if(node.owner == openlist)
 102         {
 103             h = pathlib_heuristic(node.origin,goal);
 104             g = pathlib_cost(parent,node.origin,cost);
 105             f = g + h;
 106
 107             if(node.pathlib_node_g > g)
 108             {
 109                 node.pathlib_node_h = h;
 110                 node.pathlib_node_g = g;
 111                 node.pathlib_node_f = f;
 112
 113                 node.path_prev = parent;
 114             }
 115
 116             if (!best_open_node)
 117                 best_open_node = node;
 118             else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
 119                 best_open_node = node;
 120         }
 121
 122         return 1;
 123     }
 124
 125     where = pathlib_movenode(parent.origin, to, 0);
 126
 127     if (!pathlib_movenode_goodnode)
 128     {
 129         //pathlib_showsquare(where, 0 ,30);
 130         //pathlib_showsquare(parent.origin, 1 ,30);
 131         LOG_TRACE("pathlib_movenode_goodnode = 0\n");
 132         return 0;
 133     }
 134
 135     //pathlib_showsquare(where, 1 ,30);
 136
 137     if(pathlib_nodeatpoint(where))
 138     {
 139         LOG_TRACE("NAP WHERE :",vtos(where),"\n");
 140         LOG_TRACE("not NAP TO:",vtos(to),"\n");
 141         LOG_TRACE("NAP-NNAP:",ftos(vlen(to-where)),"\n\n");
 142         return 0;
 143     }
 144
 145
 146     if(doedge)
 147         if (!tile_check(where))
 148         {
 149             LOG_TRACE("tile_check fail\n");
 150             pathlib_showsquare(where, 0 ,30);
 151             return 0;
 152         }
 153
 154
 155     h = pathlib_heuristic(where,goal);
 156     g = pathlib_cost(parent,where,cost);
 157     f = g + h;
 158
 159     /*
 160     node = findradius(where,pathlib_gridsize * 0.5);
 161     while(node)
 162     {
 163         if(node.is_path_node == true)
 164         {
 165             ++pathlib_merge_cnt;
 166             if(node.owner == openlist)
 167             {
 168                 if(node.pathlib_node_g > g)
 169                 {
 170                     //pathlib_movenode(where,node.origin,0);
 171                     //if(pathlib_movenode_goodnode)
 172                     //{
 173                         //mark_error(node.origin + '0 0 128',30);
 174                         node.pathlib_node_h = h;
 175                         node.pathlib_node_g = g;
 176                         node.pathlib_node_f = f;
 177                         node.path_prev = parent;
 178                     //}
 179                 }
 180
 181                 if (!best_open_node)
 182                     best_open_node = node;
 183                 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
 184                     best_open_node = node;
 185             }
 186
 187             return 1;
 188         }
 189         node = node.chain;
 190     }
 191     */
 192
 193     node = pathlib_mknode(where, parent);
 194     node.pathlib_node_h = h;
 195     node.pathlib_node_g = g;
 196     node.pathlib_node_f = f;
 197
 198     if (!best_open_node)
 199         best_open_node = node;
 200     else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
 201         best_open_node = node;
 202
 203     return 1;
 204 }
 205
 206 entity pathlib_getbestopen()
 207 {
 208     entity node;
 209     entity bestnode;
 210
 211     if(best_open_node)
 212     {
 213         ++pathlib_bestcash_hits;
 214         pathlib_bestcash_saved += pathlib_open_cnt;
 215
 216         return best_open_node;
 217     }
 218
 219     node = findchainentity(owner,openlist);
 220     if(!node)
 221         return world;
 222
 223     bestnode = node;
 224     while(node)
 225     {
 226         ++pathlib_bestopen_seached;
 227         if(node.pathlib_node_f < bestnode.pathlib_node_f)
 228             bestnode = node;
 229
 230         node = node.chain;
 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!\n");
 242         return;
 243     }
 244
 245     if(node == best_open_node)
 246         best_open_node = world;
 247
 248     ++pathlib_closed_cnt;
 249     --pathlib_open_cnt;
 250
 251     node.owner = closedlist;
 252
 253     if(vlen(node.origin - goal) <= pathlib_gridsize)
 254     {
 255         vector goalmove;
 256
 257         goalmove = pathlib_walknode(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 = world;
 269
 270     //return;
 271
 272     entity node;
 273
 274     node = findfloat(world,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 = world;
 283         */
 284
 285         dumpnode(node);
 286         node = findfloat(node,is_path_node, true);
 287     }
 288
 289     if(openlist)
 290         remove(openlist);
 291
 292     if(closedlist)
 293         remove(closedlist);
 294
 295     openlist       = world;
 296     closedlist     = world;
 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(vlen(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(vector n,vector c,vector p)
 327 {
 328     pathlib_walknode(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(vector from,vector to)
 370 {SELFPARAM();
 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_seached = 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   = self.maxs * 1.1;
 433     movenode_boxmin   = self.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\n");
 458     path = pathlib_mknode(from, world);
 459     pathlib_close_node(path, to);
 460     if(pathlib_foundgoal)
 461     {
 462         LOG_TRACE("AStar: Goal found on first node!\n");
 463
 464         open           = spawn();
 465         open.owner     = open;
 466         open.classname = "path_end";
 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.\n");
 477         pathlib_cleanup();
 478
 479         return world;
 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!\n");
 495             LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt),"\n");
 496             LOG_TRACE("Nodes -    open: ", ftos(pathlib_open_cnt),"\n");
 497             LOG_TRACE("Nodes -  merged: ", ftos(pathlib_merge_cnt),"\n");
 498             LOG_TRACE("Nodes -  closed: ", ftos(pathlib_closed_cnt),"\n");
 499
 500             pathlib_cleanup();
 501             return world;
 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...\n");
 516             ftime = gettime(GETTIME_REALTIME);
 517             ptime = ftime - ptime;
 518
 519             start = path_build(world,path.origin,world,world);
 520             end   = path_build(world,goal_node.origin,world,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 #ifdef DEBUGPATHING
 540             pathlib_showpath2(start);
 541
 542             LOG_TRACE("Time used -      pathfinding: ", ftos(ptime),"\n");
 543             LOG_TRACE("Time used - rebuild & filter: ", ftos(ftime),"\n");
 544             LOG_TRACE("Time used -          cleanup: ", ftos(ctime),"\n");
 545             LOG_TRACE("Time used -            total: ", ftos(ptime + ftime + ctime),"\n");
 546             LOG_TRACE("Time used -         # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)),"\n\n");
 547             LOG_TRACE("Nodes -         created: ", ftos(pathlib_made_cnt),"\n");
 548             LOG_TRACE("Nodes -            open: ", ftos(pathlib_open_cnt),"\n");
 549             LOG_TRACE("Nodes -          merged: ", ftos(pathlib_merge_cnt),"\n");
 550             LOG_TRACE("Nodes -          closed: ", ftos(pathlib_closed_cnt),"\n");
 551             LOG_TRACE("Nodes -        searched: ", ftos(pathlib_searched_cnt),"\n");
 552             LOG_TRACE("Nodes bestopen searched: ", ftos(pathlib_bestopen_seached),"\n");
 553             LOG_TRACE("Nodes bestcash -   hits: ", ftos(pathlib_bestcash_hits),"\n");
 554             LOG_TRACE("Nodes bestcash -   save: ", ftos(pathlib_bestcash_saved),"\n");
 555             LOG_TRACE("AStar done.\n");
 556 #endif
 557             return start;
 558         }
 559     }
 560
 561     LOG_TRACE("A* Faild to find a path! Try a smaller gridsize.\n");
 562
 563     pathlib_cleanup();
 564
 565     return world;
 566 }