qcsrc/server/pathlib.qc

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