qcsrc/server/pathlib.qc

   1 #include "pathlib.qh"
   2 #include "_.qh"
   3
   4 #include "g_subs.qh"
   5
   6 #define medium spawnshieldtime
   7
   8 //#define DEBUGPATHING
   9 #ifdef DEBUGPATHING
  10 float edge_show(vector point,float fsize);
  11 void mark_error(vector where,float lifetime);
  12 void mark_info(vector where,float lifetime);
  13 entity mark_misc(vector where,float lifetime);
  14
  15 void pathlib_showpath(entity start)
  16 {
  17     entity e;
  18     e = start;
  19     while(e.path_next)
  20     {
  21         te_lightning1(e,e.origin,e.path_next.origin);
  22         e = e.path_next;
  23     }
  24 }
  25
  26 void path_dbg_think()
  27 {
  28     pathlib_showpath(self);
  29     self.nextthink = time + 1;
  30 }
  31
  32 void __showpath2_think()
  33 {
  34     mark_info(self.origin,1);
  35     if(self.path_next)
  36     {
  37         self.path_next.think     = __showpath2_think;
  38         self.path_next.nextthink = time + 0.15;
  39     }
  40     else
  41     {
  42         self.owner.think     = __showpath2_think;
  43         self.owner.nextthink = time + 0.15;
  44     }
  45 }
  46
  47 void pathlib_showpath2(entity path)
  48 {
  49     path.think     = __showpath2_think;
  50     path.nextthink = time;
  51 }
  52
  53 #endif
  54
  55 void pathlib_deletepath(entity start)
  56 {
  57     entity e;
  58
  59     e = findchainentity(owner, start);
  60     while(e)
  61     {
  62         e.think = SUB_Remove;
  63         e.nextthink = time;
  64         e = e.chain;
  65     }
  66 }
  67
  68 float fsnap(float val,float fsize)
  69 {
  70     return rint(val / fsize) * fsize;
  71 }
  72
  73 vector vsnap(vector point,float fsize)
  74 {
  75     vector vret;
  76
  77     vret.x = rint(point.x / fsize) * fsize;
  78     vret.y = rint(point.y / fsize) * fsize;
  79     vret.z = ceil(point.z / fsize) * fsize;
  80
  81     return vret;
  82 }
  83
  84 float  walknode_stepsize;
  85 vector walknode_stepup;
  86 vector walknode_maxdrop;
  87 vector walknode_boxup;
  88 vector walknode_boxmax;
  89 vector walknode_boxmin;
  90 float  pathlib_movenode_goodnode;
  91
  92 float floor_ok(vector point)
  93 {
  94     float pc;
  95
  96     if(trace_dphitq3surfaceflags & Q3SURFACEFLAG_SKY)
  97         return 0;
  98
  99     pc = pointcontents(point);
 100
 101     switch(pc)
 102     {
 103         case CONTENT_SOLID:
 104         case CONTENT_SLIME:
 105         case CONTENT_LAVA:
 106         case CONTENT_SKY:
 107             return 0;
 108         case CONTENT_EMPTY:
 109             if(pointcontents(point - '0 0 1') != CONTENT_SOLID)
 110                 return 0;
 111             break;
 112         case CONTENT_WATER:
 113             return 1;
 114     }
 115     if(pointcontents(point - '0 0 1') == CONTENT_SOLID)
 116         return 1;
 117
 118     return 0;
 119 }
 120
 121 #define _pcheck(p) traceline(p+z_up,p-z_down,MOVE_WORLDONLY,self); if (!floor_ok(trace_endpos)) return 1
 122 float edge_check(vector point,float fsize)
 123 {
 124     vector z_up,z_down;
 125
 126     z_up   = '0 0 1' * fsize;
 127     z_down = '0 0 1' * fsize;
 128
 129     _pcheck(point + ('1 1 0'  * fsize));
 130     _pcheck(point + ('1 -1 0'  * fsize));
 131     _pcheck(point + ('1 0 0' * fsize));
 132
 133     _pcheck(point + ('0 1 0'  * fsize));
 134     _pcheck(point + ('0 -1 0' * fsize));
 135
 136     _pcheck(point + ('-1 0 0'  * fsize));
 137     _pcheck(point + ('-1 1 0'  * fsize));
 138     _pcheck(point + ('-1 -1 0' * fsize));
 139
 140     return 0;
 141 }
 142
 143 #ifdef DEBUGPATHING
 144 #define _pshow(p) mark_error(p,10)
 145 float edge_show(vector point,float fsize)
 146 {
 147
 148     _pshow(point + ('1 1 0'  * fsize));
 149     _pshow(point + ('1 -1 0' * fsize));
 150     _pshow(point + ('1 0 0'  * fsize));
 151
 152     _pshow(point + ('0 1 0'  * fsize));
 153     _pshow(point + ('0 -1 0' * fsize));
 154
 155     _pshow(point + ('-1 0 0'  * fsize));
 156     _pshow(point + ('-1 1 0'  * fsize));
 157     _pshow(point + ('-1 -1 0' * fsize));
 158
 159     return 0;
 160 }
 161 #endif
 162
 163 var vector pathlib_movenode(vector start,vector end,float doedge);
 164 vector pathlib_wateroutnode(vector start,vector end,float doedge)
 165 {
 166     vector surface;
 167
 168     pathlib_movenode_goodnode = 0;
 169
 170     end.x = fsnap(end.x, pathlib_gridsize);
 171     end.y = fsnap(end.y, pathlib_gridsize);
 172
 173     traceline(end + ('0 0 0.25' * pathlib_gridsize),end - ('0 0 1' * pathlib_gridsize),MOVE_WORLDONLY,self);
 174     end = trace_endpos;
 175
 176     if(pointcontents(end - '0 0 1') != CONTENT_SOLID)
 177         return end;
 178
 179     for(surface = start ; surface.z < (end.z + 32); ++surface.z)
 180     {
 181         if(pointcontents(surface) == CONTENT_EMPTY)
 182             break;
 183     }
 184
 185     if(pointcontents(surface + '0 0 1') != CONTENT_EMPTY)
 186         return end;
 187
 188     tracebox(start + '0 0 64', walknode_boxmin,walknode_boxmax, end + '0 0 64', MOVE_WORLDONLY, self);
 189     if(trace_fraction == 1)
 190         pathlib_movenode_goodnode = 1;
 191
 192     if(fabs(surface.z - end.z) > 32)
 193         pathlib_movenode_goodnode = 0;
 194
 195     return end;
 196 }
 197
 198 vector pathlib_swimnode(vector start,vector end,float doedge)
 199 {
 200     pathlib_movenode_goodnode = 0;
 201
 202     if(pointcontents(start) != CONTENT_WATER)
 203         return end;
 204
 205     end.x = fsnap(end.x, pathlib_gridsize);
 206     end.y = fsnap(end.y, pathlib_gridsize);
 207
 208     if(pointcontents(end) == CONTENT_EMPTY)
 209         return pathlib_wateroutnode( start, end, doedge);
 210
 211     tracebox(start, walknode_boxmin,walknode_boxmax, end, MOVE_WORLDONLY, self);
 212     if(trace_fraction == 1)
 213         pathlib_movenode_goodnode = 1;
 214
 215     return end;
 216 }
 217
 218 vector pathlib_flynode(vector start,vector end)
 219 {
 220     pathlib_movenode_goodnode = 0;
 221
 222     end.x = fsnap(end.x, pathlib_gridsize);
 223     end.y = fsnap(end.y, pathlib_gridsize);
 224
 225     tracebox(start, walknode_boxmin,walknode_boxmax, end, MOVE_WORLDONLY, self);
 226     if(trace_fraction == 1)
 227         pathlib_movenode_goodnode = 1;
 228
 229     return end;
 230 }
 231
 232 vector pathlib_walknode(vector start,vector end,float doedge)
 233 {
 234     vector direction,point,last_point,s,e;
 235     float steps, distance, i,laststep;
 236
 237     pathlib_movenode_goodnode = 0;
 238
 239     s   = start;
 240     e   = end;
 241     e.z = 0;
 242     s.z = 0;
 243     direction  = normalize(s - e);
 244
 245     distance    = vlen(start - end);
 246     laststep    = distance / walknode_stepsize;
 247     steps       = floor(laststep);
 248     laststep    = laststep - steps;
 249
 250     point = start;
 251     s     = point + walknode_stepup;
 252     e     = point - walknode_maxdrop;
 253
 254     traceline(s, e,MOVE_WORLDONLY,self);
 255     if(trace_fraction == 1.0)
 256         return trace_endpos;
 257
 258     if (floor_ok(trace_endpos) == 0)
 259         return trace_endpos;
 260
 261     last_point = trace_endpos;
 262
 263     for(i = 0; i < steps; ++i)
 264     {
 265         point = last_point + direction * walknode_stepsize;
 266
 267         s = point + walknode_stepup;
 268         e = point - walknode_maxdrop;
 269         traceline(s, e,MOVE_WORLDONLY,self);
 270         if(trace_fraction == 1.0)
 271             return trace_endpos;
 272
 273         point = trace_endpos;
 274         if (!floor_ok(trace_endpos))
 275             return trace_endpos;
 276
 277         tracebox(last_point + walknode_boxup, walknode_boxmin,walknode_boxmax, point + walknode_boxup, MOVE_WORLDONLY, self);
 278         if(trace_fraction != 1.0)
 279             return trace_endpos;
 280
 281         if(doedge)
 282         if(edge_check(point,pathlib_edge_check_size))
 283             return trace_endpos;
 284
 285         last_point = point;
 286     }
 287
 288     point = last_point + direction * walknode_stepsize * laststep;
 289
 290     point.x = fsnap(point.x, pathlib_gridsize);
 291     point.y = fsnap(point.y, pathlib_gridsize);
 292
 293     s = point + walknode_stepup;
 294     e = point - walknode_maxdrop;
 295     traceline(s, e,MOVE_WORLDONLY,self);
 296
 297     if(trace_fraction == 1.0)
 298         return trace_endpos;
 299
 300     point = trace_endpos;
 301
 302     if (!floor_ok(trace_endpos))
 303         return trace_endpos;
 304
 305     tracebox(last_point + walknode_boxup, walknode_boxmin,walknode_boxmax, point + walknode_boxup, MOVE_WORLDONLY, self);
 306     if(trace_fraction != 1.0)
 307         return trace_endpos;
 308
 309     pathlib_movenode_goodnode = 1;
 310     return point;
 311 }
 312
 313 var float pathlib_cost(entity parent,vector to, float static_cost);
 314 float pathlib_g_static(entity parent,vector to, float static_cost)
 315 {
 316     if(inwater(to))
 317         return parent.pathlib_node_g + static_cost * pathlib_movecost_waterfactor;
 318     else
 319         return parent.pathlib_node_g + static_cost;
 320 }
 321
 322 float pathlib_g_static_water(entity parent,vector to, float static_cost)
 323 {
 324     if(inwater(to))
 325         return parent.pathlib_node_g + static_cost * pathlib_movecost_waterfactor;
 326     else
 327         return parent.pathlib_node_g + static_cost;
 328 }
 329
 330 float pathlib_g_euclidean(entity parent,vector to, float static_cost)
 331 {
 332     return parent.pathlib_node_g + vlen(parent.origin - to);
 333 }
 334 float pathlib_g_euclidean_water(entity parent,vector to, float static_cost)
 335 {
 336     if(inwater(to))
 337         return parent.pathlib_node_g + vlen(parent.origin - to) * pathlib_movecost_waterfactor;
 338     else
 339         return parent.pathlib_node_g + vlen(parent.origin - to);
 340 }
 341
 342 var float(vector from,vector to) pathlib_heuristic;
 343
 344 /**
 345     Manhattan Menas we expect to move up,down left or right
 346     No diagonal moves espected. (like moving bewteen city blocks)
 347 **/
 348 float pathlib_h_manhattan(vector a,vector b)
 349 {
 350     //h(n) = D * (abs(n.x-goal.x) + abs(n.y-goal.y))
 351
 352     float h;
 353     h  = fabs(a.x - b.x);
 354     h += fabs(a.y - b.y);
 355     h *= pathlib_gridsize;
 356
 357     return h;
 358 }
 359
 360 /**
 361     This heuristic consider both stright and disagonal moves
 362     to have teh same cost.
 363 **/
 364 float pathlib_h_diagonal(vector a,vector b)
 365 {
 366     //h(n) = D * max(abs(n.x-goal.x), abs(n.y-goal.y))
 367     float h,x,y;
 368
 369     x = fabs(a.x - b.x);
 370     y = fabs(a.y - b.y);
 371     h = pathlib_movecost * max(x,y);
 372
 373     return h;
 374 }
 375
 376 /**
 377     This heuristic only considers the stright line distance.
 378     Will usualy mean a lower H then G meaning A* Will speand more
 379     and run slower.
 380 **/
 381 float pathlib_h_euclidean(vector a,vector b)
 382 {
 383     return vlen(a - b);
 384 }
 385
 386 /**
 387     This heuristic consider both stright and disagonal moves,
 388     But has a separate cost for diagonal moves.
 389 **/
 390 float pathlib_h_diagonal2(vector a,vector b)
 391 {
 392     float h_diag,h_str,h,x,y;
 393
 394     /*
 395     h_diagonal(n) = min(abs(n.x-goal.x), abs(n.y-goal.y))
 396     h_straight(n) = (abs(n.x-goal.x) + abs(n.y-goal.y))
 397     h(n) = D2 * h_diagonal(n) + D * (h_straight(n) - 2*h_diagonal(n)))
 398     */
 399
 400     x = fabs(a.x - b.x);
 401     y = fabs(a.y - b.y);
 402
 403     h_diag = min(x,y);
 404     h_str = x + y;
 405
 406     h =  pathlib_movecost_diag * h_diag;
 407     h += pathlib_movecost * (h_str - 2 * h_diag);
 408
 409     return h;
 410 }
 411
 412 /**
 413     This heuristic consider both stright and disagonal moves,
 414     But has a separate cost for diagonal moves.
 415
 416
 417 **/
 418 float pathlib_h_diagonal2sdp(vector preprev,vector prev,vector point,vector end)
 419 {
 420     float h_diag,h_str,h,x,y,z;
 421
 422     //h_diagonal(n) = min(abs(n.x-goal.x), abs(n.y-goal.y))
 423     //h_straight(n) = (abs(n.x-goal.x) + abs(n.y-goal.y))
 424     //h(n) = D2 * h_diagonal(n) + D * (h_straight(n) - 2*h_diagonal(n)))
 425
 426     x = fabs(point.x - end.x);
 427     y = fabs(point.y - end.y);
 428     z = fabs(point.z - end.z);
 429
 430     h_diag = min3(x,y,z);
 431     h_str = x + y + z;
 432
 433     h =  pathlib_movecost_diag * h_diag;
 434     h += pathlib_movecost * (h_str - 2 * h_diag);
 435
 436     float m;
 437     vector d1,d2;
 438
 439     d1 = normalize(preprev - point);
 440     d2 = normalize(prev    - point);
 441     m = vlen(d1-d2);
 442     //bprint("pathlib_h_diagonal2sdp-M = ",ftos(m),"\n");
 443
 444     return h * m;
 445 }
 446
 447
 448 float pathlib_h_diagonal3(vector a,vector b)
 449 {
 450     float h_diag,h_str,h,x,y,z;
 451
 452     //h_diagonal(n) = min(abs(n.x-goal.x), abs(n.y-goal.y))
 453     //h_straight(n) = (abs(n.x-goal.x) + abs(n.y-goal.y))
 454     //h(n) = D2 * h_diagonal(n) + D * (h_straight(n) - 2*h_diagonal(n)))
 455
 456     x = fabs(a.x - b.x);
 457     y = fabs(a.y - b.y);
 458     z = fabs(a.z - b.z);
 459
 460     h_diag = min3(x,y,z);
 461     h_str = x + y + z;
 462
 463     h =  pathlib_movecost_diag * h_diag;
 464     h += pathlib_movecost * (h_str - 2 * h_diag);
 465
 466     return h;
 467 }
 468
 469 const float PATHLIB_NODEEXPIRE = 0.05;
 470 float pathlib_scraplist_cnt;
 471 entity newnode()
 472 {
 473     entity n;
 474 #ifdef PATHLIB_USE_NODESCRAP
 475     if(pathlib_scraplist_cnt)
 476     {
 477         n = findentity(world,owner,scraplist);
 478         if(n)
 479         {
 480             --pathlib_scraplist_cnt;
 481             ++pathlib_recycle_cnt;
 482             return n;
 483         }
 484         else
 485             pathlib_scraplist_cnt = 0;
 486     }
 487 #endif
 488     ++pathlib_made_cnt;
 489     n = spawn();
 490     n.think      = SUB_Remove;
 491     n.nextthink  = time + PATHLIB_NODEEXPIRE;
 492     return n;
 493 }
 494
 495 void dumpnode(entity n)
 496 {
 497 #ifdef PATHLIB_USE_NODESCRAP
 498     ++pathlib_scraplist_cnt;
 499
 500     n.path_next    = world;
 501     n.path_prev    = world;
 502     n.is_path_node = false;
 503     n.owner        = scraplist;
 504 #else
 505     //n.is_path_node = false;
 506     n.think        = SUB_Remove;
 507     n.nextthink    = time;
 508 #endif
 509 }
 510
 511 entity pathlib_mknode(vector where,entity parent)
 512 {
 513     entity node;
 514
 515     node              = newnode();
 516     node.is_path_node = true;
 517     node.owner        = openlist;
 518     node.path_prev    = parent;
 519
 520     setorigin(node, where);
 521
 522     ++pathlib_open_cnt;
 523
 524     node.medium = pointcontents(where);
 525
 526     return node;
 527 }
 528
 529 var float pathlib_expandnode(entity node, vector start, vector goal);
 530 float pathlib_expandnode_star(entity node, vector start, vector goal);
 531 float pathlib_expandnode_box(entity node, vector start, vector goal);
 532
 533 var float pathlib_makenode(entity parent,vector start, vector to, vector goal,float cost);
 534 float pathlib_makenode_adaptive(entity parent,vector start, vector to, vector goal,float cost)
 535 {
 536     entity node;
 537     float h,g,f,doedge;
 538     vector where;
 539
 540     ++pathlib_searched_cnt;
 541
 542     if(inwater(parent.origin))
 543     {
 544         pathlib_expandnode = pathlib_expandnode_box;
 545         pathlib_movenode   = pathlib_swimnode;
 546         doedge = 0;
 547     }
 548     else
 549     {
 550         if(inwater(to))
 551         {
 552             pathlib_expandnode = pathlib_expandnode_box;
 553             pathlib_movenode   = pathlib_swimnode;
 554             doedge = 0;
 555         }
 556         else
 557         {
 558
 559             pathlib_expandnode = pathlib_expandnode_star;
 560             pathlib_movenode   = pathlib_walknode;
 561             doedge = 1;
 562         }
 563     }
 564
 565     where = pathlib_movenode(parent.origin,to,0);
 566     if (!pathlib_movenode_goodnode)
 567         return 0;
 568
 569     if(doedge)
 570     if(edge_check(where,pathlib_edge_check_size))
 571         return 0;
 572
 573     if(parent.path_prev)
 574         pathlib_h_diagonal2sdp(parent.path_prev.origin,parent.origin,where,goal);
 575
 576     h = pathlib_heuristic(where,goal);
 577     g = pathlib_cost(parent,where,cost);
 578     f = g + h;
 579
 580     node = findradius(where,pathlib_gridsize * 0.75);
 581     while(node)
 582     {
 583         if(node.is_path_node == true)
 584         {
 585             ++pathlib_merge_cnt;
 586             if(node.owner == openlist)
 587             {
 588                 if(node.pathlib_node_g > g)
 589                 {
 590                     node.pathlib_node_h = h;
 591                     node.pathlib_node_g = g;
 592                     node.pathlib_node_f = f;
 593                     node.path_prev = parent;
 594                 }
 595
 596                 if (!best_open_node)
 597                     best_open_node = node;
 598                 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
 599                     best_open_node = node;
 600             }
 601
 602             return 1;
 603         }
 604         node = node.chain;
 605     }
 606
 607     node = pathlib_mknode(where,parent);
 608     node.pathlib_node_h = h;
 609     node.pathlib_node_g = g;
 610     node.pathlib_node_f = f;
 611
 612     if (!best_open_node)
 613         best_open_node = node;
 614     else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
 615         best_open_node = node;
 616
 617     return 1;
 618 }
 619
 620 entity pathlib_getbestopen()
 621 {
 622     entity node;
 623     entity bestnode;
 624
 625     if(best_open_node)
 626     {
 627         ++pathlib_bestcash_hits;
 628         pathlib_bestcash_saved += pathlib_open_cnt;
 629
 630         return best_open_node;
 631     }
 632
 633     node = findchainentity(owner,openlist);
 634     if(!node)
 635         return world;
 636
 637     bestnode = node;
 638     while(node)
 639     {
 640         ++pathlib_bestopen_seached;
 641         if(node.pathlib_node_f < bestnode.pathlib_node_f)
 642             bestnode = node;
 643
 644         node = node.chain;
 645     }
 646
 647     return bestnode;
 648 }
 649
 650 void pathlib_close_node(entity node,vector goal)
 651 {
 652
 653     if(node.owner == closedlist)
 654     {
 655         dprint("Pathlib: Tried to close a closed node!\n");
 656         return;
 657     }
 658
 659     if(node == best_open_node)
 660         best_open_node = world;
 661
 662     ++pathlib_closed_cnt;
 663     --pathlib_open_cnt;
 664
 665     node.owner = closedlist;
 666
 667     if(vlen(node.origin - goal) <= pathlib_gridsize)
 668     {
 669         vector goalmove;
 670
 671         goalmove = pathlib_walknode(node.origin,goal,1);
 672         if(pathlib_movenode_goodnode)
 673         {
 674             goal_node         = node;
 675             pathlib_foundgoal = true;
 676         }
 677     }
 678 }
 679
 680 float pathlib_expandnode_star(entity node, vector start, vector goal)
 681 {
 682     vector point;
 683     vector where;
 684     float nodecnt = 0;
 685
 686     where = node.origin;
 687
 688     v_forward = '1 0 0';
 689     v_right   = '0 1 0';
 690
 691     // Forward
 692     point = where + v_forward * pathlib_gridsize;
 693     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
 694
 695     // Back
 696     point = where - v_forward * pathlib_gridsize;
 697     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
 698
 699     // Right
 700     point = where + v_right * pathlib_gridsize;
 701     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
 702
 703     // Left
 704     point = where - v_right * pathlib_gridsize;
 705     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
 706
 707     // Forward-right
 708     point = where + v_forward * pathlib_gridsize + v_right * pathlib_gridsize;
 709     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
 710
 711     // Forward-left
 712     point = where + v_forward * pathlib_gridsize - v_right * pathlib_gridsize;
 713     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
 714
 715     // Back-right
 716     point = where - v_forward * pathlib_gridsize + v_right * pathlib_gridsize;
 717     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
 718
 719     // Back-left
 720     point = where - v_forward * pathlib_gridsize - v_right * pathlib_gridsize;
 721     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
 722
 723     return pathlib_open_cnt;
 724 }
 725
 726 float pathlib_expandnode_box(entity node, vector start, vector goal)
 727 {
 728     vector v;
 729
 730     for(v.z = node.origin.z - pathlib_gridsize; v.z <= node.origin.z + pathlib_gridsize; v.z += pathlib_gridsize)
 731     for(v.y = node.origin.y - pathlib_gridsize; v.y <= node.origin.y + pathlib_gridsize; v.y += pathlib_gridsize)
 732     for(v.x = node.origin.x - pathlib_gridsize; v.x <= node.origin.x + pathlib_gridsize; v.x += pathlib_gridsize)
 733     {
 734         if(vlen(v - node.origin))
 735             pathlib_makenode(node,start,v,goal,pathlib_movecost);
 736     }
 737
 738     return pathlib_open_cnt;
 739 }
 740
 741 void pathlib_cleanup()
 742 {
 743     entity node;
 744
 745     node = findfloat(world,is_path_node, true);
 746     while(node)
 747     {
 748         dumpnode(node);
 749         node = findfloat(node,is_path_node, true);
 750     }
 751
 752     if(openlist)
 753         remove(openlist);
 754
 755     if(closedlist)
 756         remove(closedlist);
 757
 758     best_open_node = world;
 759     openlist       = world;
 760     closedlist     = world;
 761 }
 762
 763 var float buildpath_nodefilter(vector n,vector c,vector p);
 764 float buildpath_nodefilter_directional(vector n,vector c,vector p)
 765 {
 766     vector d1,d2;
 767
 768     d2 = normalize(p - c);
 769     d1 = normalize(c - n);
 770
 771     if(vlen(d1-d2) < 0.25)
 772         return 1;
 773
 774     return 0;
 775 }
 776
 777 float buildpath_nodefilter_moveskip(vector n,vector c,vector p)
 778 {
 779     pathlib_walknode(p,n,1);
 780     if(pathlib_movenode_goodnode)
 781         return 1;
 782
 783     return 0;
 784 }
 785
 786 entity path_build(entity next, vector where, entity prev, entity start)
 787 {
 788     entity path;
 789
 790     if(prev && next)
 791         if(buildpath_nodefilter)
 792             if(buildpath_nodefilter(next.origin,where,prev.origin))
 793                 return next;
 794
 795
 796     path           = spawn();
 797     path.owner     = start;
 798     path.path_next = next;
 799
 800     setorigin(path,where);
 801
 802     if(!next)
 803         path.classname = "path_end";
 804     else
 805     {
 806         if(!prev)
 807             path.classname = "path_start";
 808         else
 809             path.classname = "path_node";
 810     }
 811
 812     return path;
 813 }
 814
 815 entity pathlib_astar(vector from,vector to)
 816 {
 817     entity path, start, end, open, n, ln;
 818     float ptime, ftime, ctime;
 819
 820     ptime = gettime(GETTIME_REALTIME);
 821
 822     pathlib_cleanup();
 823
 824     // Select water<->land capable node make/link
 825     pathlib_makenode     = pathlib_makenode_adaptive;
 826     // Select XYZ cost estimate
 827     pathlib_heuristic    = pathlib_h_diagonal3;
 828     // Select distance + waterfactor cost
 829     pathlib_cost         = pathlib_g_euclidean_water;
 830     // Select star expander
 831     pathlib_expandnode   = pathlib_expandnode_star;
 832     // Select walk simulation movement test
 833     pathlib_movenode     = pathlib_walknode;
 834     // Filter final nodes by direction
 835     buildpath_nodefilter = buildpath_nodefilter_directional;
 836
 837     // If the start is in water we need diffrent settings
 838     if(inwater(from))
 839     {
 840         // Select volumetric node expaner
 841         pathlib_expandnode = pathlib_expandnode_box;
 842
 843         // Water movement test
 844         pathlib_movenode   = pathlib_swimnode;
 845     }
 846
 847     if (!openlist)
 848         openlist       = spawn();
 849
 850     if (!closedlist)
 851         closedlist     = spawn();
 852
 853     if (!scraplist)
 854         scraplist      = spawn();
 855
 856     pathlib_closed_cnt       = 0;
 857     pathlib_open_cnt         = 0;
 858     pathlib_made_cnt         = 0;
 859     pathlib_merge_cnt        = 0;
 860     pathlib_searched_cnt     = 0;
 861     pathlib_bestopen_seached = 0;
 862     pathlib_bestcash_hits    = 0;
 863     pathlib_bestcash_saved   = 0;
 864     pathlib_recycle_cnt      = 0;
 865
 866     pathlib_gridsize       = 128;
 867     pathlib_movecost       = pathlib_gridsize;
 868     pathlib_movecost_diag  = vlen(('1 1 0' * pathlib_gridsize));
 869     pathlib_movecost_waterfactor = 1.1;
 870     pathlib_foundgoal      = 0;
 871
 872     walknode_boxmax   = self.maxs * 1.5;
 873     walknode_boxmin   = self.mins * 1.5;
 874
 875     pathlib_edge_check_size = (vlen(walknode_boxmin - walknode_boxmax) * 0.5);
 876
 877     walknode_boxup    = '0 0 2' * self.maxs.z;
 878     walknode_stepsize = 32;
 879     walknode_stepup   = '0 0 1' * walknode_stepsize;
 880     walknode_maxdrop  = '0 0 3' * walknode_stepsize;
 881
 882     from.x = fsnap(from.x,pathlib_gridsize);
 883     from.y = fsnap(from.y,pathlib_gridsize);
 884
 885     to.x = fsnap(to.x,pathlib_gridsize);
 886     to.y = fsnap(to.y,pathlib_gridsize);
 887
 888     dprint("AStar init. ", ftos(pathlib_scraplist_cnt), " nodes on scrap\n");
 889     path = pathlib_mknode(from,world);
 890     pathlib_close_node(path,to);
 891     if(pathlib_foundgoal)
 892     {
 893         dprint("AStar: Goal found on first node!\n");
 894
 895         open           = spawn();
 896         open.owner     = open;
 897         open.classname = "path_end";
 898         setorigin(open,path.origin);
 899
 900         pathlib_cleanup();
 901
 902         return open;
 903     }
 904
 905     if(pathlib_expandnode(path,from,to) <= 0)
 906     {
 907         dprint("AStar path fail.\n");
 908         pathlib_cleanup();
 909
 910         return world;
 911     }
 912
 913     best_open_node = pathlib_getbestopen();
 914     n = best_open_node;
 915     pathlib_close_node(best_open_node,to);
 916     if(inwater(n.origin))
 917         pathlib_expandnode_box(n,from,to);
 918     else
 919         pathlib_expandnode_star(n,from,to);
 920
 921     while(pathlib_open_cnt)
 922     {
 923         best_open_node = pathlib_getbestopen();
 924         n = best_open_node;
 925         pathlib_close_node(best_open_node,to);
 926
 927         if(inwater(n.origin))
 928             pathlib_expandnode_box(n,from,to);
 929         else
 930             pathlib_expandnode(n,from,to);
 931
 932         if(pathlib_foundgoal)
 933         {
 934             dprint("Target found. Rebuilding and filtering path...\n");
 935             ftime = gettime(GETTIME_REALTIME);
 936             ptime = ftime - ptime;
 937
 938             start = path_build(world,path.origin,world,world);
 939             end   = path_build(world,goal_node.origin,world,start);
 940             ln    = end;
 941
 942             open = goal_node;
 943             for(open = goal_node; open.path_prev != path; open = open.path_prev)
 944             {
 945                 n    = path_build(ln,open.origin,open.path_prev,start);
 946                 ln.path_prev = n;
 947                 ln = n;
 948             }
 949             start.path_next = n;
 950             n.path_prev = start;
 951             ftime = gettime(GETTIME_REALTIME) - ftime;
 952
 953             ctime = gettime(GETTIME_REALTIME);
 954             pathlib_cleanup();
 955             ctime = gettime(GETTIME_REALTIME) - ctime;
 956
 957
 958 #ifdef DEBUGPATHING
 959             pathlib_showpath2(start);
 960
 961             dprint("Time used -      pathfinding: ", ftos(ptime),"\n");
 962             dprint("Time used - rebuild & filter: ", ftos(ftime),"\n");
 963             dprint("Time used -          cleanup: ", ftos(ctime),"\n");
 964             dprint("Time used -            total: ", ftos(ptime + ftime + ctime),"\n");
 965             dprint("Time used -         # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)),"\n\n");
 966             dprint("Nodes -         created: ", ftos(pathlib_made_cnt),"\n");
 967             dprint("Nodes -            open: ", ftos(pathlib_open_cnt),"\n");
 968             dprint("Nodes -          merged: ", ftos(pathlib_merge_cnt),"\n");
 969             dprint("Nodes -          closed: ", ftos(pathlib_closed_cnt),"\n");
 970             dprint("Nodes -        searched: ", ftos(pathlib_searched_cnt),"\n");
 971
 972         if(pathlib_recycle_cnt)
 973             dprint("Nodes -      make/reuse: ", ftos(pathlib_made_cnt / pathlib_recycle_cnt),"\n");
 974         if(pathlib_recycle_cnt)
 975             dprint("Nodes -          reused: ", ftos(pathlib_recycle_cnt),"\n");
 976
 977             dprint("Nodes bestopen searched: ", ftos(pathlib_bestopen_seached),"\n");
 978             dprint("Nodes bestcash -   hits: ", ftos(pathlib_bestcash_hits),"\n");
 979             dprint("Nodes bestcash -   save: ", ftos(pathlib_bestcash_saved),"\n");
 980             dprint("AStar done. ", ftos(pathlib_scraplist_cnt), " nodes on scrap\n\n");
 981 #endif
 982             return start;
 983         }
 984     }
 985
 986     dprint("A* Faild to find a path! Try a smaller gridsize.\n");
 987
 988     pathlib_cleanup();
 989
 990     return world;
 991 }