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