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