5 #include "../command/common.qh"
7 void pathlib_deletepath(entity start)
9 FOREACH_ENTITY_ENT(owner, start,
11 setthink(it, SUB_Remove);
16 const float PATHLIB_NODEEXPIRE = 20; // 0.05
18 void dumpnode(entity n)
20 n.is_path_node = false;
21 setthink(n, SUB_Remove);
26 void pathlib_showpath(entity start);
27 void pathlib_showpath2(entity path);
28 void pathlib_showsquare(vector where,float goodsquare,float _lifetime);
32 entity pathlib_mknode(vector where,entity parent)
34 entity node = pathlib_nodeatpoint(where);
38 mark_error(where, 60);
45 setthink(node, SUB_Remove);
46 node.nextthink = time + PATHLIB_NODEEXPIRE;
47 node.is_path_node = true;
48 node.owner = openlist;
49 node.path_prev = parent;
51 setsize(node, '0 0 0', '0 0 0');
53 setorigin(node, where);
55 pathlib_showsquare(where, 1 ,15);
63 bool pathlib_makenode_adaptive(entity parent,vector start, vector to, vector goal,float cost)
68 ++pathlib_searched_cnt;
70 if(inwater(parent.origin))
72 LOG_TRACE("FromWater");
73 pathlib_expandnode = pathlib_expandnode_box;
74 pathlib_movenode = pathlib_swimnode;
81 pathlib_expandnode = pathlib_expandnode_box;
82 pathlib_movenode = pathlib_walknode;
86 LOG_TRACE("LandToLoand");
87 //if(edge_check(parent.origin))
90 pathlib_expandnode = pathlib_expandnode_star;
91 pathlib_movenode = pathlib_walknode;
96 entity node = pathlib_nodeatpoint(to);
99 LOG_TRACE("NodeAtPoint");
102 if(node.owner == openlist)
104 h = pathlib_heuristic(node.origin,goal);
105 float g = pathlib_cost(parent,node.origin,cost);
108 if(node.pathlib_node_g > g)
110 node.pathlib_node_h = h;
111 node.pathlib_node_g = g;
112 node.pathlib_node_f = f;
114 node.path_prev = parent;
118 best_open_node = node;
119 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
120 best_open_node = node;
126 vector where = pathlib_movenode(parent, parent.origin, to, 0);
128 if (!pathlib_movenode_goodnode)
130 //pathlib_showsquare(where, 0 ,30);
131 //pathlib_showsquare(parent.origin, 1 ,30);
132 LOG_TRACE("pathlib_movenode_goodnode = 0");
136 //pathlib_showsquare(where, 1 ,30);
138 if(pathlib_nodeatpoint(where))
140 LOG_TRACE("NAP WHERE :",vtos(where));
141 LOG_TRACE("not NAP TO:",vtos(to));
142 LOG_TRACE("NAP-NNAP:",ftos(vlen(to-where)));
148 if (!tile_check(parent, where))
150 LOG_TRACE("tile_check fail");
152 pathlib_showsquare(where, 0 ,30);
158 h = pathlib_heuristic(where,goal);
159 g = pathlib_cost(parent,where,cost);
163 node = findradius(where,pathlib_gridsize * 0.5);
166 if(node.is_path_node == true)
169 if(node.owner == openlist)
171 if(node.pathlib_node_g > g)
173 //pathlib_movenode(node, where,node.origin,0);
174 //if(pathlib_movenode_goodnode)
176 //mark_error(node.origin + '0 0 128',30);
177 node.pathlib_node_h = h;
178 node.pathlib_node_g = g;
179 node.pathlib_node_f = f;
180 node.path_prev = parent;
185 best_open_node = node;
186 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
187 best_open_node = node;
196 node = pathlib_mknode(where, parent);
197 node.pathlib_node_h = h;
198 node.pathlib_node_g = g;
199 node.pathlib_node_f = f;
202 best_open_node = node;
203 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
204 best_open_node = node;
209 entity pathlib_getbestopen()
213 ++pathlib_bestcash_hits;
214 pathlib_bestcash_saved += pathlib_open_cnt;
216 return best_open_node;
219 entity bestnode = NULL;
220 FOREACH_ENTITY_ENT(owner, openlist,
222 ++pathlib_bestopen_searched;
224 if(!bestnode || it.pathlib_node_f < bestnode.pathlib_node_f)
231 void pathlib_close_node(entity node,vector goal)
233 if(node.owner == closedlist)
235 LOG_TRACE("Pathlib: Tried to close a closed node!");
239 if(node == best_open_node)
240 best_open_node = NULL;
242 ++pathlib_closed_cnt;
245 node.owner = closedlist;
247 if(vdist(node.origin - goal, <=, pathlib_gridsize))
251 goalmove = pathlib_walknode(node, node.origin, goal, 1);
252 if(pathlib_movenode_goodnode)
255 pathlib_foundgoal = true;
260 void pathlib_cleanup()
262 best_open_node = NULL;
266 FOREACH_ENTITY_FLOAT(is_path_node, true,
281 float Cosine_Interpolate(float a, float b, float c)
283 float ft = c * 3.1415927;
284 float f = (1 - cos(ft)) * 0.5;
286 return a*(1-f) + b*f;
289 bool buildpath_nodefilter_directional(vector n,vector c,vector p)
291 vector d2 = normalize(p - c);
292 vector d1 = normalize(c - n);
294 if(vdist(d1 - d2, <, 0.25))
303 bool buildpath_nodefilter_moveskip(entity this, vector n,vector c,vector p)
305 pathlib_walknode(this, p, n, 1);
306 if(pathlib_movenode_goodnode)
312 bool buildpath_nodefilter_none(vector n,vector c,vector p)
317 entity path_build(entity next, vector where, entity prev, entity start)
320 if(buildpath_nodefilter)
321 if(buildpath_nodefilter(next.origin,where,prev.origin))
325 entity path = spawn();
327 path.path_next = next;
329 setorigin(path, where);
332 path.classname = "path_end";
336 path.classname = "path_start";
338 path.classname = "path_node";
344 entity pathlib_astar(entity this, vector from, vector to)
347 float ptime = gettime(GETTIME_REALTIME);
348 pathlib_starttime = ptime;
352 // Select water<->land capable node make/link
353 pathlib_makenode = pathlib_makenode_adaptive;
355 // Select XYZ cost estimate
356 //pathlib_heuristic = pathlib_h_diagonal3;
357 pathlib_heuristic = pathlib_h_diagonal;
359 // Select distance + waterfactor cost
360 pathlib_cost = pathlib_g_euclidean_water;
362 // Select star expander
363 pathlib_expandnode = pathlib_expandnode_star;
365 // Select walk simulation movement test
366 pathlib_movenode = pathlib_walknode;
368 // Filter final nodes by direction
369 buildpath_nodefilter = buildpath_nodefilter_directional;
371 // Filter tiles with cross pattern
372 tile_check = tile_check_cross;
374 // If the start is in water we need diffrent settings
377 // Select volumetric node expaner
378 pathlib_expandnode = pathlib_expandnode_box;
380 // Water movement test
381 pathlib_movenode = pathlib_swimnode;
388 closedlist = spawn();
390 pathlib_closed_cnt = 0;
391 pathlib_open_cnt = 0;
392 pathlib_made_cnt = 0;
393 pathlib_merge_cnt = 0;
394 pathlib_searched_cnt = 0;
395 pathlib_bestopen_searched = 0;
396 pathlib_bestcash_hits = 0;
397 pathlib_bestcash_saved = 0;
399 pathlib_gridsize = 128;
400 pathlib_movecost = pathlib_gridsize;
401 pathlib_movecost_diag = vlen(('1 1 0' * pathlib_gridsize));
402 pathlib_movecost_waterfactor = 25000000;
403 pathlib_foundgoal = 0;
405 movenode_boxmax = this.maxs * 1.1;
406 movenode_boxmin = this.mins * 1.1;
408 movenode_stepsize = pathlib_gridsize * 0.25;
410 tile_check_size = pathlib_gridsize * 0.5;
411 tile_check_up = '0 0 2' * pathlib_gridsize;
412 tile_check_up = '0 0 128';
413 tile_check_down = '0 0 3' * pathlib_gridsize;
414 tile_check_down = '0 0 256';
416 //movenode_stepup = '0 0 128';
417 movenode_stepup = '0 0 36';
418 movenode_maxdrop = '0 0 512';
419 //movenode_maxdrop = '0 0 512';
420 movenode_boxup = '0 0 72';
422 from.x = fsnap(from.x, pathlib_gridsize);
423 from.y = fsnap(from.y, pathlib_gridsize);
426 to.x = fsnap(to.x, pathlib_gridsize);
427 to.y = fsnap(to.y, pathlib_gridsize);
430 LOG_TRACE("AStar init");
431 entity path = pathlib_mknode(from, NULL);
432 pathlib_close_node(path, to);
433 if(pathlib_foundgoal)
435 LOG_TRACE("AStar: Goal found on first node!");
437 open = new(path_end);
439 setorigin(open, path.origin);
446 if(pathlib_expandnode(path, from, to) <= 0)
448 LOG_TRACE("AStar path fail.");
454 best_open_node = pathlib_getbestopen();
455 entity n = best_open_node;
456 pathlib_close_node(best_open_node, to);
457 if(inwater(n.origin))
458 pathlib_expandnode_box(n, from, to);
460 pathlib_expandnode_star(n, from, to);
462 while(pathlib_open_cnt)
464 if((gettime(GETTIME_REALTIME) - pathlib_starttime) > pathlib_maxtime)
466 LOG_TRACE("Path took to long to compute!");
467 LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt));
468 LOG_TRACE("Nodes - open: ", ftos(pathlib_open_cnt));
469 LOG_TRACE("Nodes - merged: ", ftos(pathlib_merge_cnt));
470 LOG_TRACE("Nodes - closed: ", ftos(pathlib_closed_cnt));
476 best_open_node = pathlib_getbestopen();
478 pathlib_close_node(best_open_node,to);
480 if(inwater(n.origin))
481 pathlib_expandnode_box(n,from,to);
483 pathlib_expandnode(n,from,to);
485 if(pathlib_foundgoal)
487 LOG_TRACE("Target found. Rebuilding and filtering path...");
488 float ftime = gettime(GETTIME_REALTIME);
489 ptime = ftime - ptime;
491 entity start = path_build(NULL,path.origin,NULL,NULL);
492 entity end = path_build(NULL,goal_node.origin,NULL,start);
496 for(open = goal_node; open.path_prev != path; open = open.path_prev)
498 n = path_build(ln,open.origin,open.path_prev,start);
504 ftime = gettime(GETTIME_REALTIME) - ftime;
506 float ctime = gettime(GETTIME_REALTIME);
508 ctime = gettime(GETTIME_REALTIME) - ctime;
512 pathlib_showpath2(start);
514 LOG_TRACE("Time used - pathfinding: ", ftos(ptime));
515 LOG_TRACE("Time used - rebuild & filter: ", ftos(ftime));
516 LOG_TRACE("Time used - cleanup: ", ftos(ctime));
517 LOG_TRACE("Time used - total: ", ftos(ptime + ftime + ctime));
518 LOG_TRACE("Time used - # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)));
519 LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt));
520 LOG_TRACE("Nodes - open: ", ftos(pathlib_open_cnt));
521 LOG_TRACE("Nodes - merged: ", ftos(pathlib_merge_cnt));
522 LOG_TRACE("Nodes - closed: ", ftos(pathlib_closed_cnt));
523 LOG_TRACE("Nodes - searched: ", ftos(pathlib_searched_cnt));
524 LOG_TRACE("Nodes bestopen searched: ", ftos(pathlib_bestopen_searched));
525 LOG_TRACE("Nodes bestcash - hits: ", ftos(pathlib_bestcash_hits));
526 LOG_TRACE("Nodes bestcash - save: ", ftos(pathlib_bestcash_saved));
527 LOG_TRACE("AStar done.");
533 LOG_TRACE("A* Faild to find a path! Try a smaller gridsize.");