6 #define medium spawnshieldtime
10 float edge_show(vector point,float fsize);
11 void mark_error(vector where,float lifetime);
12 void mark_info(vector where,float lifetime);
13 entity mark_misc(vector where,float lifetime);
15 void pathlib_showpath(entity start)
21 te_lightning1(e,e.origin,e.path_next.origin);
28 pathlib_showpath(self);
29 self.nextthink = time + 1;
32 void __showpath2_think()
34 mark_info(self.origin,1);
37 self.path_next.think = __showpath2_think;
38 self.path_next.nextthink = time + 0.15;
42 self.owner.think = __showpath2_think;
43 self.owner.nextthink = time + 0.15;
47 void pathlib_showpath2(entity path)
49 path.think = __showpath2_think;
50 path.nextthink = time;
55 void pathlib_deletepath(entity start)
59 e = findchainentity(owner, start);
68 float fsnap(float val,float fsize)
70 return rint(val / fsize) * fsize;
73 vector vsnap(vector point,float fsize)
77 vret.x = rint(point.x / fsize) * fsize;
78 vret.y = rint(point.y / fsize) * fsize;
79 vret.z = ceil(point.z / fsize) * fsize;
84 float walknode_stepsize;
85 vector walknode_stepup;
86 vector walknode_maxdrop;
87 vector walknode_boxup;
88 vector walknode_boxmax;
89 vector walknode_boxmin;
90 float pathlib_movenode_goodnode;
92 float floor_ok(vector point)
96 if(trace_dphitq3surfaceflags & Q3SURFACEFLAG_SKY)
99 pc = pointcontents(point);
109 if(pointcontents(point - '0 0 1') != CONTENT_SOLID)
115 if(pointcontents(point - '0 0 1') == CONTENT_SOLID)
121 #define _pcheck(p) traceline(p+z_up,p-z_down,MOVE_WORLDONLY,self); if (!floor_ok(trace_endpos)) return 1
122 float edge_check(vector point,float fsize)
126 z_up = '0 0 1' * fsize;
127 z_down = '0 0 1' * fsize;
129 _pcheck(point + ('1 1 0' * fsize));
130 _pcheck(point + ('1 -1 0' * fsize));
131 _pcheck(point + ('1 0 0' * fsize));
133 _pcheck(point + ('0 1 0' * fsize));
134 _pcheck(point + ('0 -1 0' * fsize));
136 _pcheck(point + ('-1 0 0' * fsize));
137 _pcheck(point + ('-1 1 0' * fsize));
138 _pcheck(point + ('-1 -1 0' * fsize));
144 #define _pshow(p) mark_error(p,10)
145 float edge_show(vector point,float fsize)
148 _pshow(point + ('1 1 0' * fsize));
149 _pshow(point + ('1 -1 0' * fsize));
150 _pshow(point + ('1 0 0' * fsize));
152 _pshow(point + ('0 1 0' * fsize));
153 _pshow(point + ('0 -1 0' * fsize));
155 _pshow(point + ('-1 0 0' * fsize));
156 _pshow(point + ('-1 1 0' * fsize));
157 _pshow(point + ('-1 -1 0' * fsize));
163 var vector pathlib_movenode(vector start,vector end,float doedge);
164 vector pathlib_wateroutnode(vector start,vector end,float doedge)
168 pathlib_movenode_goodnode = 0;
170 end.x = fsnap(end.x, pathlib_gridsize);
171 end.y = fsnap(end.y, pathlib_gridsize);
173 traceline(end + ('0 0 0.25' * pathlib_gridsize),end - ('0 0 1' * pathlib_gridsize),MOVE_WORLDONLY,self);
176 if(pointcontents(end - '0 0 1') != CONTENT_SOLID)
179 for(surface = start ; surface.z < (end.z + 32); ++surface.z)
181 if(pointcontents(surface) == CONTENT_EMPTY)
185 if(pointcontents(surface + '0 0 1') != CONTENT_EMPTY)
188 tracebox(start + '0 0 64', walknode_boxmin,walknode_boxmax, end + '0 0 64', MOVE_WORLDONLY, self);
189 if(trace_fraction == 1)
190 pathlib_movenode_goodnode = 1;
192 if(fabs(surface.z - end.z) > 32)
193 pathlib_movenode_goodnode = 0;
198 vector pathlib_swimnode(vector start,vector end,float doedge)
200 pathlib_movenode_goodnode = 0;
202 if(pointcontents(start) != CONTENT_WATER)
205 end.x = fsnap(end.x, pathlib_gridsize);
206 end.y = fsnap(end.y, pathlib_gridsize);
208 if(pointcontents(end) == CONTENT_EMPTY)
209 return pathlib_wateroutnode( start, end, doedge);
211 tracebox(start, walknode_boxmin,walknode_boxmax, end, MOVE_WORLDONLY, self);
212 if(trace_fraction == 1)
213 pathlib_movenode_goodnode = 1;
218 vector pathlib_flynode(vector start,vector end)
220 pathlib_movenode_goodnode = 0;
222 end.x = fsnap(end.x, pathlib_gridsize);
223 end.y = fsnap(end.y, pathlib_gridsize);
225 tracebox(start, walknode_boxmin,walknode_boxmax, end, MOVE_WORLDONLY, self);
226 if(trace_fraction == 1)
227 pathlib_movenode_goodnode = 1;
232 vector pathlib_walknode(vector start,vector end,float doedge)
234 vector direction,point,last_point,s,e;
235 float steps, distance, i,laststep;
237 pathlib_movenode_goodnode = 0;
243 direction = normalize(s - e);
245 distance = vlen(start - end);
246 laststep = distance / walknode_stepsize;
247 steps = floor(laststep);
248 laststep = laststep - steps;
251 s = point + walknode_stepup;
252 e = point - walknode_maxdrop;
254 traceline(s, e,MOVE_WORLDONLY,self);
255 if(trace_fraction == 1.0)
258 if (floor_ok(trace_endpos) == 0)
261 last_point = trace_endpos;
263 for(i = 0; i < steps; ++i)
265 point = last_point + direction * walknode_stepsize;
267 s = point + walknode_stepup;
268 e = point - walknode_maxdrop;
269 traceline(s, e,MOVE_WORLDONLY,self);
270 if(trace_fraction == 1.0)
273 point = trace_endpos;
274 if (!floor_ok(trace_endpos))
277 tracebox(last_point + walknode_boxup, walknode_boxmin,walknode_boxmax, point + walknode_boxup, MOVE_WORLDONLY, self);
278 if(trace_fraction != 1.0)
282 if(edge_check(point,pathlib_edge_check_size))
288 point = last_point + direction * walknode_stepsize * laststep;
290 point.x = fsnap(point.x, pathlib_gridsize);
291 point.y = fsnap(point.y, pathlib_gridsize);
293 s = point + walknode_stepup;
294 e = point - walknode_maxdrop;
295 traceline(s, e,MOVE_WORLDONLY,self);
297 if(trace_fraction == 1.0)
300 point = trace_endpos;
302 if (!floor_ok(trace_endpos))
305 tracebox(last_point + walknode_boxup, walknode_boxmin,walknode_boxmax, point + walknode_boxup, MOVE_WORLDONLY, self);
306 if(trace_fraction != 1.0)
309 pathlib_movenode_goodnode = 1;
313 var float pathlib_cost(entity parent,vector to, float static_cost);
314 float pathlib_g_static(entity parent,vector to, float static_cost)
317 return parent.pathlib_node_g + static_cost * pathlib_movecost_waterfactor;
319 return parent.pathlib_node_g + static_cost;
322 float pathlib_g_static_water(entity parent,vector to, float static_cost)
325 return parent.pathlib_node_g + static_cost * pathlib_movecost_waterfactor;
327 return parent.pathlib_node_g + static_cost;
330 float pathlib_g_euclidean(entity parent,vector to, float static_cost)
332 return parent.pathlib_node_g + vlen(parent.origin - to);
334 float pathlib_g_euclidean_water(entity parent,vector to, float static_cost)
337 return parent.pathlib_node_g + vlen(parent.origin - to) * pathlib_movecost_waterfactor;
339 return parent.pathlib_node_g + vlen(parent.origin - to);
342 var float(vector from,vector to) pathlib_heuristic;
345 Manhattan Menas we expect to move up,down left or right
346 No diagonal moves espected. (like moving bewteen city blocks)
348 float pathlib_h_manhattan(vector a,vector b)
350 //h(n) = D * (abs(n.x-goal.x) + abs(n.y-goal.y))
354 h += fabs(a.y - b.y);
355 h *= pathlib_gridsize;
361 This heuristic consider both stright and disagonal moves
362 to have teh same cost.
364 float pathlib_h_diagonal(vector a,vector b)
366 //h(n) = D * max(abs(n.x-goal.x), abs(n.y-goal.y))
371 h = pathlib_movecost * max(x,y);
377 This heuristic only considers the stright line distance.
378 Will usualy mean a lower H then G meaning A* Will speand more
381 float pathlib_h_euclidean(vector a,vector b)
387 This heuristic consider both stright and disagonal moves,
388 But has a separate cost for diagonal moves.
390 float pathlib_h_diagonal2(vector a,vector b)
392 float h_diag,h_str,h,x,y;
395 h_diagonal(n) = min(abs(n.x-goal.x), abs(n.y-goal.y))
396 h_straight(n) = (abs(n.x-goal.x) + abs(n.y-goal.y))
397 h(n) = D2 * h_diagonal(n) + D * (h_straight(n) - 2*h_diagonal(n)))
406 h = pathlib_movecost_diag * h_diag;
407 h += pathlib_movecost * (h_str - 2 * h_diag);
413 This heuristic consider both stright and disagonal moves,
414 But has a separate cost for diagonal moves.
418 float pathlib_h_diagonal2sdp(vector preprev,vector prev,vector point,vector end)
420 float h_diag,h_str,h,x,y,z;
422 //h_diagonal(n) = min(abs(n.x-goal.x), abs(n.y-goal.y))
423 //h_straight(n) = (abs(n.x-goal.x) + abs(n.y-goal.y))
424 //h(n) = D2 * h_diagonal(n) + D * (h_straight(n) - 2*h_diagonal(n)))
426 x = fabs(point.x - end.x);
427 y = fabs(point.y - end.y);
428 z = fabs(point.z - end.z);
430 h_diag = min3(x,y,z);
433 h = pathlib_movecost_diag * h_diag;
434 h += pathlib_movecost * (h_str - 2 * h_diag);
439 d1 = normalize(preprev - point);
440 d2 = normalize(prev - point);
442 //bprint("pathlib_h_diagonal2sdp-M = ",ftos(m),"\n");
448 float pathlib_h_diagonal3(vector a,vector b)
450 float h_diag,h_str,h,x,y,z;
452 //h_diagonal(n) = min(abs(n.x-goal.x), abs(n.y-goal.y))
453 //h_straight(n) = (abs(n.x-goal.x) + abs(n.y-goal.y))
454 //h(n) = D2 * h_diagonal(n) + D * (h_straight(n) - 2*h_diagonal(n)))
460 h_diag = min3(x,y,z);
463 h = pathlib_movecost_diag * h_diag;
464 h += pathlib_movecost * (h_str - 2 * h_diag);
469 const float PATHLIB_NODEEXPIRE = 0.05;
470 float pathlib_scraplist_cnt;
474 #ifdef PATHLIB_USE_NODESCRAP
475 if(pathlib_scraplist_cnt)
477 n = findentity(world,owner,scraplist);
480 --pathlib_scraplist_cnt;
481 ++pathlib_recycle_cnt;
485 pathlib_scraplist_cnt = 0;
490 n.think = SUB_Remove;
491 n.nextthink = time + PATHLIB_NODEEXPIRE;
495 void dumpnode(entity n)
497 #ifdef PATHLIB_USE_NODESCRAP
498 ++pathlib_scraplist_cnt;
502 n.is_path_node = false;
505 //n.is_path_node = false;
506 n.think = SUB_Remove;
511 entity pathlib_mknode(vector where,entity parent)
516 node.is_path_node = true;
517 node.owner = openlist;
518 node.path_prev = parent;
520 setorigin(node, where);
524 node.medium = pointcontents(where);
529 var float pathlib_expandnode(entity node, vector start, vector goal);
530 float pathlib_expandnode_star(entity node, vector start, vector goal);
531 float pathlib_expandnode_box(entity node, vector start, vector goal);
533 var float pathlib_makenode(entity parent,vector start, vector to, vector goal,float cost);
534 float pathlib_makenode_adaptive(entity parent,vector start, vector to, vector goal,float cost)
540 ++pathlib_searched_cnt;
542 if(inwater(parent.origin))
544 pathlib_expandnode = pathlib_expandnode_box;
545 pathlib_movenode = pathlib_swimnode;
552 pathlib_expandnode = pathlib_expandnode_box;
553 pathlib_movenode = pathlib_swimnode;
559 pathlib_expandnode = pathlib_expandnode_star;
560 pathlib_movenode = pathlib_walknode;
565 where = pathlib_movenode(parent.origin,to,0);
566 if (!pathlib_movenode_goodnode)
570 if(edge_check(where,pathlib_edge_check_size))
574 pathlib_h_diagonal2sdp(parent.path_prev.origin,parent.origin,where,goal);
576 h = pathlib_heuristic(where,goal);
577 g = pathlib_cost(parent,where,cost);
580 node = findradius(where,pathlib_gridsize * 0.75);
583 if(node.is_path_node == true)
586 if(node.owner == openlist)
588 if(node.pathlib_node_g > g)
590 node.pathlib_node_h = h;
591 node.pathlib_node_g = g;
592 node.pathlib_node_f = f;
593 node.path_prev = parent;
597 best_open_node = node;
598 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
599 best_open_node = node;
607 node = pathlib_mknode(where,parent);
608 node.pathlib_node_h = h;
609 node.pathlib_node_g = g;
610 node.pathlib_node_f = f;
613 best_open_node = node;
614 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
615 best_open_node = node;
620 entity pathlib_getbestopen()
627 ++pathlib_bestcash_hits;
628 pathlib_bestcash_saved += pathlib_open_cnt;
630 return best_open_node;
633 node = findchainentity(owner,openlist);
640 ++pathlib_bestopen_seached;
641 if(node.pathlib_node_f < bestnode.pathlib_node_f)
650 void pathlib_close_node(entity node,vector goal)
653 if(node.owner == closedlist)
655 dprint("Pathlib: Tried to close a closed node!\n");
659 if(node == best_open_node)
660 best_open_node = world;
662 ++pathlib_closed_cnt;
665 node.owner = closedlist;
667 if(vlen(node.origin - goal) <= pathlib_gridsize)
671 goalmove = pathlib_walknode(node.origin,goal,1);
672 if(pathlib_movenode_goodnode)
675 pathlib_foundgoal = true;
680 float pathlib_expandnode_star(entity node, vector start, vector goal)
692 point = where + v_forward * pathlib_gridsize;
693 nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
696 point = where - v_forward * pathlib_gridsize;
697 nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
700 point = where + v_right * pathlib_gridsize;
701 nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
704 point = where - v_right * pathlib_gridsize;
705 nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
708 point = where + v_forward * pathlib_gridsize + v_right * pathlib_gridsize;
709 nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
712 point = where + v_forward * pathlib_gridsize - v_right * pathlib_gridsize;
713 nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
716 point = where - v_forward * pathlib_gridsize + v_right * pathlib_gridsize;
717 nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
720 point = where - v_forward * pathlib_gridsize - v_right * pathlib_gridsize;
721 nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
723 return pathlib_open_cnt;
726 float pathlib_expandnode_box(entity node, vector start, vector goal)
730 for(v.z = node.origin.z - pathlib_gridsize; v.z <= node.origin.z + pathlib_gridsize; v.z += pathlib_gridsize)
731 for(v.y = node.origin.y - pathlib_gridsize; v.y <= node.origin.y + pathlib_gridsize; v.y += pathlib_gridsize)
732 for(v.x = node.origin.x - pathlib_gridsize; v.x <= node.origin.x + pathlib_gridsize; v.x += pathlib_gridsize)
734 if(vlen(v - node.origin))
735 pathlib_makenode(node,start,v,goal,pathlib_movecost);
738 return pathlib_open_cnt;
741 void pathlib_cleanup()
745 node = findfloat(world,is_path_node, true);
749 node = findfloat(node,is_path_node, true);
758 best_open_node = world;
763 var float buildpath_nodefilter(vector n,vector c,vector p);
764 float buildpath_nodefilter_directional(vector n,vector c,vector p)
768 d2 = normalize(p - c);
769 d1 = normalize(c - n);
771 if(vlen(d1-d2) < 0.25)
777 float buildpath_nodefilter_moveskip(vector n,vector c,vector p)
779 pathlib_walknode(p,n,1);
780 if(pathlib_movenode_goodnode)
786 entity path_build(entity next, vector where, entity prev, entity start)
791 if(buildpath_nodefilter)
792 if(buildpath_nodefilter(next.origin,where,prev.origin))
798 path.path_next = next;
800 setorigin(path,where);
803 path.classname = "path_end";
807 path.classname = "path_start";
809 path.classname = "path_node";
815 entity pathlib_astar(vector from,vector to)
817 entity path, start, end, open, n, ln;
818 float ptime, ftime, ctime;
820 ptime = gettime(GETTIME_REALTIME);
824 // Select water<->land capable node make/link
825 pathlib_makenode = pathlib_makenode_adaptive;
826 // Select XYZ cost estimate
827 pathlib_heuristic = pathlib_h_diagonal3;
828 // Select distance + waterfactor cost
829 pathlib_cost = pathlib_g_euclidean_water;
830 // Select star expander
831 pathlib_expandnode = pathlib_expandnode_star;
832 // Select walk simulation movement test
833 pathlib_movenode = pathlib_walknode;
834 // Filter final nodes by direction
835 buildpath_nodefilter = buildpath_nodefilter_directional;
837 // If the start is in water we need diffrent settings
840 // Select volumetric node expaner
841 pathlib_expandnode = pathlib_expandnode_box;
843 // Water movement test
844 pathlib_movenode = pathlib_swimnode;
851 closedlist = spawn();
856 pathlib_closed_cnt = 0;
857 pathlib_open_cnt = 0;
858 pathlib_made_cnt = 0;
859 pathlib_merge_cnt = 0;
860 pathlib_searched_cnt = 0;
861 pathlib_bestopen_seached = 0;
862 pathlib_bestcash_hits = 0;
863 pathlib_bestcash_saved = 0;
864 pathlib_recycle_cnt = 0;
866 pathlib_gridsize = 128;
867 pathlib_movecost = pathlib_gridsize;
868 pathlib_movecost_diag = vlen(('1 1 0' * pathlib_gridsize));
869 pathlib_movecost_waterfactor = 1.1;
870 pathlib_foundgoal = 0;
872 walknode_boxmax = self.maxs * 1.5;
873 walknode_boxmin = self.mins * 1.5;
875 pathlib_edge_check_size = (vlen(walknode_boxmin - walknode_boxmax) * 0.5);
877 walknode_boxup = '0 0 2' * self.maxs.z;
878 walknode_stepsize = 32;
879 walknode_stepup = '0 0 1' * walknode_stepsize;
880 walknode_maxdrop = '0 0 3' * walknode_stepsize;
882 from.x = fsnap(from.x,pathlib_gridsize);
883 from.y = fsnap(from.y,pathlib_gridsize);
885 to.x = fsnap(to.x,pathlib_gridsize);
886 to.y = fsnap(to.y,pathlib_gridsize);
888 dprint("AStar init. ", ftos(pathlib_scraplist_cnt), " nodes on scrap\n");
889 path = pathlib_mknode(from,world);
890 pathlib_close_node(path,to);
891 if(pathlib_foundgoal)
893 dprint("AStar: Goal found on first node!\n");
897 open.classname = "path_end";
898 setorigin(open,path.origin);
905 if(pathlib_expandnode(path,from,to) <= 0)
907 dprint("AStar path fail.\n");
913 best_open_node = pathlib_getbestopen();
915 pathlib_close_node(best_open_node,to);
916 if(inwater(n.origin))
917 pathlib_expandnode_box(n,from,to);
919 pathlib_expandnode_star(n,from,to);
921 while(pathlib_open_cnt)
923 best_open_node = pathlib_getbestopen();
925 pathlib_close_node(best_open_node,to);
927 if(inwater(n.origin))
928 pathlib_expandnode_box(n,from,to);
930 pathlib_expandnode(n,from,to);
932 if(pathlib_foundgoal)
934 dprint("Target found. Rebuilding and filtering path...\n");
935 ftime = gettime(GETTIME_REALTIME);
936 ptime = ftime - ptime;
938 start = path_build(world,path.origin,world,world);
939 end = path_build(world,goal_node.origin,world,start);
943 for(open = goal_node; open.path_prev != path; open = open.path_prev)
945 n = path_build(ln,open.origin,open.path_prev,start);
951 ftime = gettime(GETTIME_REALTIME) - ftime;
953 ctime = gettime(GETTIME_REALTIME);
955 ctime = gettime(GETTIME_REALTIME) - ctime;
959 pathlib_showpath2(start);
961 dprint("Time used - pathfinding: ", ftos(ptime),"\n");
962 dprint("Time used - rebuild & filter: ", ftos(ftime),"\n");
963 dprint("Time used - cleanup: ", ftos(ctime),"\n");
964 dprint("Time used - total: ", ftos(ptime + ftime + ctime),"\n");
965 dprint("Time used - # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)),"\n\n");
966 dprint("Nodes - created: ", ftos(pathlib_made_cnt),"\n");
967 dprint("Nodes - open: ", ftos(pathlib_open_cnt),"\n");
968 dprint("Nodes - merged: ", ftos(pathlib_merge_cnt),"\n");
969 dprint("Nodes - closed: ", ftos(pathlib_closed_cnt),"\n");
970 dprint("Nodes - searched: ", ftos(pathlib_searched_cnt),"\n");
972 if(pathlib_recycle_cnt)
973 dprint("Nodes - make/reuse: ", ftos(pathlib_made_cnt / pathlib_recycle_cnt),"\n");
974 if(pathlib_recycle_cnt)
975 dprint("Nodes - reused: ", ftos(pathlib_recycle_cnt),"\n");
977 dprint("Nodes bestopen searched: ", ftos(pathlib_bestopen_seached),"\n");
978 dprint("Nodes bestcash - hits: ", ftos(pathlib_bestcash_hits),"\n");
979 dprint("Nodes bestcash - save: ", ftos(pathlib_bestcash_saved),"\n");
980 dprint("AStar done. ", ftos(pathlib_scraplist_cnt), " nodes on scrap\n\n");
986 dprint("A* Faild to find a path! Try a smaller gridsize.\n");