3 #include <server/defs.qh>
4 #include <server/miscfunctions.qh>
7 #include "../command/common.qh"
9 void pathlib_deletepath(entity start)
14 FOREACH_ENTITY_ENT(owner, start,
16 setthink(it, SUB_Remove);
21 const float PATHLIB_NODEEXPIRE = 20; // 0.05
23 void dumpnode(entity n)
26 IL_REMOVE(g_pathlib_nodes, n);
27 n.is_path_node = false;
28 setthink(n, SUB_Remove);
33 void pathlib_showpath(entity start);
34 void pathlib_showpath2(entity path);
35 void pathlib_showsquare(vector where,float goodsquare,float _lifetime);
39 entity pathlib_mknode(vector where,entity parent)
41 entity node = pathlib_nodeatpoint(where);
45 mark_error(where, 60);
52 setthink(node, SUB_Remove);
53 node.nextthink = time + PATHLIB_NODEEXPIRE;
54 IL_PUSH(g_pathlib_nodes, node);
55 node.is_path_node = true;
56 node.owner = openlist;
57 node.path_prev = parent;
59 setsize(node, '0 0 0', '0 0 0');
61 setorigin(node, where);
63 pathlib_showsquare(where, 1 ,15);
71 bool pathlib_makenode_adaptive(entity parent,vector start, vector to, vector goal,float cost)
76 ++pathlib_searched_cnt;
78 if(inwater(parent.origin))
80 LOG_DEBUG("FromWater");
81 pathlib_expandnode = pathlib_expandnode_box;
82 pathlib_movenode = pathlib_swimnode;
89 pathlib_expandnode = pathlib_expandnode_box;
90 pathlib_movenode = pathlib_walknode;
94 LOG_DEBUG("LandToLoand");
95 //if(edge_check(parent.origin))
98 pathlib_expandnode = pathlib_expandnode_star;
99 pathlib_movenode = pathlib_walknode;
104 entity node = pathlib_nodeatpoint(to);
107 LOG_DEBUG("NodeAtPoint");
110 if(node.owner == openlist)
112 h = pathlib_heuristic(node.origin,goal);
113 float g = pathlib_cost(parent,node.origin,cost);
116 if(node.pathlib_node_g > g)
118 node.pathlib_node_h = h;
119 node.pathlib_node_g = g;
120 node.pathlib_node_f = f;
122 node.path_prev = parent;
126 best_open_node = node;
127 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
128 best_open_node = node;
134 vector where = pathlib_movenode(parent, parent.origin, to, 0);
136 if (!pathlib_movenode_goodnode)
138 //pathlib_showsquare(where, 0 ,30);
139 //pathlib_showsquare(parent.origin, 1 ,30);
140 LOG_DEBUG("pathlib_movenode_goodnode = 0");
144 //pathlib_showsquare(where, 1 ,30);
146 if(pathlib_nodeatpoint(where))
148 LOG_DEBUG("NAP WHERE :",vtos(where));
149 LOG_DEBUG("not NAP TO:",vtos(to));
150 LOG_DEBUG("NAP-NNAP:",ftos(vlen(to-where)));
156 if (!tile_check(parent, where))
158 LOG_DEBUG("tile_check fail");
160 pathlib_showsquare(where, 0 ,30);
166 h = pathlib_heuristic(where,goal);
167 g = pathlib_cost(parent,where,cost);
171 node = findradius(where,pathlib_gridsize * 0.5);
174 if(node.is_path_node == true)
177 if(node.owner == openlist)
179 if(node.pathlib_node_g > g)
181 //pathlib_movenode(node, where,node.origin,0);
182 //if(pathlib_movenode_goodnode)
184 //mark_error(node.origin + '0 0 128',30);
185 node.pathlib_node_h = h;
186 node.pathlib_node_g = g;
187 node.pathlib_node_f = f;
188 node.path_prev = parent;
193 best_open_node = node;
194 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
195 best_open_node = node;
204 node = pathlib_mknode(where, parent);
205 node.pathlib_node_h = h;
206 node.pathlib_node_g = g;
207 node.pathlib_node_f = f;
210 best_open_node = node;
211 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
212 best_open_node = node;
217 entity pathlib_getbestopen()
221 ++pathlib_bestcash_hits;
222 pathlib_bestcash_saved += pathlib_open_cnt;
224 return best_open_node;
227 entity bestnode = NULL;
228 FOREACH_ENTITY_ENT(owner, openlist,
230 ++pathlib_bestopen_searched;
232 if(!bestnode || it.pathlib_node_f < bestnode.pathlib_node_f)
239 void pathlib_close_node(entity node,vector goal)
241 if(node.owner == closedlist)
243 LOG_TRACE("Pathlib: Tried to close a closed node!");
247 if(node == best_open_node)
248 best_open_node = NULL;
250 ++pathlib_closed_cnt;
253 node.owner = closedlist;
255 if(vdist(node.origin - goal, <=, pathlib_gridsize))
259 goalmove = pathlib_walknode(node, node.origin, goal, 1);
260 if(pathlib_movenode_goodnode)
263 pathlib_foundgoal = true;
268 void pathlib_cleanup()
270 best_open_node = NULL;
274 IL_EACH(g_pathlib_nodes, it.is_path_node,
279 IL_CLEAR(g_pathlib_nodes);
291 float Cosine_Interpolate(float a, float b, float c)
293 float ft = c * 3.1415927;
294 float f = (1 - cos(ft)) * 0.5;
296 return a*(1-f) + b*f;
299 bool buildpath_nodefilter_directional(vector n,vector c,vector p)
301 vector d2 = normalize(p - c);
302 vector d1 = normalize(c - n);
304 if(vdist(d1 - d2, <, 0.25))
313 bool buildpath_nodefilter_moveskip(entity this, vector n,vector c,vector p)
315 pathlib_walknode(this, p, n, 1);
316 if(pathlib_movenode_goodnode)
322 bool buildpath_nodefilter_none(vector n,vector c,vector p)
327 entity path_build(entity next, vector where, entity prev, entity start)
330 if(buildpath_nodefilter)
331 if(buildpath_nodefilter(next.origin,where,prev.origin))
335 entity path = spawn();
337 path.path_next = next;
339 setorigin(path, where);
342 path.classname = "path_end";
346 path.classname = "path_start";
348 path.classname = "path_node";
354 entity pathlib_astar(entity this, vector from, vector to)
357 float ptime = gettime(GETTIME_REALTIME);
358 pathlib_starttime = ptime;
362 // Select water<->land capable node make/link
363 pathlib_makenode = pathlib_makenode_adaptive;
365 // Select XYZ cost estimate
366 //pathlib_heuristic = pathlib_h_diagonal3;
367 pathlib_heuristic = pathlib_h_diagonal;
369 // Select distance + waterfactor cost
370 pathlib_cost = pathlib_g_euclidean_water;
372 // Select star expander
373 pathlib_expandnode = pathlib_expandnode_star;
375 // Select walk simulation movement test
376 pathlib_movenode = pathlib_walknode;
378 // Filter final nodes by direction
379 buildpath_nodefilter = buildpath_nodefilter_directional;
381 // Filter tiles with cross pattern
382 tile_check = tile_check_cross;
384 // If the start is in water we need diffrent settings
387 // Select volumetric node expaner
388 pathlib_expandnode = pathlib_expandnode_box;
390 // Water movement test
391 pathlib_movenode = pathlib_swimnode;
398 closedlist = spawn();
400 pathlib_closed_cnt = 0;
401 pathlib_open_cnt = 0;
402 pathlib_made_cnt = 0;
403 pathlib_merge_cnt = 0;
404 pathlib_searched_cnt = 0;
405 pathlib_bestopen_searched = 0;
406 pathlib_bestcash_hits = 0;
407 pathlib_bestcash_saved = 0;
409 pathlib_gridsize = 128;
410 pathlib_movecost = pathlib_gridsize;
411 pathlib_movecost_diag = vlen(('1 1 0' * pathlib_gridsize));
412 pathlib_movecost_waterfactor = 25000000;
413 pathlib_foundgoal = 0;
415 movenode_boxmax = this.maxs * 1.1;
416 movenode_boxmin = this.mins * 1.1;
418 movenode_stepsize = pathlib_gridsize * 0.25;
420 tile_check_size = pathlib_gridsize * 0.5;
421 tile_check_up = '0 0 2' * pathlib_gridsize;
422 tile_check_up = '0 0 128';
423 tile_check_down = '0 0 3' * pathlib_gridsize;
424 tile_check_down = '0 0 256';
426 //movenode_stepup = '0 0 128';
427 movenode_stepup = '0 0 36';
428 movenode_maxdrop = '0 0 512';
429 //movenode_maxdrop = '0 0 512';
430 movenode_boxup = '0 0 72';
432 from.x = fsnap(from.x, pathlib_gridsize);
433 from.y = fsnap(from.y, pathlib_gridsize);
436 to.x = fsnap(to.x, pathlib_gridsize);
437 to.y = fsnap(to.y, pathlib_gridsize);
440 LOG_DEBUG("AStar init");
441 entity path = pathlib_mknode(from, NULL);
442 pathlib_close_node(path, to);
443 if(pathlib_foundgoal)
445 LOG_DEBUG("AStar: Goal found on first node!");
447 open = new(path_end);
449 setorigin(open, path.origin);
456 if(pathlib_expandnode(path, from, to) <= 0)
458 LOG_TRACE("AStar path fail.");
464 best_open_node = pathlib_getbestopen();
465 entity n = best_open_node;
466 pathlib_close_node(best_open_node, to);
467 if(inwater(n.origin))
468 pathlib_expandnode_box(n, from, to);
470 pathlib_expandnode_star(n, from, to);
472 while(pathlib_open_cnt)
474 if((gettime(GETTIME_REALTIME) - pathlib_starttime) > pathlib_maxtime)
476 LOG_TRACE("Path took too long to compute!");
477 LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt));
478 LOG_TRACE("Nodes - open: ", ftos(pathlib_open_cnt));
479 LOG_TRACE("Nodes - merged: ", ftos(pathlib_merge_cnt));
480 LOG_TRACE("Nodes - closed: ", ftos(pathlib_closed_cnt));
486 best_open_node = pathlib_getbestopen();
488 pathlib_close_node(best_open_node,to);
490 if(inwater(n.origin))
491 pathlib_expandnode_box(n,from,to);
493 pathlib_expandnode(n,from,to);
495 if(pathlib_foundgoal)
497 LOG_DEBUG("Target found. Rebuilding and filtering path...");
498 float ftime = gettime(GETTIME_REALTIME);
499 ptime = ftime - ptime;
501 entity start = path_build(NULL,path.origin,NULL,NULL);
502 entity end = path_build(NULL,goal_node.origin,NULL,start);
506 for(open = goal_node; open.path_prev != path; open = open.path_prev)
508 n = path_build(ln,open.origin,open.path_prev,start);
514 ftime = gettime(GETTIME_REALTIME) - ftime;
516 float ctime = gettime(GETTIME_REALTIME);
518 ctime = gettime(GETTIME_REALTIME) - ctime;
522 pathlib_showpath2(start);
524 LOG_TRACE("Time used - pathfinding: ", ftos(ptime));
525 LOG_TRACE("Time used - rebuild & filter: ", ftos(ftime));
526 LOG_TRACE("Time used - cleanup: ", ftos(ctime));
527 LOG_TRACE("Time used - total: ", ftos(ptime + ftime + ctime));
528 LOG_TRACE("Time used - # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)));
529 LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt));
530 LOG_TRACE("Nodes - open: ", ftos(pathlib_open_cnt));
531 LOG_TRACE("Nodes - merged: ", ftos(pathlib_merge_cnt));
532 LOG_TRACE("Nodes - closed: ", ftos(pathlib_closed_cnt));
533 LOG_TRACE("Nodes - searched: ", ftos(pathlib_searched_cnt));
534 LOG_TRACE("Nodes bestopen searched: ", ftos(pathlib_bestopen_searched));
535 LOG_TRACE("Nodes bestcash - hits: ", ftos(pathlib_bestcash_hits));
536 LOG_TRACE("Nodes bestcash - save: ", ftos(pathlib_bestcash_saved));
537 LOG_TRACE("AStar done.");
543 LOG_TRACE("A* Faild to find a path! Try a smaller gridsize.");