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);
32 void pathlib_showsquare(vector where,float goodsquare,float _lifetime);
36 entity pathlib_mknode(vector where,entity parent)
40 node = pathlib_nodeatpoint(where);
44 mark_error(where, 60);
51 node.think = SUB_Remove_self;
52 node.nextthink = time + PATHLIB_NODEEXPIRE;
53 node.is_path_node = true;
54 node.owner = openlist;
55 node.path_prev = parent;
58 setsize(node, '0 0 0', '0 0 0');
60 setorigin(node, where);
61 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");
161 pathlib_showsquare(where, 0 ,30);
167 h = pathlib_heuristic(where,goal);
168 g = pathlib_cost(parent,where,cost);
172 node = findradius(where,pathlib_gridsize * 0.5);
175 if(node.is_path_node == true)
178 if(node.owner == openlist)
180 if(node.pathlib_node_g > g)
182 //pathlib_movenode(where,node.origin,0);
183 //if(pathlib_movenode_goodnode)
185 //mark_error(node.origin + '0 0 128',30);
186 node.pathlib_node_h = h;
187 node.pathlib_node_g = g;
188 node.pathlib_node_f = f;
189 node.path_prev = parent;
194 best_open_node = node;
195 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
196 best_open_node = node;
205 node = pathlib_mknode(where, parent);
206 node.pathlib_node_h = h;
207 node.pathlib_node_g = g;
208 node.pathlib_node_f = f;
211 best_open_node = node;
212 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
213 best_open_node = node;
218 entity pathlib_getbestopen()
225 ++pathlib_bestcash_hits;
226 pathlib_bestcash_saved += pathlib_open_cnt;
228 return best_open_node;
231 node = findchainentity(owner,openlist);
238 ++pathlib_bestopen_seached;
239 if(node.pathlib_node_f < bestnode.pathlib_node_f)
248 void pathlib_close_node(entity node,vector goal)
251 if(node.owner == closedlist)
253 LOG_TRACE("Pathlib: Tried to close a closed node!\n");
257 if(node == best_open_node)
258 best_open_node = world;
260 ++pathlib_closed_cnt;
263 node.owner = closedlist;
265 if(vlen(node.origin - goal) <= pathlib_gridsize)
269 goalmove = pathlib_walknode(node.origin,goal,1);
270 if(pathlib_movenode_goodnode)
273 pathlib_foundgoal = true;
278 void pathlib_cleanup()
280 best_open_node = world;
286 node = findfloat(world,is_path_node, true);
290 node.owner = openlist;
291 node.pathlib_node_g = 0;
292 node.pathlib_node_h = 0;
293 node.pathlib_node_f = 0;
294 node.path_prev = world;
298 node = findfloat(node,is_path_node, true);
312 float Cosine_Interpolate(float a, float b, float x)
317 f = (1 - cos(ft)) * 0.5;
319 return a*(1-f) + b*f;
322 float buildpath_nodefilter_directional(vector n,vector c,vector p)
326 d2 = normalize(p - c);
327 d1 = normalize(c - n);
329 if(vlen(d1-d2) < 0.25)
338 float buildpath_nodefilter_moveskip(vector n,vector c,vector p)
340 pathlib_walknode(p,n,1);
341 if(pathlib_movenode_goodnode)
347 float buildpath_nodefilter_none(vector n,vector c,vector p)
352 entity path_build(entity next, vector where, entity prev, entity start)
357 if(buildpath_nodefilter)
358 if(buildpath_nodefilter(next.origin,where,prev.origin))
364 path.path_next = next;
366 setorigin(path,where);
369 path.classname = "path_end";
373 path.classname = "path_start";
375 path.classname = "path_node";
381 entity pathlib_astar(vector from,vector to)
383 entity path, start, end, open, n, ln;
384 float ptime, ftime, ctime;
386 ptime = gettime(GETTIME_REALTIME);
387 pathlib_starttime = ptime;
391 // Select water<->land capable node make/link
392 pathlib_makenode = pathlib_makenode_adaptive;
394 // Select XYZ cost estimate
395 //pathlib_heuristic = pathlib_h_diagonal3;
396 pathlib_heuristic = pathlib_h_diagonal;
398 // Select distance + waterfactor cost
399 pathlib_cost = pathlib_g_euclidean_water;
401 // Select star expander
402 pathlib_expandnode = pathlib_expandnode_star;
404 // Select walk simulation movement test
405 pathlib_movenode = pathlib_walknode;
407 // Filter final nodes by direction
408 buildpath_nodefilter = buildpath_nodefilter_directional;
410 // Filter tiles with cross pattern
411 tile_check = tile_check_cross;
413 // If the start is in water we need diffrent settings
416 // Select volumetric node expaner
417 pathlib_expandnode = pathlib_expandnode_box;
419 // Water movement test
420 pathlib_movenode = pathlib_swimnode;
427 closedlist = spawn();
429 pathlib_closed_cnt = 0;
430 pathlib_open_cnt = 0;
431 pathlib_made_cnt = 0;
432 pathlib_merge_cnt = 0;
433 pathlib_searched_cnt = 0;
434 pathlib_bestopen_seached = 0;
435 pathlib_bestcash_hits = 0;
436 pathlib_bestcash_saved = 0;
438 pathlib_gridsize = 128;
439 pathlib_movecost = pathlib_gridsize;
440 pathlib_movecost_diag = vlen(('1 1 0' * pathlib_gridsize));
441 pathlib_movecost_waterfactor = 25000000;
442 pathlib_foundgoal = 0;
444 movenode_boxmax = self.maxs * 1.1;
445 movenode_boxmin = self.mins * 1.1;
447 movenode_stepsize = pathlib_gridsize * 0.25;
449 tile_check_size = pathlib_gridsize * 0.5;
450 tile_check_up = '0 0 2' * pathlib_gridsize;
451 tile_check_up = '0 0 128';
452 tile_check_down = '0 0 3' * pathlib_gridsize;
453 tile_check_down = '0 0 256';
455 //movenode_stepup = '0 0 128';
456 movenode_stepup = '0 0 36';
457 movenode_maxdrop = '0 0 512';
458 //movenode_maxdrop = '0 0 512';
459 movenode_boxup = '0 0 72';
461 from.x = fsnap(from.x, pathlib_gridsize);
462 from.y = fsnap(from.y, pathlib_gridsize);
465 to.x = fsnap(to.x, pathlib_gridsize);
466 to.y = fsnap(to.y, pathlib_gridsize);
469 LOG_TRACE("AStar init\n");
470 path = pathlib_mknode(from, world);
471 pathlib_close_node(path, to);
472 if(pathlib_foundgoal)
474 LOG_TRACE("AStar: Goal found on first node!\n");
476 open = new(path_end);
478 setorigin(open,path.origin);
485 if(pathlib_expandnode(path, from, to) <= 0)
487 LOG_TRACE("AStar path fail.\n");
493 best_open_node = pathlib_getbestopen();
495 pathlib_close_node(best_open_node, to);
496 if(inwater(n.origin))
497 pathlib_expandnode_box(n, from, to);
499 pathlib_expandnode_star(n, from, to);
501 while(pathlib_open_cnt)
503 if((gettime(GETTIME_REALTIME) - pathlib_starttime) > pathlib_maxtime)
505 LOG_TRACE("Path took to long to compute!\n");
506 LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt),"\n");
507 LOG_TRACE("Nodes - open: ", ftos(pathlib_open_cnt),"\n");
508 LOG_TRACE("Nodes - merged: ", ftos(pathlib_merge_cnt),"\n");
509 LOG_TRACE("Nodes - closed: ", ftos(pathlib_closed_cnt),"\n");
515 best_open_node = pathlib_getbestopen();
517 pathlib_close_node(best_open_node,to);
519 if(inwater(n.origin))
520 pathlib_expandnode_box(n,from,to);
522 pathlib_expandnode(n,from,to);
524 if(pathlib_foundgoal)
526 LOG_TRACE("Target found. Rebuilding and filtering path...\n");
527 ftime = gettime(GETTIME_REALTIME);
528 ptime = ftime - ptime;
530 start = path_build(world,path.origin,world,world);
531 end = path_build(world,goal_node.origin,world,start);
535 for(open = goal_node; open.path_prev != path; open = open.path_prev)
537 n = path_build(ln,open.origin,open.path_prev,start);
543 ftime = gettime(GETTIME_REALTIME) - ftime;
545 ctime = gettime(GETTIME_REALTIME);
547 ctime = gettime(GETTIME_REALTIME) - ctime;
551 pathlib_showpath2(start);
553 LOG_TRACE("Time used - pathfinding: ", ftos(ptime),"\n");
554 LOG_TRACE("Time used - rebuild & filter: ", ftos(ftime),"\n");
555 LOG_TRACE("Time used - cleanup: ", ftos(ctime),"\n");
556 LOG_TRACE("Time used - total: ", ftos(ptime + ftime + ctime),"\n");
557 LOG_TRACE("Time used - # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)),"\n\n");
558 LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt),"\n");
559 LOG_TRACE("Nodes - open: ", ftos(pathlib_open_cnt),"\n");
560 LOG_TRACE("Nodes - merged: ", ftos(pathlib_merge_cnt),"\n");
561 LOG_TRACE("Nodes - closed: ", ftos(pathlib_closed_cnt),"\n");
562 LOG_TRACE("Nodes - searched: ", ftos(pathlib_searched_cnt),"\n");
563 LOG_TRACE("Nodes bestopen searched: ", ftos(pathlib_bestopen_seached),"\n");
564 LOG_TRACE("Nodes bestcash - hits: ", ftos(pathlib_bestcash_hits),"\n");
565 LOG_TRACE("Nodes bestcash - save: ", ftos(pathlib_bestcash_saved),"\n");
566 LOG_TRACE("AStar done.\n");
572 LOG_TRACE("A* Faild to find a path! Try a smaller gridsize.\n");