qcsrc/server/pathlib/main.qc

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