data/qcsrc/server/pathlib.qc

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