5 #include "../command/common.qh"
7 void pathlib_deletepath(entity start)
9 FOREACH_ENTITY_ENT(owner, start,
11 setthink(it, SUB_Remove);
16 //#define PATHLIB_NODEEXPIRE 0.05
17 const float PATHLIB_NODEEXPIRE = 20;
19 void dumpnode(entity n)
21 n.is_path_node = false;
22 setthink(n, SUB_Remove);
27 void pathlib_showpath(entity start);
28 void pathlib_showpath2(entity path);
29 void pathlib_showsquare(vector where,float goodsquare,float _lifetime);
33 entity pathlib_mknode(vector where,entity parent)
37 node = pathlib_nodeatpoint(where);
41 mark_error(where, 60);
48 setthink(node, SUB_Remove);
49 node.nextthink = time + PATHLIB_NODEEXPIRE;
50 node.is_path_node = true;
51 node.owner = openlist;
52 node.path_prev = parent;
55 setsize(node, '0 0 0', '0 0 0');
57 setorigin(node, where);
59 pathlib_showsquare(where, 1 ,15);
67 float pathlib_makenode_adaptive(entity parent,vector start, vector to, vector goal,float cost)
70 float h,g,f,doedge = 0;
73 ++pathlib_searched_cnt;
75 if(inwater(parent.origin))
77 LOG_TRACE("FromWater");
78 pathlib_expandnode = pathlib_expandnode_box;
79 pathlib_movenode = pathlib_swimnode;
86 pathlib_expandnode = pathlib_expandnode_box;
87 pathlib_movenode = pathlib_walknode;
91 LOG_TRACE("LandToLoand");
92 //if(edge_check(parent.origin))
95 pathlib_expandnode = pathlib_expandnode_star;
96 pathlib_movenode = pathlib_walknode;
101 node = pathlib_nodeatpoint(to);
104 LOG_TRACE("NodeAtPoint");
107 if(node.owner == openlist)
109 h = pathlib_heuristic(node.origin,goal);
110 g = pathlib_cost(parent,node.origin,cost);
113 if(node.pathlib_node_g > g)
115 node.pathlib_node_h = h;
116 node.pathlib_node_g = g;
117 node.pathlib_node_f = f;
119 node.path_prev = parent;
123 best_open_node = node;
124 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
125 best_open_node = node;
131 where = pathlib_movenode(parent, parent.origin, to, 0);
133 if (!pathlib_movenode_goodnode)
135 //pathlib_showsquare(where, 0 ,30);
136 //pathlib_showsquare(parent.origin, 1 ,30);
137 LOG_TRACE("pathlib_movenode_goodnode = 0");
141 //pathlib_showsquare(where, 1 ,30);
143 if(pathlib_nodeatpoint(where))
145 LOG_TRACE("NAP WHERE :",vtos(where));
146 LOG_TRACE("not NAP TO:",vtos(to));
147 LOG_TRACE("NAP-NNAP:",ftos(vlen(to-where)));
153 if (!tile_check(parent, where))
155 LOG_TRACE("tile_check fail");
157 pathlib_showsquare(where, 0 ,30);
163 h = pathlib_heuristic(where,goal);
164 g = pathlib_cost(parent,where,cost);
168 node = findradius(where,pathlib_gridsize * 0.5);
171 if(node.is_path_node == true)
174 if(node.owner == openlist)
176 if(node.pathlib_node_g > g)
178 //pathlib_movenode(node, where,node.origin,0);
179 //if(pathlib_movenode_goodnode)
181 //mark_error(node.origin + '0 0 128',30);
182 node.pathlib_node_h = h;
183 node.pathlib_node_g = g;
184 node.pathlib_node_f = f;
185 node.path_prev = parent;
190 best_open_node = node;
191 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
192 best_open_node = node;
201 node = pathlib_mknode(where, parent);
202 node.pathlib_node_h = h;
203 node.pathlib_node_g = g;
204 node.pathlib_node_f = f;
207 best_open_node = node;
208 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
209 best_open_node = node;
214 entity pathlib_getbestopen()
218 ++pathlib_bestcash_hits;
219 pathlib_bestcash_saved += pathlib_open_cnt;
221 return best_open_node;
224 entity bestnode = NULL;
225 FOREACH_ENTITY_ENT(owner, openlist,
227 ++pathlib_bestopen_searched;
229 if(!bestnode || it.pathlib_node_f < bestnode.pathlib_node_f)
236 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;
274 node = findfloat(NULL,is_path_node, true);
278 node.owner = openlist;
279 node.pathlib_node_g = 0;
280 node.pathlib_node_h = 0;
281 node.pathlib_node_f = 0;
282 node.path_prev = NULL;
286 node = findfloat(node,is_path_node, true);
300 float Cosine_Interpolate(float a, float b, float x)
305 f = (1 - cos(ft)) * 0.5;
307 return a*(1-f) + b*f;
310 float buildpath_nodefilter_directional(vector n,vector c,vector p)
314 d2 = normalize(p - c);
315 d1 = normalize(c - n);
317 if(vdist(d1 - d2, <, 0.25))
326 float buildpath_nodefilter_moveskip(entity this, vector n,vector c,vector p)
328 pathlib_walknode(this, p, n, 1);
329 if(pathlib_movenode_goodnode)
335 float buildpath_nodefilter_none(vector n,vector c,vector p)
340 entity path_build(entity next, vector where, entity prev, entity start)
345 if(buildpath_nodefilter)
346 if(buildpath_nodefilter(next.origin,where,prev.origin))
352 path.path_next = next;
354 setorigin(path, where);
357 path.classname = "path_end";
361 path.classname = "path_start";
363 path.classname = "path_node";
369 entity pathlib_astar(entity this, vector from,vector to)
371 entity path, start, end, open, n, ln;
372 float ptime, ftime, ctime;
374 ptime = gettime(GETTIME_REALTIME);
375 pathlib_starttime = ptime;
379 // Select water<->land capable node make/link
380 pathlib_makenode = pathlib_makenode_adaptive;
382 // Select XYZ cost estimate
383 //pathlib_heuristic = pathlib_h_diagonal3;
384 pathlib_heuristic = pathlib_h_diagonal;
386 // Select distance + waterfactor cost
387 pathlib_cost = pathlib_g_euclidean_water;
389 // Select star expander
390 pathlib_expandnode = pathlib_expandnode_star;
392 // Select walk simulation movement test
393 pathlib_movenode = pathlib_walknode;
395 // Filter final nodes by direction
396 buildpath_nodefilter = buildpath_nodefilter_directional;
398 // Filter tiles with cross pattern
399 tile_check = tile_check_cross;
401 // If the start is in water we need diffrent settings
404 // Select volumetric node expaner
405 pathlib_expandnode = pathlib_expandnode_box;
407 // Water movement test
408 pathlib_movenode = pathlib_swimnode;
415 closedlist = spawn();
417 pathlib_closed_cnt = 0;
418 pathlib_open_cnt = 0;
419 pathlib_made_cnt = 0;
420 pathlib_merge_cnt = 0;
421 pathlib_searched_cnt = 0;
422 pathlib_bestopen_searched = 0;
423 pathlib_bestcash_hits = 0;
424 pathlib_bestcash_saved = 0;
426 pathlib_gridsize = 128;
427 pathlib_movecost = pathlib_gridsize;
428 pathlib_movecost_diag = vlen(('1 1 0' * pathlib_gridsize));
429 pathlib_movecost_waterfactor = 25000000;
430 pathlib_foundgoal = 0;
432 movenode_boxmax = this.maxs * 1.1;
433 movenode_boxmin = this.mins * 1.1;
435 movenode_stepsize = pathlib_gridsize * 0.25;
437 tile_check_size = pathlib_gridsize * 0.5;
438 tile_check_up = '0 0 2' * pathlib_gridsize;
439 tile_check_up = '0 0 128';
440 tile_check_down = '0 0 3' * pathlib_gridsize;
441 tile_check_down = '0 0 256';
443 //movenode_stepup = '0 0 128';
444 movenode_stepup = '0 0 36';
445 movenode_maxdrop = '0 0 512';
446 //movenode_maxdrop = '0 0 512';
447 movenode_boxup = '0 0 72';
449 from.x = fsnap(from.x, pathlib_gridsize);
450 from.y = fsnap(from.y, pathlib_gridsize);
453 to.x = fsnap(to.x, pathlib_gridsize);
454 to.y = fsnap(to.y, pathlib_gridsize);
457 LOG_TRACE("AStar init");
458 path = pathlib_mknode(from, NULL);
459 pathlib_close_node(path, to);
460 if(pathlib_foundgoal)
462 LOG_TRACE("AStar: Goal found on first node!");
464 open = new(path_end);
466 setorigin(open, path.origin);
473 if(pathlib_expandnode(path, from, to) <= 0)
475 LOG_TRACE("AStar path fail.");
481 best_open_node = pathlib_getbestopen();
483 pathlib_close_node(best_open_node, to);
484 if(inwater(n.origin))
485 pathlib_expandnode_box(n, from, to);
487 pathlib_expandnode_star(n, from, to);
489 while(pathlib_open_cnt)
491 if((gettime(GETTIME_REALTIME) - pathlib_starttime) > pathlib_maxtime)
493 LOG_TRACE("Path took to long to compute!");
494 LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt));
495 LOG_TRACE("Nodes - open: ", ftos(pathlib_open_cnt));
496 LOG_TRACE("Nodes - merged: ", ftos(pathlib_merge_cnt));
497 LOG_TRACE("Nodes - closed: ", ftos(pathlib_closed_cnt));
503 best_open_node = pathlib_getbestopen();
505 pathlib_close_node(best_open_node,to);
507 if(inwater(n.origin))
508 pathlib_expandnode_box(n,from,to);
510 pathlib_expandnode(n,from,to);
512 if(pathlib_foundgoal)
514 LOG_TRACE("Target found. Rebuilding and filtering path...");
515 ftime = gettime(GETTIME_REALTIME);
516 ptime = ftime - ptime;
518 start = path_build(NULL,path.origin,NULL,NULL);
519 end = path_build(NULL,goal_node.origin,NULL,start);
523 for(open = goal_node; open.path_prev != path; open = open.path_prev)
525 n = path_build(ln,open.origin,open.path_prev,start);
531 ftime = gettime(GETTIME_REALTIME) - ftime;
533 ctime = gettime(GETTIME_REALTIME);
535 ctime = gettime(GETTIME_REALTIME) - ctime;
539 pathlib_showpath2(start);
541 LOG_TRACE("Time used - pathfinding: ", ftos(ptime));
542 LOG_TRACE("Time used - rebuild & filter: ", ftos(ftime));
543 LOG_TRACE("Time used - cleanup: ", ftos(ctime));
544 LOG_TRACE("Time used - total: ", ftos(ptime + ftime + ctime));
545 LOG_TRACE("Time used - # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)));
546 LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt));
547 LOG_TRACE("Nodes - open: ", ftos(pathlib_open_cnt));
548 LOG_TRACE("Nodes - merged: ", ftos(pathlib_merge_cnt));
549 LOG_TRACE("Nodes - closed: ", ftos(pathlib_closed_cnt));
550 LOG_TRACE("Nodes - searched: ", ftos(pathlib_searched_cnt));
551 LOG_TRACE("Nodes bestopen searched: ", ftos(pathlib_bestopen_searched));
552 LOG_TRACE("Nodes bestcash - hits: ", ftos(pathlib_bestcash_hits));
553 LOG_TRACE("Nodes bestcash - save: ", ftos(pathlib_bestcash_saved));
554 LOG_TRACE("AStar done.");
560 LOG_TRACE("A* Faild to find a path! Try a smaller gridsize.");