3 #include <common/weapons/_all.qh>
4 #include <common/stats.qh>
7 #include <common/turrets/util.qh>
8 #include "../command/common.qh"
10 void pathlib_deletepath(entity start)
15 FOREACH_ENTITY_ENT(owner, start,
17 setthink(it, SUB_Remove);
22 const float PATHLIB_NODEEXPIRE = 20; // 0.05
24 void dumpnode(entity n)
27 IL_REMOVE(g_pathlib_nodes, n);
28 n.is_path_node = false;
29 setthink(n, SUB_Remove);
38 entity pathlib_mknode(vector where,entity parent)
40 entity node = pathlib_nodeatpoint(where);
44 mark_error(where, 60);
51 setthink(node, SUB_Remove);
52 node.nextthink = time + PATHLIB_NODEEXPIRE;
53 IL_PUSH(g_pathlib_nodes, node);
54 node.is_path_node = true;
55 node.owner = openlist;
56 node.path_prev = parent;
58 setsize(node, '0 0 0', '0 0 0');
60 setorigin(node, where);
62 pathlib_showsquare(where, 1 ,15);
70 bool pathlib_makenode_adaptive(entity parent,vector start, vector to, vector goal,float cost)
75 ++pathlib_searched_cnt;
77 if(inwater(parent.origin))
79 LOG_DEBUG("FromWater");
80 pathlib_expandnode = pathlib_expandnode_box;
81 pathlib_movenode = pathlib_swimnode;
88 pathlib_expandnode = pathlib_expandnode_box;
89 pathlib_movenode = pathlib_walknode;
93 LOG_DEBUG("LandToLoand");
94 //if(edge_check(parent.origin))
97 pathlib_expandnode = pathlib_expandnode_star;
98 pathlib_movenode = pathlib_walknode;
103 entity node = pathlib_nodeatpoint(to);
106 LOG_DEBUG("NodeAtPoint");
109 if(node.owner == openlist)
111 h = pathlib_heuristic(node.origin,goal);
112 float g = pathlib_cost(parent,node.origin,cost);
115 if(node.pathlib_node_g > g)
117 node.pathlib_node_h = h;
118 node.pathlib_node_g = g;
119 node.pathlib_node_f = f;
121 node.path_prev = parent;
125 best_open_node = node;
126 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
127 best_open_node = node;
133 vector where = pathlib_movenode(parent, parent.origin, to, 0);
135 if (!pathlib_movenode_goodnode)
137 //pathlib_showsquare(where, 0 ,30);
138 //pathlib_showsquare(parent.origin, 1 ,30);
139 LOG_DEBUG("pathlib_movenode_goodnode = 0");
143 //pathlib_showsquare(where, 1 ,30);
145 if(pathlib_nodeatpoint(where))
147 LOG_DEBUG("NAP WHERE :",vtos(where));
148 LOG_DEBUG("not NAP TO:",vtos(to));
149 LOG_DEBUG("NAP-NNAP:",ftos(vlen(to-where)));
155 if (!tile_check(parent, where))
157 LOG_DEBUG("tile_check fail");
159 pathlib_showsquare(where, 0 ,30);
165 h = pathlib_heuristic(where,goal);
166 g = pathlib_cost(parent,where,cost);
170 node = findradius(where,pathlib_gridsize * 0.5);
173 if(node.is_path_node == true)
176 if(node.owner == openlist)
178 if(node.pathlib_node_g > g)
180 //pathlib_movenode(node, where,node.origin,0);
181 //if(pathlib_movenode_goodnode)
183 //mark_error(node.origin + '0 0 128',30);
184 node.pathlib_node_h = h;
185 node.pathlib_node_g = g;
186 node.pathlib_node_f = f;
187 node.path_prev = parent;
192 best_open_node = node;
193 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
194 best_open_node = node;
203 node = pathlib_mknode(where, parent);
204 node.pathlib_node_h = h;
205 node.pathlib_node_g = g;
206 node.pathlib_node_f = f;
209 best_open_node = node;
210 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
211 best_open_node = node;
216 entity pathlib_getbestopen()
220 ++pathlib_bestcash_hits;
221 pathlib_bestcash_saved += pathlib_open_cnt;
223 return best_open_node;
226 entity bestnode = NULL;
227 FOREACH_ENTITY_ENT(owner, openlist,
229 ++pathlib_bestopen_searched;
231 if(!bestnode || it.pathlib_node_f < bestnode.pathlib_node_f)
238 void pathlib_close_node(entity node,vector goal)
240 if(node.owner == closedlist)
242 LOG_TRACE("Pathlib: Tried to close a closed node!");
246 if(node == best_open_node)
247 best_open_node = NULL;
249 ++pathlib_closed_cnt;
252 node.owner = closedlist;
254 if(vdist(node.origin - goal, <=, pathlib_gridsize))
258 goalmove = pathlib_walknode(node, node.origin, goal, 1);
259 if(pathlib_movenode_goodnode)
262 pathlib_foundgoal = true;
267 void pathlib_cleanup()
269 best_open_node = NULL;
273 IL_EACH(g_pathlib_nodes, it.is_path_node,
278 IL_CLEAR(g_pathlib_nodes);
290 float Cosine_Interpolate(float a, float b, float c)
293 float f = (1 - cos(ft)) * 0.5;
295 return a*(1-f) + b*f;
298 bool buildpath_nodefilter_directional(vector n,vector c,vector p)
300 vector d2 = normalize(p - c);
301 vector d1 = normalize(c - n);
303 if(vdist(d1 - d2, <, 0.25))
312 bool buildpath_nodefilter_moveskip(entity this, vector n,vector c,vector p)
314 pathlib_walknode(this, p, n, 1);
315 if(pathlib_movenode_goodnode)
321 bool buildpath_nodefilter_none(vector n,vector c,vector p)
326 entity path_build(entity next, vector where, entity prev, entity start)
329 if(buildpath_nodefilter)
330 if(buildpath_nodefilter(next.origin,where,prev.origin))
334 entity path = spawn();
336 path.path_next = next;
338 setorigin(path, where);
341 path.classname = "path_end";
345 path.classname = "path_start";
347 path.classname = "path_node";
353 entity pathlib_astar(entity this, vector from, vector to)
356 float ptime = gettime(GETTIME_REALTIME);
357 pathlib_starttime = ptime;
361 // Select water<->land capable node make/link
362 pathlib_makenode = pathlib_makenode_adaptive;
364 // Select XYZ cost estimate
365 //pathlib_heuristic = pathlib_h_diagonal3;
366 pathlib_heuristic = pathlib_h_diagonal;
368 // Select distance + waterfactor cost
369 pathlib_cost = pathlib_g_euclidean_water;
371 // Select star expander
372 pathlib_expandnode = pathlib_expandnode_star;
374 // Select walk simulation movement test
375 pathlib_movenode = pathlib_walknode;
377 // Filter final nodes by direction
378 buildpath_nodefilter = buildpath_nodefilter_directional;
380 // Filter tiles with cross pattern
381 tile_check = tile_check_cross;
383 // If the start is in water we need diffrent settings
386 // Select volumetric node expaner
387 pathlib_expandnode = pathlib_expandnode_box;
389 // Water movement test
390 pathlib_movenode = pathlib_swimnode;
397 closedlist = spawn();
399 pathlib_closed_cnt = 0;
400 pathlib_open_cnt = 0;
401 pathlib_made_cnt = 0;
402 pathlib_merge_cnt = 0;
403 pathlib_searched_cnt = 0;
404 pathlib_bestopen_searched = 0;
405 pathlib_bestcash_hits = 0;
406 pathlib_bestcash_saved = 0;
408 pathlib_gridsize = 128;
409 pathlib_movecost = pathlib_gridsize;
410 pathlib_movecost_diag = vlen(('1 1 0' * pathlib_gridsize));
411 pathlib_movecost_waterfactor = 25000000;
412 pathlib_foundgoal = 0;
414 movenode_boxmax = this.maxs * 1.1;
415 movenode_boxmin = this.mins * 1.1;
417 movenode_stepsize = pathlib_gridsize * 0.25;
419 tile_check_size = pathlib_gridsize * 0.5;
420 tile_check_up = '0 0 2' * pathlib_gridsize;
421 tile_check_up = '0 0 128';
422 tile_check_down = '0 0 3' * pathlib_gridsize;
423 tile_check_down = '0 0 256';
425 //movenode_stepup = '0 0 128';
426 movenode_stepup = '0 0 36';
427 movenode_maxdrop = '0 0 512';
428 //movenode_maxdrop = '0 0 512';
429 movenode_boxup = '0 0 72';
431 from.x = fsnap(from.x, pathlib_gridsize);
432 from.y = fsnap(from.y, pathlib_gridsize);
435 to.x = fsnap(to.x, pathlib_gridsize);
436 to.y = fsnap(to.y, pathlib_gridsize);
439 LOG_DEBUG("AStar init");
440 entity path = pathlib_mknode(from, NULL);
441 pathlib_close_node(path, to);
442 if(pathlib_foundgoal)
444 LOG_DEBUG("AStar: Goal found on first node!");
446 open = new(path_end);
448 setorigin(open, path.origin);
455 if(pathlib_expandnode(path, from, to) <= 0)
457 LOG_TRACE("AStar path fail.");
463 best_open_node = pathlib_getbestopen();
464 entity n = best_open_node;
465 pathlib_close_node(best_open_node, to);
466 if(inwater(n.origin))
467 pathlib_expandnode_box(n, from, to);
469 pathlib_expandnode_star(n, from, to);
471 while(pathlib_open_cnt)
473 if((gettime(GETTIME_REALTIME) - pathlib_starttime) > pathlib_maxtime)
475 LOG_TRACE("Path took too long to compute!");
476 LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt));
477 LOG_TRACE("Nodes - open: ", ftos(pathlib_open_cnt));
478 LOG_TRACE("Nodes - merged: ", ftos(pathlib_merge_cnt));
479 LOG_TRACE("Nodes - closed: ", ftos(pathlib_closed_cnt));
485 best_open_node = pathlib_getbestopen();
487 pathlib_close_node(best_open_node,to);
489 if(inwater(n.origin))
490 pathlib_expandnode_box(n,from,to);
492 pathlib_expandnode(n,from,to);
494 if(pathlib_foundgoal)
496 LOG_DEBUG("Target found. Rebuilding and filtering path...");
497 float ftime = gettime(GETTIME_REALTIME);
498 ptime = ftime - ptime;
500 entity start = path_build(NULL,path.origin,NULL,NULL);
501 entity end = path_build(NULL,goal_node.origin,NULL,start);
505 for(open = goal_node; open.path_prev != path; open = open.path_prev)
507 n = path_build(ln,open.origin,open.path_prev,start);
513 ftime = gettime(GETTIME_REALTIME) - ftime;
515 float ctime = gettime(GETTIME_REALTIME);
517 ctime = gettime(GETTIME_REALTIME) - ctime;
521 pathlib_showpath2(start);
523 LOG_TRACE("Time used - pathfinding: ", ftos(ptime));
524 LOG_TRACE("Time used - rebuild & filter: ", ftos(ftime));
525 LOG_TRACE("Time used - cleanup: ", ftos(ctime));
526 LOG_TRACE("Time used - total: ", ftos(ptime + ftime + ctime));
527 LOG_TRACE("Time used - # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)));
528 LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt));
529 LOG_TRACE("Nodes - open: ", ftos(pathlib_open_cnt));
530 LOG_TRACE("Nodes - merged: ", ftos(pathlib_merge_cnt));
531 LOG_TRACE("Nodes - closed: ", ftos(pathlib_closed_cnt));
532 LOG_TRACE("Nodes - searched: ", ftos(pathlib_searched_cnt));
533 LOG_TRACE("Nodes bestopen searched: ", ftos(pathlib_bestopen_searched));
534 LOG_TRACE("Nodes bestcash - hits: ", ftos(pathlib_bestcash_hits));
535 LOG_TRACE("Nodes bestcash - save: ", ftos(pathlib_bestcash_saved));
536 LOG_TRACE("AStar done.");
542 LOG_TRACE("A* Faild to find a path! Try a smaller gridsize.");