5 #include "../command/common.qh"
7 void pathlib_deletepath(entity start)
12 FOREACH_ENTITY_ENT(owner, start,
14 setthink(it, SUB_Remove);
19 const float PATHLIB_NODEEXPIRE = 20; // 0.05
21 void dumpnode(entity n)
24 IL_REMOVE(g_pathlib_nodes, n);
25 n.is_path_node = false;
26 setthink(n, SUB_Remove);
31 void pathlib_showpath(entity start);
32 void pathlib_showpath2(entity path);
33 void pathlib_showsquare(vector where,float goodsquare,float _lifetime);
37 entity pathlib_mknode(vector where,entity parent)
39 entity node = pathlib_nodeatpoint(where);
43 mark_error(where, 60);
50 setthink(node, SUB_Remove);
51 node.nextthink = time + PATHLIB_NODEEXPIRE;
52 IL_PUSH(g_pathlib_nodes, node);
53 node.is_path_node = true;
54 node.owner = openlist;
55 node.path_prev = parent;
57 setsize(node, '0 0 0', '0 0 0');
59 setorigin(node, where);
61 pathlib_showsquare(where, 1 ,15);
69 bool pathlib_makenode_adaptive(entity parent,vector start, vector to, vector goal,float cost)
74 ++pathlib_searched_cnt;
76 if(inwater(parent.origin))
78 LOG_DEBUG("FromWater");
79 pathlib_expandnode = pathlib_expandnode_box;
80 pathlib_movenode = pathlib_swimnode;
87 pathlib_expandnode = pathlib_expandnode_box;
88 pathlib_movenode = pathlib_walknode;
92 LOG_DEBUG("LandToLoand");
93 //if(edge_check(parent.origin))
96 pathlib_expandnode = pathlib_expandnode_star;
97 pathlib_movenode = pathlib_walknode;
102 entity node = pathlib_nodeatpoint(to);
105 LOG_DEBUG("NodeAtPoint");
108 if(node.owner == openlist)
110 h = pathlib_heuristic(node.origin,goal);
111 float g = pathlib_cost(parent,node.origin,cost);
114 if(node.pathlib_node_g > g)
116 node.pathlib_node_h = h;
117 node.pathlib_node_g = g;
118 node.pathlib_node_f = f;
120 node.path_prev = parent;
124 best_open_node = node;
125 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
126 best_open_node = node;
132 vector where = pathlib_movenode(parent, parent.origin, to, 0);
134 if (!pathlib_movenode_goodnode)
136 //pathlib_showsquare(where, 0 ,30);
137 //pathlib_showsquare(parent.origin, 1 ,30);
138 LOG_DEBUG("pathlib_movenode_goodnode = 0");
142 //pathlib_showsquare(where, 1 ,30);
144 if(pathlib_nodeatpoint(where))
146 LOG_DEBUG("NAP WHERE :",vtos(where));
147 LOG_DEBUG("not NAP TO:",vtos(to));
148 LOG_DEBUG("NAP-NNAP:",ftos(vlen(to-where)));
154 if (!tile_check(parent, where))
156 LOG_DEBUG("tile_check fail");
158 pathlib_showsquare(where, 0 ,30);
164 h = pathlib_heuristic(where,goal);
165 g = pathlib_cost(parent,where,cost);
169 node = findradius(where,pathlib_gridsize * 0.5);
172 if(node.is_path_node == true)
175 if(node.owner == openlist)
177 if(node.pathlib_node_g > g)
179 //pathlib_movenode(node, where,node.origin,0);
180 //if(pathlib_movenode_goodnode)
182 //mark_error(node.origin + '0 0 128',30);
183 node.pathlib_node_h = h;
184 node.pathlib_node_g = g;
185 node.pathlib_node_f = f;
186 node.path_prev = parent;
191 best_open_node = node;
192 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
193 best_open_node = node;
202 node = pathlib_mknode(where, parent);
203 node.pathlib_node_h = h;
204 node.pathlib_node_g = g;
205 node.pathlib_node_f = f;
208 best_open_node = node;
209 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
210 best_open_node = node;
215 entity pathlib_getbestopen()
219 ++pathlib_bestcash_hits;
220 pathlib_bestcash_saved += pathlib_open_cnt;
222 return best_open_node;
225 entity bestnode = NULL;
226 FOREACH_ENTITY_ENT(owner, openlist,
228 ++pathlib_bestopen_searched;
230 if(!bestnode || it.pathlib_node_f < bestnode.pathlib_node_f)
237 void pathlib_close_node(entity node,vector goal)
239 if(node.owner == closedlist)
241 LOG_TRACE("Pathlib: Tried to close a closed node!");
245 if(node == best_open_node)
246 best_open_node = NULL;
248 ++pathlib_closed_cnt;
251 node.owner = closedlist;
253 if(vdist(node.origin - goal, <=, pathlib_gridsize))
257 goalmove = pathlib_walknode(node, node.origin, goal, 1);
258 if(pathlib_movenode_goodnode)
261 pathlib_foundgoal = true;
266 void pathlib_cleanup()
268 best_open_node = NULL;
272 IL_EACH(g_pathlib_nodes, it.is_path_node,
277 IL_CLEAR(g_pathlib_nodes);
289 float Cosine_Interpolate(float a, float b, float c)
291 float ft = c * 3.1415927;
292 float f = (1 - cos(ft)) * 0.5;
294 return a*(1-f) + b*f;
297 bool buildpath_nodefilter_directional(vector n,vector c,vector p)
299 vector d2 = normalize(p - c);
300 vector d1 = normalize(c - n);
302 if(vdist(d1 - d2, <, 0.25))
311 bool buildpath_nodefilter_moveskip(entity this, vector n,vector c,vector p)
313 pathlib_walknode(this, p, n, 1);
314 if(pathlib_movenode_goodnode)
320 bool buildpath_nodefilter_none(vector n,vector c,vector p)
325 entity path_build(entity next, vector where, entity prev, entity start)
328 if(buildpath_nodefilter)
329 if(buildpath_nodefilter(next.origin,where,prev.origin))
333 entity path = spawn();
335 path.path_next = next;
337 setorigin(path, where);
340 path.classname = "path_end";
344 path.classname = "path_start";
346 path.classname = "path_node";
352 entity pathlib_astar(entity this, vector from, vector to)
355 float ptime = gettime(GETTIME_REALTIME);
356 pathlib_starttime = ptime;
360 // Select water<->land capable node make/link
361 pathlib_makenode = pathlib_makenode_adaptive;
363 // Select XYZ cost estimate
364 //pathlib_heuristic = pathlib_h_diagonal3;
365 pathlib_heuristic = pathlib_h_diagonal;
367 // Select distance + waterfactor cost
368 pathlib_cost = pathlib_g_euclidean_water;
370 // Select star expander
371 pathlib_expandnode = pathlib_expandnode_star;
373 // Select walk simulation movement test
374 pathlib_movenode = pathlib_walknode;
376 // Filter final nodes by direction
377 buildpath_nodefilter = buildpath_nodefilter_directional;
379 // Filter tiles with cross pattern
380 tile_check = tile_check_cross;
382 // If the start is in water we need diffrent settings
385 // Select volumetric node expaner
386 pathlib_expandnode = pathlib_expandnode_box;
388 // Water movement test
389 pathlib_movenode = pathlib_swimnode;
396 closedlist = spawn();
398 pathlib_closed_cnt = 0;
399 pathlib_open_cnt = 0;
400 pathlib_made_cnt = 0;
401 pathlib_merge_cnt = 0;
402 pathlib_searched_cnt = 0;
403 pathlib_bestopen_searched = 0;
404 pathlib_bestcash_hits = 0;
405 pathlib_bestcash_saved = 0;
407 pathlib_gridsize = 128;
408 pathlib_movecost = pathlib_gridsize;
409 pathlib_movecost_diag = vlen(('1 1 0' * pathlib_gridsize));
410 pathlib_movecost_waterfactor = 25000000;
411 pathlib_foundgoal = 0;
413 movenode_boxmax = this.maxs * 1.1;
414 movenode_boxmin = this.mins * 1.1;
416 movenode_stepsize = pathlib_gridsize * 0.25;
418 tile_check_size = pathlib_gridsize * 0.5;
419 tile_check_up = '0 0 2' * pathlib_gridsize;
420 tile_check_up = '0 0 128';
421 tile_check_down = '0 0 3' * pathlib_gridsize;
422 tile_check_down = '0 0 256';
424 //movenode_stepup = '0 0 128';
425 movenode_stepup = '0 0 36';
426 movenode_maxdrop = '0 0 512';
427 //movenode_maxdrop = '0 0 512';
428 movenode_boxup = '0 0 72';
430 from.x = fsnap(from.x, pathlib_gridsize);
431 from.y = fsnap(from.y, pathlib_gridsize);
434 to.x = fsnap(to.x, pathlib_gridsize);
435 to.y = fsnap(to.y, pathlib_gridsize);
438 LOG_DEBUG("AStar init");
439 entity path = pathlib_mknode(from, NULL);
440 pathlib_close_node(path, to);
441 if(pathlib_foundgoal)
443 LOG_DEBUG("AStar: Goal found on first node!");
445 open = new(path_end);
447 setorigin(open, path.origin);
454 if(pathlib_expandnode(path, from, to) <= 0)
456 LOG_TRACE("AStar path fail.");
462 best_open_node = pathlib_getbestopen();
463 entity n = best_open_node;
464 pathlib_close_node(best_open_node, to);
465 if(inwater(n.origin))
466 pathlib_expandnode_box(n, from, to);
468 pathlib_expandnode_star(n, from, to);
470 while(pathlib_open_cnt)
472 if((gettime(GETTIME_REALTIME) - pathlib_starttime) > pathlib_maxtime)
474 LOG_TRACE("Path took too long to compute!");
475 LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt));
476 LOG_TRACE("Nodes - open: ", ftos(pathlib_open_cnt));
477 LOG_TRACE("Nodes - merged: ", ftos(pathlib_merge_cnt));
478 LOG_TRACE("Nodes - closed: ", ftos(pathlib_closed_cnt));
484 best_open_node = pathlib_getbestopen();
486 pathlib_close_node(best_open_node,to);
488 if(inwater(n.origin))
489 pathlib_expandnode_box(n,from,to);
491 pathlib_expandnode(n,from,to);
493 if(pathlib_foundgoal)
495 LOG_DEBUG("Target found. Rebuilding and filtering path...");
496 float ftime = gettime(GETTIME_REALTIME);
497 ptime = ftime - ptime;
499 entity start = path_build(NULL,path.origin,NULL,NULL);
500 entity end = path_build(NULL,goal_node.origin,NULL,start);
504 for(open = goal_node; open.path_prev != path; open = open.path_prev)
506 n = path_build(ln,open.origin,open.path_prev,start);
512 ftime = gettime(GETTIME_REALTIME) - ftime;
514 float ctime = gettime(GETTIME_REALTIME);
516 ctime = gettime(GETTIME_REALTIME) - ctime;
520 pathlib_showpath2(start);
522 LOG_TRACE("Time used - pathfinding: ", ftos(ptime));
523 LOG_TRACE("Time used - rebuild & filter: ", ftos(ftime));
524 LOG_TRACE("Time used - cleanup: ", ftos(ctime));
525 LOG_TRACE("Time used - total: ", ftos(ptime + ftime + ctime));
526 LOG_TRACE("Time used - # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)));
527 LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt));
528 LOG_TRACE("Nodes - open: ", ftos(pathlib_open_cnt));
529 LOG_TRACE("Nodes - merged: ", ftos(pathlib_merge_cnt));
530 LOG_TRACE("Nodes - closed: ", ftos(pathlib_closed_cnt));
531 LOG_TRACE("Nodes - searched: ", ftos(pathlib_searched_cnt));
532 LOG_TRACE("Nodes bestopen searched: ", ftos(pathlib_bestopen_searched));
533 LOG_TRACE("Nodes bestcash - hits: ", ftos(pathlib_bestcash_hits));
534 LOG_TRACE("Nodes bestcash - save: ", ftos(pathlib_bestcash_saved));
535 LOG_TRACE("AStar done.");
541 LOG_TRACE("A* Faild to find a path! Try a smaller gridsize.");