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