qcsrc/server/pathlib/main.qc

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