data/qcsrc/server/pathlib/main.qc

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