data/qcsrc/server/pathlib/main.qc

   1 void pathlib_deletepath(entity start)\r
   2 {\r
   3     entity e;\r
   4 \r
   5     e = findchainentity(owner, start);\r
   6     while(e)\r
   7     {\r
   8         e.think = SUB_Remove;\r
   9         e.nextthink = time;\r
  10         e = e.chain;\r
  11     }\r
  12 }\r
  13 \r
  14 //#define PATHLIB_NODEEXPIRE 0.05\r
  15 #define PATHLIB_NODEEXPIRE 20\r
  16 \r
  17 void dumpnode(entity n)\r
  18 {\r
  19     n.is_path_node = FALSE;\r
  20     n.think        = SUB_Remove;\r
  21     n.nextthink    = time;\r
  22 }\r
  23 \r
  24 entity pathlib_mknode(vector where,entity parent)\r
  25 {\r
  26     entity node;\r
  27 \r
  28     node = pathlib_nodeatpoint(where);\r
  29     if(node)\r
  30     {\r
  31         mark_error(where,60);\r
  32         return node;\r
  33     }\r
  34 \r
  35     node = spawn();\r
  36 \r
  37     node.think        = SUB_Remove;\r
  38     node.nextthink    = time + PATHLIB_NODEEXPIRE;\r
  39     node.is_path_node = TRUE;\r
  40     node.owner        = openlist;\r
  41     node.path_prev    = parent;\r
  42 \r
  43     setmodel(node,"models/pathlib/square.md3");\r
  44     setsize(node,'0 0 0','0 0 0');\r
  45     node.colormod = randomvec() * 2;\r
  46     node.alpha = 0.25;\r
  47     node.scale     = pathlib_gridsize / 512.001;\r
  48 \r
  49     //pathlib_showsquare2(node,'1 1 1',0);//(node.medium & CONTENT_EMPTY));\r
  50     setorigin(node, where);\r
  51     node.medium = pointcontents(where);\r
  52 \r
  53     mark_info(where,60);\r
  54     //pathlib_showsquare(where,1,30);\r
  55 \r
  56 \r
  57     ++pathlib_made_cnt;\r
  58     ++pathlib_open_cnt;\r
  59 \r
  60     return node;\r
  61 }\r
  62 \r
  63 float pathlib_makenode_adaptive(entity parent,vector start, vector to, vector goal,float cost)\r
  64 {\r
  65     entity node;\r
  66     float h,g,f,doedge;\r
  67     vector where;\r
  68 \r
  69     ++pathlib_searched_cnt;\r
  70 \r
  71     if(inwater(parent.origin))\r
  72     {\r
  73         pathlib_expandnode = pathlib_expandnode_box;\r
  74         pathlib_movenode   = pathlib_swimnode;\r
  75     }\r
  76     else\r
  77     {\r
  78         if(inwater(to))\r
  79         {\r
  80             pathlib_expandnode = pathlib_expandnode_box;\r
  81             pathlib_movenode   = pathlib_walknode;\r
  82         }\r
  83         else\r
  84         {\r
  85             //if(edge_check(parent.origin))\r
  86             //    return 0;\r
  87 \r
  88             pathlib_expandnode = pathlib_expandnode_star;\r
  89             pathlib_movenode   = pathlib_walknode;\r
  90             doedge = 1;\r
  91         }\r
  92     }\r
  93 \r
  94     node = pathlib_nodeatpoint(to);\r
  95     if(node)\r
  96     {\r
  97         ++pathlib_merge_cnt;\r
  98 \r
  99         if(node.owner == openlist)\r
 100         {\r
 101             h = pathlib_heuristic(node.origin,goal);\r
 102             g = pathlib_cost(parent,node.origin,cost);\r
 103             f = g + h;\r
 104 \r
 105             if(node.pathlib_node_g > g)\r
 106             {\r
 107                 node.pathlib_node_h = h;\r
 108                 node.pathlib_node_g = g;\r
 109                 node.pathlib_node_f = f;\r
 110 \r
 111                 node.path_prev = parent;\r
 112             }\r
 113 \r
 114             if not (best_open_node)\r
 115                 best_open_node = node;\r
 116             else if(best_open_node.pathlib_node_f > node.pathlib_node_f)\r
 117                 best_open_node = node;\r
 118         }\r
 119 \r
 120         return 1;\r
 121     }\r
 122 \r
 123     where = pathlib_movenode(parent.origin,to,0);\r
 124     if not(pathlib_movenode_goodnode)\r
 125         return 0;\r
 126 \r
 127     if(pathlib_nodeatpoint(where))\r
 128     {\r
 129         dprint("NAP WHERE :",vtos(where),"\n");\r
 130         dprint("not NAP TO:",vtos(to),"\n");\r
 131         dprint("NAP-NNAP:",ftos(vlen(to-where)),"\n\n");\r
 132         return 0;\r
 133     }\r
 134 \r
 135     if(doedge)\r
 136         if not (tile_check(where))\r
 137             return 0;\r
 138 \r
 139     h = pathlib_heuristic(where,goal);\r
 140     g = pathlib_cost(parent,where,cost);\r
 141     f = g + h;\r
 142 \r
 143 \r
 144     /*\r
 145     node = findradius(where,pathlib_gridsize * 0.5);\r
 146     while(node)\r
 147     {\r
 148         if(node.is_path_node == TRUE)\r
 149         {\r
 150             ++pathlib_merge_cnt;\r
 151             if(node.owner == openlist)\r
 152             {\r
 153                 if(node.pathlib_node_g > g)\r
 154                 {\r
 155                     //pathlib_movenode(where,node.origin,0);\r
 156                     //if(pathlib_movenode_goodnode)\r
 157                     //{\r
 158                         //mark_error(node.origin + '0 0 128',30);\r
 159                         node.pathlib_node_h = h;\r
 160                         node.pathlib_node_g = g;\r
 161                         node.pathlib_node_f = f;\r
 162                         node.path_prev = parent;\r
 163                     //}\r
 164                 }\r
 165 \r
 166                 if not (best_open_node)\r
 167                     best_open_node = node;\r
 168                 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)\r
 169                     best_open_node = node;\r
 170             }\r
 171 \r
 172             return 1;\r
 173         }\r
 174         node = node.chain;\r
 175     }\r
 176     */\r
 177 \r
 178     node = pathlib_mknode(where,parent);\r
 179     node.pathlib_node_h = h;\r
 180     node.pathlib_node_g = g;\r
 181     node.pathlib_node_f = f;\r
 182 \r
 183     if not (best_open_node)\r
 184         best_open_node = node;\r
 185     else if(best_open_node.pathlib_node_f > node.pathlib_node_f)\r
 186         best_open_node = node;\r
 187 \r
 188     return 1;\r
 189 }\r
 190 \r
 191 entity pathlib_getbestopen()\r
 192 {\r
 193     entity node;\r
 194     entity bestnode;\r
 195 \r
 196     if(best_open_node)\r
 197     {\r
 198         ++pathlib_bestcash_hits;\r
 199         pathlib_bestcash_saved += pathlib_open_cnt;\r
 200 \r
 201         return best_open_node;\r
 202     }\r
 203 \r
 204     node = findchainentity(owner,openlist);\r
 205     if(!node)\r
 206         return world;\r
 207 \r
 208     bestnode = node;\r
 209     while(node)\r
 210     {\r
 211         ++pathlib_bestopen_seached;\r
 212         if(node.pathlib_node_f < bestnode.pathlib_node_f)\r
 213             bestnode = node;\r
 214 \r
 215         node = node.chain;\r
 216     }\r
 217 \r
 218     return bestnode;\r
 219 }\r
 220 \r
 221 void pathlib_close_node(entity node,vector goal)\r
 222 {\r
 223 \r
 224     if(node.owner == closedlist)\r
 225     {\r
 226         dprint("Pathlib: Tried to close a closed node!\n");\r
 227         return;\r
 228     }\r
 229 \r
 230     if(node == best_open_node)\r
 231         best_open_node = world;\r
 232 \r
 233     ++pathlib_closed_cnt;\r
 234     --pathlib_open_cnt;\r
 235 \r
 236     node.owner = closedlist;\r
 237 \r
 238     if(vlen(node.origin - goal) <= pathlib_gridsize)\r
 239     {\r
 240         vector goalmove;\r
 241 \r
 242         goalmove = pathlib_walknode(node.origin,goal,1);\r
 243         if(pathlib_movenode_goodnode)\r
 244         {\r
 245             goal_node         = node;\r
 246             pathlib_foundgoal = TRUE;\r
 247         }\r
 248     }\r
 249 }\r
 250 \r
 251 void pathlib_cleanup()\r
 252 {\r
 253     best_open_node = world;\r
 254 \r
 255     //return;\r
 256 \r
 257     entity node;\r
 258 \r
 259     node = findfloat(world,is_path_node, TRUE);\r
 260     while(node)\r
 261     {\r
 262         /*\r
 263         node.owner = openlist;\r
 264         node.pathlib_node_g = 0;\r
 265         node.pathlib_node_h = 0;\r
 266         node.pathlib_node_f = 0;\r
 267         node.path_prev = world;\r
 268         */\r
 269 \r
 270         dumpnode(node);\r
 271         node = findfloat(node,is_path_node, TRUE);\r
 272     }\r
 273 \r
 274     if(openlist)\r
 275         remove(openlist);\r
 276 \r
 277     if(closedlist)\r
 278         remove(closedlist);\r
 279 \r
 280     openlist       = world;\r
 281     closedlist     = world;\r
 282 \r
 283 }\r
 284 \r
 285 float Cosine_Interpolate(float a, float b, float x)\r
 286 {\r
 287     float ft,f;\r
 288 \r
 289         ft = x * 3.1415927;\r
 290         f = (1 - cos(ft)) * 0.5;\r
 291 \r
 292         return  a*(1-f) + b*f;\r
 293 }\r
 294 \r
 295 float buildpath_nodefilter_directional(vector n,vector c,vector p)\r
 296 {\r
 297     vector d1,d2;\r
 298 \r
 299     d2 = normalize(p - c);\r
 300     d1 = normalize(c - n);\r
 301 \r
 302     if(vlen(d1-d2) < 0.25)\r
 303     {\r
 304         //mark_error(c,30);\r
 305         return 1;\r
 306     }\r
 307     //mark_info(c,30);\r
 308     return 0;\r
 309 }\r
 310 \r
 311 float buildpath_nodefilter_moveskip(vector n,vector c,vector p)\r
 312 {\r
 313     pathlib_walknode(p,n,1);\r
 314     if(pathlib_movenode_goodnode)\r
 315         return 1;\r
 316 \r
 317     return 0;\r
 318 }\r
 319 \r
 320 entity path_build(entity next, vector where, entity prev, entity start)\r
 321 {\r
 322     entity path;\r
 323 \r
 324     if(prev && next)\r
 325         if(buildpath_nodefilter)\r
 326             if(buildpath_nodefilter(next.origin,where,prev.origin))\r
 327                 return next;\r
 328 \r
 329 \r
 330     path           = spawn();\r
 331     path.owner     = start;\r
 332     path.path_next = next;\r
 333 \r
 334     setorigin(path,where);\r
 335 \r
 336     if(!next)\r
 337         path.classname = "path_end";\r
 338     else\r
 339     {\r
 340         if(!prev)\r
 341             path.classname = "path_start";\r
 342         else\r
 343             path.classname = "path_node";\r
 344     }\r
 345 \r
 346     return path;\r
 347 }\r
 348 \r
 349 entity pathlib_astar(vector from,vector to)\r
 350 {\r
 351     entity path, start, end, open, n, ln;\r
 352     float ptime, ftime, ctime;\r
 353 \r
 354     ptime = gettime(GETTIME_REALTIME);\r
 355     pathlib_starttime = ptime;\r
 356 \r
 357     pathlib_cleanup();\r
 358 \r
 359     // Select water<->land capable node make/link\r
 360     pathlib_makenode     = pathlib_makenode_adaptive;\r
 361     // Select XYZ cost estimate\r
 362     //pathlib_heuristic    = pathlib_h_diagonal3;\r
 363     pathlib_heuristic    = pathlib_h_diagonal;\r
 364     // Select distance + waterfactor cost\r
 365     pathlib_cost         = pathlib_g_euclidean_water;\r
 366     // Select star expander\r
 367     pathlib_expandnode   = pathlib_expandnode_star;\r
 368     // Select walk simulation movement test\r
 369     pathlib_movenode     = pathlib_walknode;\r
 370     // Filter final nodes by direction\r
 371     buildpath_nodefilter = buildpath_nodefilter_directional;\r
 372     // Filter tiles with cross pattern\r
 373     tile_check = tile_check_cross;\r
 374 \r
 375     // If the start is in water we need diffrent settings\r
 376     if(inwater(from))\r
 377     {\r
 378         // Select volumetric node expaner\r
 379         pathlib_expandnode = pathlib_expandnode_box;\r
 380 \r
 381         // Water movement test\r
 382         pathlib_movenode   = pathlib_swimnode;\r
 383     }\r
 384 \r
 385     if not(openlist)\r
 386         openlist       = spawn();\r
 387 \r
 388     if not(closedlist)\r
 389         closedlist     = spawn();\r
 390 \r
 391     pathlib_closed_cnt       = 0;\r
 392     pathlib_open_cnt         = 0;\r
 393     pathlib_made_cnt         = 0;\r
 394     pathlib_merge_cnt        = 0;\r
 395     pathlib_searched_cnt     = 0;\r
 396     pathlib_bestopen_seached = 0;\r
 397     pathlib_bestcash_hits    = 0;\r
 398     pathlib_bestcash_saved   = 0;\r
 399 \r
 400     pathlib_gridsize       = 128;\r
 401     pathlib_movecost       = pathlib_gridsize;\r
 402     pathlib_movecost_diag  = vlen(('1 1 0' * pathlib_gridsize));\r
 403     pathlib_movecost_waterfactor = 1;\r
 404     pathlib_foundgoal      = 0;\r
 405 \r
 406     movenode_boxmax   = self.maxs * 1.25;\r
 407     movenode_boxmin   = self.mins * 1.25;\r
 408 \r
 409     movenode_stepsize = 32;\r
 410 \r
 411     tile_check_size = 65;\r
 412     tile_check_up   = '0 0 128';\r
 413     tile_check_down = '0 0 128';\r
 414 \r
 415     movenode_stepup   = '0 0 36';\r
 416     movenode_maxdrop  = '0 0 128';\r
 417     movenode_boxup    = '0 0 72';\r
 418 \r
 419     from_x = fsnap(from_x,pathlib_gridsize);\r
 420     from_y = fsnap(from_y,pathlib_gridsize);\r
 421 \r
 422     to_x = fsnap(to_x,pathlib_gridsize);\r
 423     to_y = fsnap(to_y,pathlib_gridsize);\r
 424 \r
 425     dprint("AStar init\n");\r
 426     path = pathlib_mknode(from,world);\r
 427     pathlib_close_node(path,to);\r
 428     if(pathlib_foundgoal)\r
 429     {\r
 430         dprint("AStar: Goal found on first node!\n");\r
 431 \r
 432         open           = spawn();\r
 433         open.owner     = open;\r
 434         open.classname = "path_end";\r
 435         setorigin(open,path.origin);\r
 436 \r
 437         pathlib_cleanup();\r
 438 \r
 439         return open;\r
 440     }\r
 441 \r
 442     if(pathlib_expandnode(path,from,to) <= 0)\r
 443     {\r
 444         dprint("AStar path fail.\n");\r
 445         pathlib_cleanup();\r
 446 \r
 447         return world;\r
 448     }\r
 449 \r
 450     best_open_node = pathlib_getbestopen();\r
 451     n = best_open_node;\r
 452     pathlib_close_node(best_open_node,to);\r
 453     if(inwater(n.origin))\r
 454         pathlib_expandnode_box(n,from,to);\r
 455     else\r
 456         pathlib_expandnode_star(n,from,to);\r
 457 \r
 458     while(pathlib_open_cnt)\r
 459     {\r
 460         if((gettime(GETTIME_REALTIME) - pathlib_starttime) > pathlib_maxtime)\r
 461         {\r
 462             dprint("Path took to long to compute!\n");\r
 463             dprint("Nodes - created: ", ftos(pathlib_made_cnt),"\n");\r
 464             dprint("Nodes -    open: ", ftos(pathlib_open_cnt),"\n");\r
 465             dprint("Nodes -  merged: ", ftos(pathlib_merge_cnt),"\n");\r
 466             dprint("Nodes -  closed: ", ftos(pathlib_closed_cnt),"\n");\r
 467 \r
 468             pathlib_cleanup();\r
 469             return world;\r
 470         }\r
 471 \r
 472         best_open_node = pathlib_getbestopen();\r
 473         n = best_open_node;\r
 474         pathlib_close_node(best_open_node,to);\r
 475 \r
 476         if(inwater(n.origin))\r
 477             pathlib_expandnode_box(n,from,to);\r
 478         else\r
 479             pathlib_expandnode(n,from,to);\r
 480 \r
 481         if(pathlib_foundgoal)\r
 482         {\r
 483             dprint("Target found. Rebuilding and filtering path...\n");\r
 484             ftime = gettime(GETTIME_REALTIME);\r
 485             ptime = ftime - ptime;\r
 486 \r
 487             start = path_build(world,path.origin,world,world);\r
 488             end   = path_build(world,goal_node.origin,world,start);\r
 489             ln    = end;\r
 490 \r
 491             open = goal_node;\r
 492             for(open = goal_node; open.path_prev != path; open = open.path_prev)\r
 493             {\r
 494                 n    = path_build(ln,open.origin,open.path_prev,start);\r
 495                 ln.path_prev = n;\r
 496                 ln = n;\r
 497             }\r
 498             start.path_next = n;\r
 499             n.path_prev = start;\r
 500             ftime = gettime(GETTIME_REALTIME) - ftime;\r
 501 \r
 502             ctime = gettime(GETTIME_REALTIME);\r
 503             pathlib_cleanup();\r
 504             ctime = gettime(GETTIME_REALTIME) - ctime;\r
 505 \r
 506 \r
 507 #ifdef DEBUGPATHING\r
 508             pathlib_showpath2(start);\r
 509 \r
 510             dprint("Time used -      pathfinding: ", ftos(ptime),"\n");\r
 511             dprint("Time used - rebuild & filter: ", ftos(ftime),"\n");\r
 512             dprint("Time used -          cleanup: ", ftos(ctime),"\n");\r
 513             dprint("Time used -            total: ", ftos(ptime + ftime + ctime),"\n");\r
 514             dprint("Time used -         # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)),"\n\n");\r
 515             dprint("Nodes -         created: ", ftos(pathlib_made_cnt),"\n");\r
 516             dprint("Nodes -            open: ", ftos(pathlib_open_cnt),"\n");\r
 517             dprint("Nodes -          merged: ", ftos(pathlib_merge_cnt),"\n");\r
 518             dprint("Nodes -          closed: ", ftos(pathlib_closed_cnt),"\n");\r
 519             dprint("Nodes -        searched: ", ftos(pathlib_searched_cnt),"\n");\r
 520             dprint("Nodes bestopen searched: ", ftos(pathlib_bestopen_seached),"\n");\r
 521             dprint("Nodes bestcash -   hits: ", ftos(pathlib_bestcash_hits),"\n");\r
 522             dprint("Nodes bestcash -   save: ", ftos(pathlib_bestcash_saved),"\n");\r
 523             dprint("AStar done.\n");\r
 524 #endif\r
 525             return start;\r
 526         }\r
 527     }\r
 528 \r
 529     dprint("A* Faild to find a path! Try a smaller gridsize.\n");\r
 530 \r
 531     pathlib_cleanup();\r
 532 \r
 533     return world;\r
 534 }\r