qcsrc/server/pathlib/main.qc

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