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 not (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 not(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)
 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 not(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)
 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 not(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 not(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     }
 602     else
 603     {
 604         if(inwater(to))
 605         {
 606             pathlib_expandnode = pathlib_expandnode_box;
 607             pathlib_movenode   = pathlib_swimnode;
 608         }
 609         else
 610         {
 611
 612             pathlib_expandnode = pathlib_expandnode_star;
 613             pathlib_movenode   = pathlib_walknode;
 614             doedge = 1;
 615         }
 616     }
 617
 618     where = pathlib_movenode(parent.origin,to,0);
 619     if not(pathlib_movenode_goodnode)
 620         return 0;
 621
 622     if(doedge)
 623     if(edge_check(where,pathlib_edge_check_size))
 624         return 0;
 625
 626     if(parent.path_prev)
 627         pathlib_h_diagonal2sdp(parent.path_prev.origin,parent.origin,where,goal);
 628
 629     h = pathlib_heuristic(where,goal);
 630     g = pathlib_cost(parent,where,cost);
 631     f = g + h;
 632
 633     node = findradius(where,pathlib_gridsize * 0.75);
 634     while(node)
 635     {
 636         if(node.is_path_node == TRUE)
 637         {
 638             ++pathlib_merge_cnt;
 639             if(node.owner == openlist)
 640             {
 641                 if(node.pathlib_node_g > g)
 642                 {
 643                     node.pathlib_node_h = h;
 644                     node.pathlib_node_g = g;
 645                     node.pathlib_node_f = f;
 646                     node.path_prev = parent;
 647                 }
 648
 649                 if not (best_open_node)
 650                     best_open_node = node;
 651                 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
 652                     best_open_node = node;
 653             }
 654
 655             return 1;
 656         }
 657         node = node.chain;
 658     }
 659
 660     node = pathlib_mknode(where,parent);
 661     node.pathlib_node_h = h;
 662     node.pathlib_node_g = g;
 663     node.pathlib_node_f = f;
 664
 665     if not (best_open_node)
 666         best_open_node = node;
 667     else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
 668         best_open_node = node;
 669
 670     return 1;
 671 }
 672
 673 entity pathlib_getbestopen()
 674 {
 675     entity node;
 676     entity bestnode;
 677
 678     if(best_open_node)
 679     {
 680         ++pathlib_bestcash_hits;
 681         pathlib_bestcash_saved += pathlib_open_cnt;
 682
 683         return best_open_node;
 684     }
 685
 686     node = findchainentity(owner,openlist);
 687     if(!node)
 688         return world;
 689
 690     bestnode = node;
 691     while(node)
 692     {
 693         ++pathlib_bestopen_seached;
 694         if(node.pathlib_node_f < bestnode.pathlib_node_f)
 695             bestnode = node;
 696
 697         node = node.chain;
 698     }
 699
 700     return bestnode;
 701 }
 702
 703 void pathlib_close_node(entity node,vector goal)
 704 {
 705
 706     if(node.owner == closedlist)
 707     {
 708         dprint("Pathlib: Tried to close a closed node!\n");
 709         return;
 710     }
 711
 712     if(node == best_open_node)
 713         best_open_node = world;
 714
 715     ++pathlib_closed_cnt;
 716     --pathlib_open_cnt;
 717
 718     node.owner = closedlist;
 719
 720     if(vlen(node.origin - goal) <= pathlib_gridsize)
 721     {
 722         vector goalmove;
 723
 724         goalmove = pathlib_walknode(node.origin,goal,1);
 725         if(pathlib_movenode_goodnode)
 726         {
 727             goal_node         = node;
 728             pathlib_foundgoal = TRUE;
 729         }
 730     }
 731 }
 732
 733 float pathlib_expandnode_star(entity node, vector start, vector goal)
 734 {
 735     vector point;
 736     vector where;
 737     float nodecnt;
 738
 739     where = node.origin;
 740
 741     v_forward = '1 0 0';
 742     v_right   = '0 1 0';
 743
 744     // Forward
 745     point = where + v_forward * pathlib_gridsize;
 746     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
 747
 748     // Back
 749     point = where - v_forward * pathlib_gridsize;
 750     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
 751
 752     // Right
 753     point = where + v_right * pathlib_gridsize;
 754     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
 755
 756     // Left
 757     point = where - v_right * pathlib_gridsize;
 758     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
 759
 760     // Forward-right
 761     point = where + v_forward * pathlib_gridsize + v_right * pathlib_gridsize;
 762     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
 763
 764     // Forward-left
 765     point = where + v_forward * pathlib_gridsize - v_right * pathlib_gridsize;
 766     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
 767
 768     // Back-right
 769     point = where - v_forward * pathlib_gridsize + v_right * pathlib_gridsize;
 770     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
 771
 772     // Back-left
 773     point = where - v_forward * pathlib_gridsize - v_right * pathlib_gridsize;
 774     nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
 775
 776     return pathlib_open_cnt;
 777 }
 778
 779 float pathlib_expandnode_box(entity node, vector start, vector goal)
 780 {
 781     vector v;
 782
 783     for(v_z = node.origin_z - pathlib_gridsize; v_z <= node.origin_z + pathlib_gridsize; v_z += pathlib_gridsize)
 784     for(v_y = node.origin_y - pathlib_gridsize; v_y <= node.origin_y + pathlib_gridsize; v_y += pathlib_gridsize)
 785     for(v_x = node.origin_x - pathlib_gridsize; v_x <= node.origin_x + pathlib_gridsize; v_x += pathlib_gridsize)
 786     {
 787         if(vlen(v - node.origin))
 788             pathlib_makenode(node,start,v,goal,pathlib_movecost);
 789     }
 790
 791     return pathlib_open_cnt;
 792 }
 793
 794 void pathlib_cleanup()
 795 {
 796     entity node;
 797
 798     node = findfloat(world,is_path_node, TRUE);
 799     while(node)
 800     {
 801         dumpnode(node);
 802         node = findfloat(node,is_path_node, TRUE);
 803     }
 804
 805     if(openlist)
 806         remove(openlist);
 807
 808     if(closedlist)
 809         remove(closedlist);
 810
 811     best_open_node = world;
 812     openlist       = world;
 813     closedlist     = world;
 814 }
 815
 816 var float buildpath_nodefilter(vector n,vector c,vector p);
 817 float buildpath_nodefilter_directional(vector n,vector c,vector p)
 818 {
 819     vector d1,d2;
 820
 821     d2 = normalize(p - c);
 822     d1 = normalize(c - n);
 823
 824     if(vlen(d1-d2) < 0.25)
 825         return 1;
 826
 827     return 0;
 828 }
 829
 830 float buildpath_nodefilter_moveskip(vector n,vector c,vector p)
 831 {
 832     pathlib_walknode(p,n,1);
 833     if(pathlib_movenode_goodnode)
 834         return 1;
 835
 836     return 0;
 837 }
 838
 839 entity path_build(entity next, vector where, entity prev, entity start)
 840 {
 841     entity path;
 842
 843     if(prev && next)
 844         if(buildpath_nodefilter)
 845             if(buildpath_nodefilter(next.origin,where,prev.origin))
 846                 return next;
 847
 848
 849     path           = spawn();
 850     path.owner     = start;
 851     path.path_next = next;
 852
 853     setorigin(path,where);
 854
 855     if(!next)
 856         path.classname = "path_end";
 857     else
 858     {
 859         if(!prev)
 860             path.classname = "path_start";
 861         else
 862             path.classname = "path_node";
 863     }
 864
 865     return path;
 866 }
 867
 868 entity pathlib_astar(vector from,vector to)
 869 {
 870     entity path, start, end, open, n, ln;
 871     float ptime, ftime, ctime;
 872
 873     ptime = gettime(GETTIME_REALTIME);
 874
 875     pathlib_cleanup();
 876
 877     // Select water<->land capable node make/link
 878     pathlib_makenode     = pathlib_makenode_adaptive;
 879     // Select XYZ cost estimate
 880     pathlib_heuristic    = pathlib_h_diagonal3;
 881     // Select distance + waterfactor cost
 882     pathlib_cost         = pathlib_g_euclidean_water;
 883     // Select star expander
 884     pathlib_expandnode   = pathlib_expandnode_star;
 885     // Select walk simulation movement test
 886     pathlib_movenode     = pathlib_walknode;
 887     // Filter final nodes by direction
 888     buildpath_nodefilter = buildpath_nodefilter_directional;
 889
 890     // If the start is in water we need diffrent settings
 891     if(inwater(from))
 892     {
 893         // Select volumetric node expaner
 894         pathlib_expandnode = pathlib_expandnode_box;
 895
 896         // Water movement test
 897         pathlib_movenode   = pathlib_swimnode;
 898     }
 899
 900     if not(openlist)
 901         openlist       = spawn();
 902
 903     if not(closedlist)
 904         closedlist     = spawn();
 905
 906     if not(scraplist)
 907         scraplist      = spawn();
 908
 909     pathlib_closed_cnt       = 0;
 910     pathlib_open_cnt         = 0;
 911     pathlib_made_cnt         = 0;
 912     pathlib_merge_cnt        = 0;
 913     pathlib_searched_cnt     = 0;
 914     pathlib_bestopen_seached = 0;
 915     pathlib_bestcash_hits    = 0;
 916     pathlib_bestcash_saved   = 0;
 917     pathlib_recycle_cnt      = 0;
 918
 919     pathlib_gridsize       = 128;
 920     pathlib_movecost       = pathlib_gridsize;
 921     pathlib_movecost_diag  = vlen(('1 1 0' * pathlib_gridsize));
 922     pathlib_movecost_waterfactor = 1.1;
 923     pathlib_foundgoal      = 0;
 924
 925     walknode_boxmax   = self.maxs * 1.5;
 926     walknode_boxmin   = self.mins * 1.5;
 927
 928     pathlib_edge_check_size = (vlen(walknode_boxmin - walknode_boxmax) * 0.5);
 929
 930     walknode_boxup    = '0 0 2' * self.maxs_z;
 931     walknode_stepsize = 32;
 932     walknode_stepup   = '0 0 1' * walknode_stepsize;
 933     walknode_maxdrop  = '0 0 3' * walknode_stepsize;
 934
 935     from_x = fsnap(from_x,pathlib_gridsize);
 936     from_y = fsnap(from_y,pathlib_gridsize);
 937
 938     to_x = fsnap(to_x,pathlib_gridsize);
 939     to_y = fsnap(to_y,pathlib_gridsize);
 940
 941     dprint("AStar init. ", ftos(pathlib_scraplist_cnt), " nodes on scrap\n");
 942     path = pathlib_mknode(from,world);
 943     pathlib_close_node(path,to);
 944     if(pathlib_foundgoal)
 945     {
 946         dprint("AStar: Goal found on first node!\n");
 947
 948         open           = spawn();
 949         open.owner     = open;
 950         open.classname = "path_end";
 951         setorigin(open,path.origin);
 952
 953         pathlib_cleanup();
 954
 955         return open;
 956     }
 957
 958     if(pathlib_expandnode(path,from,to) <= 0)
 959     {
 960         dprint("AStar path fail.\n");
 961         pathlib_cleanup();
 962
 963         return world;
 964     }
 965
 966     best_open_node = pathlib_getbestopen();
 967     n = best_open_node;
 968     pathlib_close_node(best_open_node,to);
 969     if(inwater(n.origin))
 970         pathlib_expandnode_box(n,from,to);
 971     else
 972         pathlib_expandnode_star(n,from,to);
 973
 974     while(pathlib_open_cnt)
 975     {
 976         best_open_node = pathlib_getbestopen();
 977         n = best_open_node;
 978         pathlib_close_node(best_open_node,to);
 979
 980         if(inwater(n.origin))
 981             pathlib_expandnode_box(n,from,to);
 982         else
 983             pathlib_expandnode(n,from,to);
 984
 985         if(pathlib_foundgoal)
 986         {
 987             dprint("Target found. Rebuilding and filtering path...\n");
 988             ftime = gettime(GETTIME_REALTIME);
 989             ptime = ftime - ptime;
 990
 991             start = path_build(world,path.origin,world,world);
 992             end   = path_build(world,goal_node.origin,world,start);
 993             ln    = end;
 994
 995             open = goal_node;
 996             for(open = goal_node; open.path_prev != path; open = open.path_prev)
 997             {
 998                 n    = path_build(ln,open.origin,open.path_prev,start);
 999                 ln.path_prev = n;
1000                 ln = n;
1001             }
1002             start.path_next = n;
1003             n.path_prev = start;
1004             ftime = gettime(GETTIME_REALTIME) - ftime;
1005
1006             ctime = gettime(GETTIME_REALTIME);
1007             pathlib_cleanup();
1008             ctime = gettime(GETTIME_REALTIME) - ctime;
1009
1010
1011 #ifdef DEBUGPATHING
1012             pathlib_showpath2(start);
1013
1014             dprint("Time used -      pathfinding: ", ftos(ptime),"\n");
1015             dprint("Time used - rebuild & filter: ", ftos(ftime),"\n");
1016             dprint("Time used -          cleanup: ", ftos(ctime),"\n");
1017             dprint("Time used -            total: ", ftos(ptime + ftime + ctime),"\n");
1018             dprint("Time used -         # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)),"\n\n");
1019             dprint("Nodes -         created: ", ftos(pathlib_made_cnt),"\n");
1020             dprint("Nodes -            open: ", ftos(pathlib_open_cnt),"\n");
1021             dprint("Nodes -          merged: ", ftos(pathlib_merge_cnt),"\n");
1022             dprint("Nodes -          closed: ", ftos(pathlib_closed_cnt),"\n");
1023             dprint("Nodes -        searched: ", ftos(pathlib_searched_cnt),"\n");
1024
1025         if(pathlib_recycle_cnt)
1026             dprint("Nodes -      make/reuse: ", ftos(pathlib_made_cnt / pathlib_recycle_cnt),"\n");
1027         if(pathlib_recycle_cnt)
1028             dprint("Nodes -          reused: ", ftos(pathlib_recycle_cnt),"\n");
1029
1030             dprint("Nodes bestopen searched: ", ftos(pathlib_bestopen_seached),"\n");
1031             dprint("Nodes bestcash -   hits: ", ftos(pathlib_bestcash_hits),"\n");
1032             dprint("Nodes bestcash -   save: ", ftos(pathlib_bestcash_saved),"\n");
1033             dprint("AStar done. ", ftos(pathlib_scraplist_cnt), " nodes on scrap\n\n");
1034 #endif
1035             return start;
1036         }
1037     }
1038
1039     dprint("A* Faild to find a path! Try a smaller gridsize.\n");
1040
1041     pathlib_cleanup();
1042
1043     return world;
1044 }
1045
1046
1047