qcsrc/server/pathlib.qc

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