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