4 #define medium spawnshieldtime
8 float edge_show(vector point,float fsize);
9 void mark_error(vector where,float lifetime);
10 void mark_info(vector where,float lifetime);
11 entity mark_misc(vector where,float lifetime);
13 void pathlib_showpath(entity start)
19 te_lightning1(e,e.origin,e.path_next.origin);
26 pathlib_showpath(self);
27 self.nextthink = time + 1;
30 void __showpath2_think()
32 mark_info(self.origin,1);
35 self.path_next.think = __showpath2_think;
36 self.path_next.nextthink = time + 0.15;
40 self.owner.think = __showpath2_think;
41 self.owner.nextthink = time + 0.15;
45 void pathlib_showpath2(entity path)
47 path.think = __showpath2_think;
48 path.nextthink = time;
53 void pathlib_deletepath(entity start)
57 e = findchainentity(owner, start);
66 float walknode_stepsize;
67 vector walknode_stepup;
68 vector walknode_maxdrop;
69 vector walknode_boxup;
70 vector walknode_boxmax;
71 vector walknode_boxmin;
72 float pathlib_movenode_goodnode;
74 float floor_ok(vector point)
78 if(trace_dphitq3surfaceflags & Q3SURFACEFLAG_SKY)
81 pc = pointcontents(point);
91 if(pointcontents(point - '0 0 1') != CONTENT_SOLID)
97 if(pointcontents(point - '0 0 1') == CONTENT_SOLID)
103 #define _pcheck(p) traceline(p+z_up,p-z_down,MOVE_WORLDONLY,self); if (!floor_ok(trace_endpos)) return 1
104 float edge_check(vector point,float fsize)
108 z_up = '0 0 1' * fsize;
109 z_down = '0 0 1' * fsize;
111 _pcheck(point + ('1 1 0' * fsize));
112 _pcheck(point + ('1 -1 0' * fsize));
113 _pcheck(point + ('1 0 0' * fsize));
115 _pcheck(point + ('0 1 0' * fsize));
116 _pcheck(point + ('0 -1 0' * fsize));
118 _pcheck(point + ('-1 0 0' * fsize));
119 _pcheck(point + ('-1 1 0' * fsize));
120 _pcheck(point + ('-1 -1 0' * fsize));
126 #define _pshow(p) mark_error(p,10)
127 float edge_show(vector point,float fsize)
130 _pshow(point + ('1 1 0' * fsize));
131 _pshow(point + ('1 -1 0' * fsize));
132 _pshow(point + ('1 0 0' * fsize));
134 _pshow(point + ('0 1 0' * fsize));
135 _pshow(point + ('0 -1 0' * fsize));
137 _pshow(point + ('-1 0 0' * fsize));
138 _pshow(point + ('-1 1 0' * fsize));
139 _pshow(point + ('-1 -1 0' * fsize));
145 var vector pathlib_movenode(vector start,vector end,float doedge);
146 vector pathlib_wateroutnode(vector start,vector end,float doedge)
150 pathlib_movenode_goodnode = 0;
152 end.x = fsnap(end.x, pathlib_gridsize);
153 end.y = fsnap(end.y, pathlib_gridsize);
155 traceline(end + ('0 0 0.25' * pathlib_gridsize),end - ('0 0 1' * pathlib_gridsize),MOVE_WORLDONLY,self);
158 if(pointcontents(end - '0 0 1') != CONTENT_SOLID)
161 for(surface = start ; surface.z < (end.z + 32); ++surface.z)
163 if(pointcontents(surface) == CONTENT_EMPTY)
167 if(pointcontents(surface + '0 0 1') != CONTENT_EMPTY)
170 tracebox(start + '0 0 64', walknode_boxmin,walknode_boxmax, end + '0 0 64', MOVE_WORLDONLY, self);
171 if(trace_fraction == 1)
172 pathlib_movenode_goodnode = 1;
174 if(fabs(surface.z - end.z) > 32)
175 pathlib_movenode_goodnode = 0;
180 vector pathlib_swimnode(vector start,vector end,float doedge)
182 pathlib_movenode_goodnode = 0;
184 if(pointcontents(start) != CONTENT_WATER)
187 end.x = fsnap(end.x, pathlib_gridsize);
188 end.y = fsnap(end.y, pathlib_gridsize);
190 if(pointcontents(end) == CONTENT_EMPTY)
191 return pathlib_wateroutnode( start, end, doedge);
193 tracebox(start, walknode_boxmin,walknode_boxmax, end, MOVE_WORLDONLY, self);
194 if(trace_fraction == 1)
195 pathlib_movenode_goodnode = 1;
200 vector pathlib_flynode(vector start,vector end)
202 pathlib_movenode_goodnode = 0;
204 end.x = fsnap(end.x, pathlib_gridsize);
205 end.y = fsnap(end.y, pathlib_gridsize);
207 tracebox(start, walknode_boxmin,walknode_boxmax, end, MOVE_WORLDONLY, self);
208 if(trace_fraction == 1)
209 pathlib_movenode_goodnode = 1;
214 vector pathlib_walknode(vector start,vector end,float doedge)
216 vector direction,point,last_point,s,e;
217 float steps, distance, i,laststep;
219 pathlib_movenode_goodnode = 0;
225 direction = normalize(s - e);
227 distance = vlen(start - end);
228 laststep = distance / walknode_stepsize;
229 steps = floor(laststep);
230 laststep = laststep - steps;
233 s = point + walknode_stepup;
234 e = point - walknode_maxdrop;
236 traceline(s, e,MOVE_WORLDONLY,self);
237 if(trace_fraction == 1.0)
240 if (floor_ok(trace_endpos) == 0)
243 last_point = trace_endpos;
245 for(i = 0; i < steps; ++i)
247 point = last_point + direction * walknode_stepsize;
249 s = point + walknode_stepup;
250 e = point - walknode_maxdrop;
251 traceline(s, e,MOVE_WORLDONLY,self);
252 if(trace_fraction == 1.0)
255 point = trace_endpos;
256 if (!floor_ok(trace_endpos))
259 tracebox(last_point + walknode_boxup, walknode_boxmin,walknode_boxmax, point + walknode_boxup, MOVE_WORLDONLY, self);
260 if(trace_fraction != 1.0)
264 if(edge_check(point,pathlib_edge_check_size))
270 point = last_point + direction * walknode_stepsize * laststep;
272 point.x = fsnap(point.x, pathlib_gridsize);
273 point.y = fsnap(point.y, pathlib_gridsize);
275 s = point + walknode_stepup;
276 e = point - walknode_maxdrop;
277 traceline(s, e,MOVE_WORLDONLY,self);
279 if(trace_fraction == 1.0)
282 point = trace_endpos;
284 if (!floor_ok(trace_endpos))
287 tracebox(last_point + walknode_boxup, walknode_boxmin,walknode_boxmax, point + walknode_boxup, MOVE_WORLDONLY, self);
288 if(trace_fraction != 1.0)
291 pathlib_movenode_goodnode = 1;
295 var float pathlib_cost(entity parent,vector to, float static_cost);
296 float pathlib_g_static(entity parent,vector to, float static_cost)
299 return parent.pathlib_node_g + static_cost * pathlib_movecost_waterfactor;
301 return parent.pathlib_node_g + static_cost;
304 float pathlib_g_static_water(entity parent,vector to, float static_cost)
307 return parent.pathlib_node_g + static_cost * pathlib_movecost_waterfactor;
309 return parent.pathlib_node_g + static_cost;
312 float pathlib_g_euclidean(entity parent,vector to, float static_cost)
314 return parent.pathlib_node_g + vlen(parent.origin - to);
316 float pathlib_g_euclidean_water(entity parent,vector to, float static_cost)
319 return parent.pathlib_node_g + vlen(parent.origin - to) * pathlib_movecost_waterfactor;
321 return parent.pathlib_node_g + vlen(parent.origin - to);
324 var float(vector from,vector to) pathlib_heuristic;
327 Manhattan Menas we expect to move up,down left or right
328 No diagonal moves espected. (like moving bewteen city blocks)
330 float pathlib_h_manhattan(vector a,vector b)
332 //h(n) = D * (abs(n.x-goal.x) + abs(n.y-goal.y))
336 h += fabs(a.y - b.y);
337 h *= pathlib_gridsize;
343 This heuristic consider both stright and disagonal moves
344 to have teh same cost.
346 float pathlib_h_diagonal(vector a,vector b)
348 //h(n) = D * max(abs(n.x-goal.x), abs(n.y-goal.y))
353 h = pathlib_movecost * max(x,y);
359 This heuristic only considers the stright line distance.
360 Will usualy mean a lower H then G meaning A* Will speand more
363 float pathlib_h_euclidean(vector a,vector b)
369 This heuristic consider both stright and disagonal moves,
370 But has a separate cost for diagonal moves.
372 float pathlib_h_diagonal2(vector a,vector b)
374 float h_diag,h_str,h,x,y;
377 h_diagonal(n) = min(abs(n.x-goal.x), abs(n.y-goal.y))
378 h_straight(n) = (abs(n.x-goal.x) + abs(n.y-goal.y))
379 h(n) = D2 * h_diagonal(n) + D * (h_straight(n) - 2*h_diagonal(n)))
388 h = pathlib_movecost_diag * h_diag;
389 h += pathlib_movecost * (h_str - 2 * h_diag);
395 This heuristic consider both stright and disagonal moves,
396 But has a separate cost for diagonal moves.
400 float pathlib_h_diagonal2sdp(vector preprev,vector prev,vector point,vector end)
402 float h_diag,h_str,h,x,y,z;
404 //h_diagonal(n) = min(abs(n.x-goal.x), abs(n.y-goal.y))
405 //h_straight(n) = (abs(n.x-goal.x) + abs(n.y-goal.y))
406 //h(n) = D2 * h_diagonal(n) + D * (h_straight(n) - 2*h_diagonal(n)))
408 x = fabs(point.x - end.x);
409 y = fabs(point.y - end.y);
410 z = fabs(point.z - end.z);
412 h_diag = min3(x,y,z);
415 h = pathlib_movecost_diag * h_diag;
416 h += pathlib_movecost * (h_str - 2 * h_diag);
421 d1 = normalize(preprev - point);
422 d2 = normalize(prev - point);
424 //bprint("pathlib_h_diagonal2sdp-M = ",ftos(m),"\n");
430 float pathlib_h_diagonal3(vector a,vector b)
432 float h_diag,h_str,h,x,y,z;
434 //h_diagonal(n) = min(abs(n.x-goal.x), abs(n.y-goal.y))
435 //h_straight(n) = (abs(n.x-goal.x) + abs(n.y-goal.y))
436 //h(n) = D2 * h_diagonal(n) + D * (h_straight(n) - 2*h_diagonal(n)))
442 h_diag = min3(x,y,z);
445 h = pathlib_movecost_diag * h_diag;
446 h += pathlib_movecost * (h_str - 2 * h_diag);
451 const float PATHLIB_NODEEXPIRE = 0.05;
452 float pathlib_scraplist_cnt;
456 #ifdef PATHLIB_USE_NODESCRAP
457 if(pathlib_scraplist_cnt)
459 n = findentity(world,owner,scraplist);
462 --pathlib_scraplist_cnt;
463 ++pathlib_recycle_cnt;
467 pathlib_scraplist_cnt = 0;
472 n.think = SUB_Remove;
473 n.nextthink = time + PATHLIB_NODEEXPIRE;
477 void dumpnode(entity n)
479 #ifdef PATHLIB_USE_NODESCRAP
480 ++pathlib_scraplist_cnt;
484 n.is_path_node = false;
487 //n.is_path_node = false;
488 n.think = SUB_Remove;
493 entity pathlib_mknode(vector where,entity parent)
498 node.is_path_node = true;
499 node.owner = openlist;
500 node.path_prev = parent;
502 setorigin(node, where);
506 node.medium = pointcontents(where);
511 var float pathlib_expandnode(entity node, vector start, vector goal);
512 float pathlib_expandnode_star(entity node, vector start, vector goal);
513 float pathlib_expandnode_box(entity node, vector start, vector goal);
515 var float pathlib_makenode(entity parent,vector start, vector to, vector goal,float cost);
516 float pathlib_makenode_adaptive(entity parent,vector start, vector to, vector goal,float cost)
522 ++pathlib_searched_cnt;
524 if(inwater(parent.origin))
526 pathlib_expandnode = pathlib_expandnode_box;
527 pathlib_movenode = pathlib_swimnode;
534 pathlib_expandnode = pathlib_expandnode_box;
535 pathlib_movenode = pathlib_swimnode;
541 pathlib_expandnode = pathlib_expandnode_star;
542 pathlib_movenode = pathlib_walknode;
547 where = pathlib_movenode(parent.origin,to,0);
548 if (!pathlib_movenode_goodnode)
552 if(edge_check(where,pathlib_edge_check_size))
556 pathlib_h_diagonal2sdp(parent.path_prev.origin,parent.origin,where,goal);
558 h = pathlib_heuristic(where,goal);
559 g = pathlib_cost(parent,where,cost);
562 node = findradius(where,pathlib_gridsize * 0.75);
565 if(node.is_path_node == true)
568 if(node.owner == openlist)
570 if(node.pathlib_node_g > g)
572 node.pathlib_node_h = h;
573 node.pathlib_node_g = g;
574 node.pathlib_node_f = f;
575 node.path_prev = parent;
579 best_open_node = node;
580 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
581 best_open_node = node;
589 node = pathlib_mknode(where,parent);
590 node.pathlib_node_h = h;
591 node.pathlib_node_g = g;
592 node.pathlib_node_f = f;
595 best_open_node = node;
596 else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
597 best_open_node = node;
602 entity pathlib_getbestopen()
609 ++pathlib_bestcash_hits;
610 pathlib_bestcash_saved += pathlib_open_cnt;
612 return best_open_node;
615 node = findchainentity(owner,openlist);
622 ++pathlib_bestopen_seached;
623 if(node.pathlib_node_f < bestnode.pathlib_node_f)
632 void pathlib_close_node(entity node,vector goal)
635 if(node.owner == closedlist)
637 LOG_TRACE("Pathlib: Tried to close a closed node!\n");
641 if(node == best_open_node)
642 best_open_node = world;
644 ++pathlib_closed_cnt;
647 node.owner = closedlist;
649 if(vlen(node.origin - goal) <= pathlib_gridsize)
653 goalmove = pathlib_walknode(node.origin,goal,1);
654 if(pathlib_movenode_goodnode)
657 pathlib_foundgoal = true;
662 float pathlib_expandnode_star(entity node, vector start, vector goal)
674 point = where + v_forward * pathlib_gridsize;
675 nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
678 point = where - v_forward * pathlib_gridsize;
679 nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
682 point = where + v_right * pathlib_gridsize;
683 nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
686 point = where - v_right * pathlib_gridsize;
687 nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost);
690 point = where + v_forward * pathlib_gridsize + v_right * pathlib_gridsize;
691 nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
694 point = where + v_forward * pathlib_gridsize - v_right * pathlib_gridsize;
695 nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
698 point = where - v_forward * pathlib_gridsize + v_right * pathlib_gridsize;
699 nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
702 point = where - v_forward * pathlib_gridsize - v_right * pathlib_gridsize;
703 nodecnt += pathlib_makenode(node,start,point,goal,pathlib_movecost_diag);
705 return pathlib_open_cnt;
708 float pathlib_expandnode_box(entity node, vector start, vector goal)
712 for(v.z = node.origin.z - pathlib_gridsize; v.z <= node.origin.z + pathlib_gridsize; v.z += pathlib_gridsize)
713 for(v.y = node.origin.y - pathlib_gridsize; v.y <= node.origin.y + pathlib_gridsize; v.y += pathlib_gridsize)
714 for(v.x = node.origin.x - pathlib_gridsize; v.x <= node.origin.x + pathlib_gridsize; v.x += pathlib_gridsize)
716 if(vlen(v - node.origin))
717 pathlib_makenode(node,start,v,goal,pathlib_movecost);
720 return pathlib_open_cnt;
723 void pathlib_cleanup()
727 node = findfloat(world,is_path_node, true);
731 node = findfloat(node,is_path_node, true);
740 best_open_node = world;
745 var float buildpath_nodefilter(vector n,vector c,vector p);
746 float buildpath_nodefilter_directional(vector n,vector c,vector p)
750 d2 = normalize(p - c);
751 d1 = normalize(c - n);
753 if(vlen(d1-d2) < 0.25)
759 float buildpath_nodefilter_moveskip(vector n,vector c,vector p)
761 pathlib_walknode(p,n,1);
762 if(pathlib_movenode_goodnode)
768 entity path_build(entity next, vector where, entity prev, entity start)
773 if(buildpath_nodefilter)
774 if(buildpath_nodefilter(next.origin,where,prev.origin))
780 path.path_next = next;
782 setorigin(path,where);
785 path.classname = "path_end";
789 path.classname = "path_start";
791 path.classname = "path_node";
797 entity pathlib_astar(vector from,vector to)
799 entity path, start, end, open, n, ln;
800 float ptime, ftime, ctime;
802 ptime = gettime(GETTIME_REALTIME);
806 // Select water<->land capable node make/link
807 pathlib_makenode = pathlib_makenode_adaptive;
808 // Select XYZ cost estimate
809 pathlib_heuristic = pathlib_h_diagonal3;
810 // Select distance + waterfactor cost
811 pathlib_cost = pathlib_g_euclidean_water;
812 // Select star expander
813 pathlib_expandnode = pathlib_expandnode_star;
814 // Select walk simulation movement test
815 pathlib_movenode = pathlib_walknode;
816 // Filter final nodes by direction
817 buildpath_nodefilter = buildpath_nodefilter_directional;
819 // If the start is in water we need diffrent settings
822 // Select volumetric node expaner
823 pathlib_expandnode = pathlib_expandnode_box;
825 // Water movement test
826 pathlib_movenode = pathlib_swimnode;
833 closedlist = spawn();
838 pathlib_closed_cnt = 0;
839 pathlib_open_cnt = 0;
840 pathlib_made_cnt = 0;
841 pathlib_merge_cnt = 0;
842 pathlib_searched_cnt = 0;
843 pathlib_bestopen_seached = 0;
844 pathlib_bestcash_hits = 0;
845 pathlib_bestcash_saved = 0;
846 pathlib_recycle_cnt = 0;
848 pathlib_gridsize = 128;
849 pathlib_movecost = pathlib_gridsize;
850 pathlib_movecost_diag = vlen(('1 1 0' * pathlib_gridsize));
851 pathlib_movecost_waterfactor = 1.1;
852 pathlib_foundgoal = 0;
854 walknode_boxmax = self.maxs * 1.5;
855 walknode_boxmin = self.mins * 1.5;
857 pathlib_edge_check_size = (vlen(walknode_boxmin - walknode_boxmax) * 0.5);
859 walknode_boxup = '0 0 2' * self.maxs.z;
860 walknode_stepsize = 32;
861 walknode_stepup = '0 0 1' * walknode_stepsize;
862 walknode_maxdrop = '0 0 3' * walknode_stepsize;
864 from.x = fsnap(from.x,pathlib_gridsize);
865 from.y = fsnap(from.y,pathlib_gridsize);
867 to.x = fsnap(to.x,pathlib_gridsize);
868 to.y = fsnap(to.y,pathlib_gridsize);
870 LOG_TRACE("AStar init. ", ftos(pathlib_scraplist_cnt), " nodes on scrap\n");
871 path = pathlib_mknode(from,world);
872 pathlib_close_node(path,to);
873 if(pathlib_foundgoal)
875 LOG_TRACE("AStar: Goal found on first node!\n");
879 open.classname = "path_end";
880 setorigin(open,path.origin);
887 if(pathlib_expandnode(path,from,to) <= 0)
889 LOG_TRACE("AStar path fail.\n");
895 best_open_node = pathlib_getbestopen();
897 pathlib_close_node(best_open_node,to);
898 if(inwater(n.origin))
899 pathlib_expandnode_box(n,from,to);
901 pathlib_expandnode_star(n,from,to);
903 while(pathlib_open_cnt)
905 best_open_node = pathlib_getbestopen();
907 pathlib_close_node(best_open_node,to);
909 if(inwater(n.origin))
910 pathlib_expandnode_box(n,from,to);
912 pathlib_expandnode(n,from,to);
914 if(pathlib_foundgoal)
916 LOG_TRACE("Target found. Rebuilding and filtering path...\n");
917 ftime = gettime(GETTIME_REALTIME);
918 ptime = ftime - ptime;
920 start = path_build(world,path.origin,world,world);
921 end = path_build(world,goal_node.origin,world,start);
925 for(open = goal_node; open.path_prev != path; open = open.path_prev)
927 n = path_build(ln,open.origin,open.path_prev,start);
933 ftime = gettime(GETTIME_REALTIME) - ftime;
935 ctime = gettime(GETTIME_REALTIME);
937 ctime = gettime(GETTIME_REALTIME) - ctime;
941 pathlib_showpath2(start);
943 LOG_TRACE("Time used - pathfinding: ", ftos(ptime),"\n");
944 LOG_TRACE("Time used - rebuild & filter: ", ftos(ftime),"\n");
945 LOG_TRACE("Time used - cleanup: ", ftos(ctime),"\n");
946 LOG_TRACE("Time used - total: ", ftos(ptime + ftime + ctime),"\n");
947 LOG_TRACE("Time used - # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)),"\n\n");
948 LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt),"\n");
949 LOG_TRACE("Nodes - open: ", ftos(pathlib_open_cnt),"\n");
950 LOG_TRACE("Nodes - merged: ", ftos(pathlib_merge_cnt),"\n");
951 LOG_TRACE("Nodes - closed: ", ftos(pathlib_closed_cnt),"\n");
952 LOG_TRACE("Nodes - searched: ", ftos(pathlib_searched_cnt),"\n");
954 if(pathlib_recycle_cnt)
955 LOG_TRACE("Nodes - make/reuse: ", ftos(pathlib_made_cnt / pathlib_recycle_cnt),"\n");
956 if(pathlib_recycle_cnt)
957 LOG_TRACE("Nodes - reused: ", ftos(pathlib_recycle_cnt),"\n");
959 LOG_TRACE("Nodes bestopen searched: ", ftos(pathlib_bestopen_seached),"\n");
960 LOG_TRACE("Nodes bestcash - hits: ", ftos(pathlib_bestcash_hits),"\n");
961 LOG_TRACE("Nodes bestcash - save: ", ftos(pathlib_bestcash_saved),"\n");
962 LOG_TRACE("AStar done. ", ftos(pathlib_scraplist_cnt), " nodes on scrap\n\n");
968 LOG_TRACE("A* Faild to find a path! Try a smaller gridsize.\n");