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