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