4 #include "../command/common.qh"
6 void pathlib_deletepath(entity start)
10 e = findchainentity(owner, start);
13 e.think = SUB_Remove_self;
19 //#define PATHLIB_NODEEXPIRE 0.05
20 const float PATHLIB_NODEEXPIRE = 20;
22 void dumpnode(entity n)
24 n.is_path_node = false;
25 n.think = SUB_Remove_self;
30 void pathlib_showpath(entity start);
31 void pathlib_showpath2(entity path);
34 void pathlib_showsquare(vector where,float goodsquare,float _lifetime);
35 void pathlib_showsquare2(entity node ,vector ncolor,float align);
37 entity pathlib_mknode(vector where,entity parent)
41 node = pathlib_nodeatpoint(where);
45 mark_error(where, 60);
52 node.think = SUB_Remove_self;
53 node.nextthink = time + PATHLIB_NODEEXPIRE;
54 node.is_path_node = true;
55 node.owner = openlist;
56 node.path_prev = parent;
59 setsize(node, '0 0 0', '0 0 0');
61 setorigin(node, where);
62 node.medium = pointcontents(where);
63 pathlib_showsquare(where, 1 ,15);
71 float pathlib_makenode_adaptive(entity parent,vector start, vector to, vector goal,float cost)
74 float h,g,f,doedge = 0;
77 ++pathlib_searched_cnt;
79 if(inwater(parent.origin))
81 LOG_TRACE("FromWater\n");
82 pathlib_expandnode = pathlib_expandnode_box;
83 pathlib_movenode = pathlib_swimnode;
89 LOG_TRACE("ToWater\n");
90 pathlib_expandnode = pathlib_expandnode_box;
91 pathlib_movenode = pathlib_walknode;
95 LOG_TRACE("LandToLoand\n");
96 //if(edge_check(parent.origin))
99 pathlib_expandnode = pathlib_expandnode_star;
100 pathlib_movenode = pathlib_walknode;
105 node = pathlib_nodeatpoint(to);
108 LOG_TRACE("NodeAtPoint\n");
111 if(node.owner == openlist)
113 h = pathlib_heuristic(node.origin,goal);
114 g = pathlib_cost(parent,node.origin,cost);
117 if(node.pathlib_node_g > g)
119 node.pathlib_node_h = h;
120 node.pathlib_node_g = g;
121 node.pathlib_node_f = f;
123 node.path_prev = parent;
127 best_open_node = node;
128 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
129 best_open_node = node;
135 where = pathlib_movenode(parent.origin, to, 0);
137 if (!pathlib_movenode_goodnode)
139 //pathlib_showsquare(where, 0 ,30);
140 //pathlib_showsquare(parent.origin, 1 ,30);
141 LOG_TRACE("pathlib_movenode_goodnode = 0\n");
145 //pathlib_showsquare(where, 1 ,30);
147 if(pathlib_nodeatpoint(where))
149 LOG_TRACE("NAP WHERE :",vtos(where),"\n");
150 LOG_TRACE("not NAP TO:",vtos(to),"\n");
151 LOG_TRACE("NAP-NNAP:",ftos(vlen(to-where)),"\n\n");
157 if (!tile_check(where))
159 LOG_TRACE("tile_check fail\n");
160 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(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()
223 ++pathlib_bestcash_hits;
224 pathlib_bestcash_saved += pathlib_open_cnt;
226 return best_open_node;
229 node = findchainentity(owner,openlist);
236 ++pathlib_bestopen_seached;
237 if(node.pathlib_node_f < bestnode.pathlib_node_f)
246 void pathlib_close_node(entity node,vector goal)
249 if(node.owner == closedlist)
251 LOG_TRACE("Pathlib: Tried to close a closed node!\n");
255 if(node == best_open_node)
256 best_open_node = world;
258 ++pathlib_closed_cnt;
261 node.owner = closedlist;
263 if(vlen(node.origin - goal) <= pathlib_gridsize)
267 goalmove = pathlib_walknode(node.origin,goal,1);
268 if(pathlib_movenode_goodnode)
271 pathlib_foundgoal = true;
276 void pathlib_cleanup()
278 best_open_node = world;
284 node = findfloat(world,is_path_node, true);
288 node.owner = openlist;
289 node.pathlib_node_g = 0;
290 node.pathlib_node_h = 0;
291 node.pathlib_node_f = 0;
292 node.path_prev = world;
296 node = findfloat(node,is_path_node, true);
310 float Cosine_Interpolate(float a, float b, float x)
315 f = (1 - cos(ft)) * 0.5;
317 return a*(1-f) + b*f;
320 float buildpath_nodefilter_directional(vector n,vector c,vector p)
324 d2 = normalize(p - c);
325 d1 = normalize(c - n);
327 if(vlen(d1-d2) < 0.25)
336 float buildpath_nodefilter_moveskip(vector n,vector c,vector p)
338 pathlib_walknode(p,n,1);
339 if(pathlib_movenode_goodnode)
345 float buildpath_nodefilter_none(vector n,vector c,vector p)
350 entity path_build(entity next, vector where, entity prev, entity start)
355 if(buildpath_nodefilter)
356 if(buildpath_nodefilter(next.origin,where,prev.origin))
362 path.path_next = next;
364 setorigin(path,where);
367 path.classname = "path_end";
371 path.classname = "path_start";
373 path.classname = "path_node";
379 entity pathlib_astar(vector from,vector to)
381 entity path, start, end, open, n, ln;
382 float ptime, ftime, ctime;
384 ptime = gettime(GETTIME_REALTIME);
385 pathlib_starttime = ptime;
389 // Select water<->land capable node make/link
390 pathlib_makenode = pathlib_makenode_adaptive;
392 // Select XYZ cost estimate
393 //pathlib_heuristic = pathlib_h_diagonal3;
394 pathlib_heuristic = pathlib_h_diagonal;
396 // Select distance + waterfactor cost
397 pathlib_cost = pathlib_g_euclidean_water;
399 // Select star expander
400 pathlib_expandnode = pathlib_expandnode_star;
402 // Select walk simulation movement test
403 pathlib_movenode = pathlib_walknode;
405 // Filter final nodes by direction
406 buildpath_nodefilter = buildpath_nodefilter_directional;
408 // Filter tiles with cross pattern
409 tile_check = tile_check_cross;
411 // If the start is in water we need diffrent settings
414 // Select volumetric node expaner
415 pathlib_expandnode = pathlib_expandnode_box;
417 // Water movement test
418 pathlib_movenode = pathlib_swimnode;
425 closedlist = spawn();
427 pathlib_closed_cnt = 0;
428 pathlib_open_cnt = 0;
429 pathlib_made_cnt = 0;
430 pathlib_merge_cnt = 0;
431 pathlib_searched_cnt = 0;
432 pathlib_bestopen_seached = 0;
433 pathlib_bestcash_hits = 0;
434 pathlib_bestcash_saved = 0;
436 pathlib_gridsize = 128;
437 pathlib_movecost = pathlib_gridsize;
438 pathlib_movecost_diag = vlen(('1 1 0' * pathlib_gridsize));
439 pathlib_movecost_waterfactor = 25000000;
440 pathlib_foundgoal = 0;
442 movenode_boxmax = self.maxs * 1.1;
443 movenode_boxmin = self.mins * 1.1;
445 movenode_stepsize = pathlib_gridsize * 0.25;
447 tile_check_size = pathlib_gridsize * 0.5;
448 tile_check_up = '0 0 2' * pathlib_gridsize;
449 tile_check_up = '0 0 128';
450 tile_check_down = '0 0 3' * pathlib_gridsize;
451 tile_check_down = '0 0 256';
453 //movenode_stepup = '0 0 128';
454 movenode_stepup = '0 0 36';
455 movenode_maxdrop = '0 0 512';
456 //movenode_maxdrop = '0 0 512';
457 movenode_boxup = '0 0 72';
459 from.x = fsnap(from.x, pathlib_gridsize);
460 from.y = fsnap(from.y, pathlib_gridsize);
463 to.x = fsnap(to.x, pathlib_gridsize);
464 to.y = fsnap(to.y, pathlib_gridsize);
467 LOG_TRACE("AStar init\n");
468 path = pathlib_mknode(from, world);
469 pathlib_close_node(path, to);
470 if(pathlib_foundgoal)
472 LOG_TRACE("AStar: Goal found on first node!\n");
474 open = new(path_end);
476 setorigin(open,path.origin);
483 if(pathlib_expandnode(path, from, to) <= 0)
485 LOG_TRACE("AStar path fail.\n");
491 best_open_node = pathlib_getbestopen();
493 pathlib_close_node(best_open_node, to);
494 if(inwater(n.origin))
495 pathlib_expandnode_box(n, from, to);
497 pathlib_expandnode_star(n, from, to);
499 while(pathlib_open_cnt)
501 if((gettime(GETTIME_REALTIME) - pathlib_starttime) > pathlib_maxtime)
503 LOG_TRACE("Path took to long to compute!\n");
504 LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt),"\n");
505 LOG_TRACE("Nodes - open: ", ftos(pathlib_open_cnt),"\n");
506 LOG_TRACE("Nodes - merged: ", ftos(pathlib_merge_cnt),"\n");
507 LOG_TRACE("Nodes - closed: ", ftos(pathlib_closed_cnt),"\n");
513 best_open_node = pathlib_getbestopen();
515 pathlib_close_node(best_open_node,to);
517 if(inwater(n.origin))
518 pathlib_expandnode_box(n,from,to);
520 pathlib_expandnode(n,from,to);
522 if(pathlib_foundgoal)
524 LOG_TRACE("Target found. Rebuilding and filtering path...\n");
525 ftime = gettime(GETTIME_REALTIME);
526 ptime = ftime - ptime;
528 start = path_build(world,path.origin,world,world);
529 end = path_build(world,goal_node.origin,world,start);
533 for(open = goal_node; open.path_prev != path; open = open.path_prev)
535 n = path_build(ln,open.origin,open.path_prev,start);
541 ftime = gettime(GETTIME_REALTIME) - ftime;
543 ctime = gettime(GETTIME_REALTIME);
545 ctime = gettime(GETTIME_REALTIME) - ctime;
549 pathlib_showpath2(start);
551 LOG_TRACE("Time used - pathfinding: ", ftos(ptime),"\n");
552 LOG_TRACE("Time used - rebuild & filter: ", ftos(ftime),"\n");
553 LOG_TRACE("Time used - cleanup: ", ftos(ctime),"\n");
554 LOG_TRACE("Time used - total: ", ftos(ptime + ftime + ctime),"\n");
555 LOG_TRACE("Time used - # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)),"\n\n");
556 LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt),"\n");
557 LOG_TRACE("Nodes - open: ", ftos(pathlib_open_cnt),"\n");
558 LOG_TRACE("Nodes - merged: ", ftos(pathlib_merge_cnt),"\n");
559 LOG_TRACE("Nodes - closed: ", ftos(pathlib_closed_cnt),"\n");
560 LOG_TRACE("Nodes - searched: ", ftos(pathlib_searched_cnt),"\n");
561 LOG_TRACE("Nodes bestopen searched: ", ftos(pathlib_bestopen_seached),"\n");
562 LOG_TRACE("Nodes bestcash - hits: ", ftos(pathlib_bestcash_hits),"\n");
563 LOG_TRACE("Nodes bestcash - save: ", ftos(pathlib_bestcash_saved),"\n");
564 LOG_TRACE("AStar done.\n");
570 LOG_TRACE("A* Faild to find a path! Try a smaller gridsize.\n");