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 entity path_build(entity next, vector where, entity prev, entity start)
 331 {
 332     entity path;
 333
 334     if(prev && next)
 335         if(buildpath_nodefilter)
 336             if(buildpath_nodefilter(next.origin,where,prev.origin))
 337                 return next;
 338
 339
 340     path           = spawn();
 341     path.owner     = start;
 342     path.path_next = next;
 343
 344     setorigin(path,where);
 345
 346     if(!next)
 347         path.classname = "path_end";
 348     else
 349     {
 350         if(!prev)
 351             path.classname = "path_start";
 352         else
 353             path.classname = "path_node";
 354     }
 355
 356     return path;
 357 }
 358
 359 entity pathlib_astar(vector from,vector to)
 360 {
 361     entity path, start, end, open, n, ln;
 362     float ptime, ftime, ctime;
 363
 364     ptime = gettime(GETTIME_REALTIME);
 365     pathlib_starttime = ptime;
 366
 367     pathlib_cleanup();
 368
 369     // Select water<->land capable node make/link
 370     pathlib_makenode     = pathlib_makenode_adaptive;
 371
 372     // Select XYZ cost estimate
 373     //pathlib_heuristic    = pathlib_h_diagonal3;
 374     pathlib_heuristic    = pathlib_h_diagonal;
 375
 376     // Select distance + waterfactor cost
 377     pathlib_cost         = pathlib_g_euclidean_water;
 378
 379     // Select star expander
 380     pathlib_expandnode   = pathlib_expandnode_star;
 381
 382     // Select walk simulation movement test
 383     pathlib_movenode     = pathlib_walknode;
 384
 385     // Filter final nodes by direction
 386     buildpath_nodefilter = buildpath_nodefilter_directional;
 387
 388     // Filter tiles with cross pattern
 389     tile_check = tile_check_cross;
 390
 391     // If the start is in water we need diffrent settings
 392     if(inwater(from))
 393     {
 394         // Select volumetric node expaner
 395         pathlib_expandnode = pathlib_expandnode_box;
 396
 397         // Water movement test
 398         pathlib_movenode   = pathlib_swimnode;
 399     }
 400
 401     if not(openlist)
 402         openlist       = spawn();
 403
 404     if not(closedlist)
 405         closedlist     = spawn();
 406
 407     pathlib_closed_cnt       = 0;
 408     pathlib_open_cnt         = 0;
 409     pathlib_made_cnt         = 0;
 410     pathlib_merge_cnt        = 0;
 411     pathlib_searched_cnt     = 0;
 412     pathlib_bestopen_seached = 0;
 413     pathlib_bestcash_hits    = 0;
 414     pathlib_bestcash_saved   = 0;
 415
 416     pathlib_gridsize       = 128;
 417     pathlib_movecost       = pathlib_gridsize;
 418     pathlib_movecost_diag  = vlen(('1 1 0' * pathlib_gridsize));
 419     pathlib_movecost_waterfactor = 25000000;
 420     pathlib_foundgoal      = 0;
 421
 422     movenode_boxmax   = self.maxs * 1.1;
 423     movenode_boxmin   = self.mins * 1.1;
 424
 425     movenode_stepsize = pathlib_gridsize * 0.25;
 426
 427     tile_check_size = pathlib_gridsize * 0.5;
 428     tile_check_up   = '0 0 2' * pathlib_gridsize;
 429     tile_check_up   = '0 0 128';
 430     tile_check_down = '0 0 3' * pathlib_gridsize;
 431     tile_check_down = '0 0 256';
 432
 433     //movenode_stepup   = '0 0 128';
 434     movenode_stepup   = '0 0 36';
 435     movenode_maxdrop  = '0 0 512';
 436     //movenode_maxdrop  = '0 0 512';
 437     movenode_boxup    = '0 0 72';
 438
 439     from_x = fsnap(from_x, pathlib_gridsize);
 440     from_y = fsnap(from_y, pathlib_gridsize);
 441     //from_z += 32;
 442
 443     to_x = fsnap(to_x, pathlib_gridsize);
 444     to_y = fsnap(to_y, pathlib_gridsize);
 445     //to_z += 32;
 446
 447     dprint("AStar init\n");
 448     path = pathlib_mknode(from, world);
 449     pathlib_close_node(path, to);
 450     if(pathlib_foundgoal)
 451     {
 452         dprint("AStar: Goal found on first node!\n");
 453
 454         open           = spawn();
 455         open.owner     = open;
 456         open.classname = "path_end";
 457         setorigin(open,path.origin);
 458
 459         pathlib_cleanup();
 460
 461         return open;
 462     }
 463
 464     if(pathlib_expandnode(path, from, to) <= 0)
 465     {
 466         dprint("AStar path fail.\n");
 467         pathlib_cleanup();
 468
 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     if(inwater(n.origin))
 476         pathlib_expandnode_box(n, from, to);
 477     else
 478         pathlib_expandnode_star(n, from, to);
 479
 480     while(pathlib_open_cnt)
 481     {
 482         if((gettime(GETTIME_REALTIME) - pathlib_starttime) > pathlib_maxtime)
 483         {
 484             dprint("Path took to long to compute!\n");
 485             dprint("Nodes - created: ", ftos(pathlib_made_cnt),"\n");
 486             dprint("Nodes -    open: ", ftos(pathlib_open_cnt),"\n");
 487             dprint("Nodes -  merged: ", ftos(pathlib_merge_cnt),"\n");
 488             dprint("Nodes -  closed: ", ftos(pathlib_closed_cnt),"\n");
 489
 490             pathlib_cleanup();
 491             return world;
 492         }
 493
 494         best_open_node = pathlib_getbestopen();
 495         n = best_open_node;
 496         pathlib_close_node(best_open_node,to);
 497
 498         if(inwater(n.origin))
 499             pathlib_expandnode_box(n,from,to);
 500         else
 501             pathlib_expandnode(n,from,to);
 502
 503         if(pathlib_foundgoal)
 504         {
 505             dprint("Target found. Rebuilding and filtering path...\n");
 506             ftime = gettime(GETTIME_REALTIME);
 507             ptime = ftime - ptime;
 508
 509             start = path_build(world,path.origin,world,world);
 510             end   = path_build(world,goal_node.origin,world,start);
 511             ln    = end;
 512
 513             open = goal_node;
 514             for(open = goal_node; open.path_prev != path; open = open.path_prev)
 515             {
 516                 n    = path_build(ln,open.origin,open.path_prev,start);
 517                 ln.path_prev = n;
 518                 ln = n;
 519             }
 520             start.path_next = n;
 521             n.path_prev = start;
 522             ftime = gettime(GETTIME_REALTIME) - ftime;
 523
 524             ctime = gettime(GETTIME_REALTIME);
 525             pathlib_cleanup();
 526             ctime = gettime(GETTIME_REALTIME) - ctime;
 527
 528
 529 #ifdef DEBUGPATHING
 530             pathlib_showpath2(start);
 531
 532             dprint("Time used -      pathfinding: ", ftos(ptime),"\n");
 533             dprint("Time used - rebuild & filter: ", ftos(ftime),"\n");
 534             dprint("Time used -          cleanup: ", ftos(ctime),"\n");
 535             dprint("Time used -            total: ", ftos(ptime + ftime + ctime),"\n");
 536             dprint("Time used -         # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)),"\n\n");
 537             dprint("Nodes -         created: ", ftos(pathlib_made_cnt),"\n");
 538             dprint("Nodes -            open: ", ftos(pathlib_open_cnt),"\n");
 539             dprint("Nodes -          merged: ", ftos(pathlib_merge_cnt),"\n");
 540             dprint("Nodes -          closed: ", ftos(pathlib_closed_cnt),"\n");
 541             dprint("Nodes -        searched: ", ftos(pathlib_searched_cnt),"\n");
 542             dprint("Nodes bestopen searched: ", ftos(pathlib_bestopen_seached),"\n");
 543             dprint("Nodes bestcash -   hits: ", ftos(pathlib_bestcash_hits),"\n");
 544             dprint("Nodes bestcash -   save: ", ftos(pathlib_bestcash_saved),"\n");
 545             dprint("AStar done.\n");
 546 #endif
 547             return start;
 548         }
 549     }
 550
 551     dprint("A* Faild to find a path! Try a smaller gridsize.\n");
 552
 553     pathlib_cleanup();
 554
 555     return world;
 556 }