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