--- /dev/null
+#include "pathlib.qh"
+#include "utility.qh"
+
+void pathlib_deletepath(entity start)
+{
+ entity e;
+
+ e = findchainentity(owner, start);
+ while(e)
+ {
+ e.think = SUB_Remove;
+ e.nextthink = time;
+ e = e.chain;
+ }
+}
+
+//#define PATHLIB_NODEEXPIRE 0.05
+const float PATHLIB_NODEEXPIRE = 20;
+
+void dumpnode(entity n)
+{
+ n.is_path_node = false;
+ n.think = SUB_Remove;
+ n.nextthink = time;
+}
+
+entity pathlib_mknode(vector where,entity parent)
+{
+ entity node;
+
+ node = pathlib_nodeatpoint(where);
+ if(node)
+ {
+ #ifdef TURRET_DEBUG
+ mark_error(where, 60);
+ #endif
+ return node;
+ }
+
+ node = spawn();
+
+ node.think = SUB_Remove;
+ node.nextthink = time + PATHLIB_NODEEXPIRE;
+ node.is_path_node = true;
+ node.owner = openlist;
+ node.path_prev = parent;
+
+
+ setsize(node, '0 0 0', '0 0 0');
+
+ setorigin(node, where);
+ node.medium = pointcontents(where);
+ pathlib_showsquare(where, 1 ,15);
+
+ ++pathlib_made_cnt;
+ ++pathlib_open_cnt;
+
+ return node;
+}
+
+float pathlib_makenode_adaptive(entity parent,vector start, vector to, vector goal,float cost)
+{
+ entity node;
+ float h,g,f,doedge = 0;
+ vector where;
+
+ ++pathlib_searched_cnt;
+
+ if(inwater(parent.origin))
+ {
+ LOG_TRACE("FromWater\n");
+ pathlib_expandnode = pathlib_expandnode_box;
+ pathlib_movenode = pathlib_swimnode;
+ }
+ else
+ {
+ if(inwater(to))
+ {
+ LOG_TRACE("ToWater\n");
+ pathlib_expandnode = pathlib_expandnode_box;
+ pathlib_movenode = pathlib_walknode;
+ }
+ else
+ {
+ LOG_TRACE("LandToLoand\n");
+ //if(edge_check(parent.origin))
+ // return 0;
+
+ pathlib_expandnode = pathlib_expandnode_star;
+ pathlib_movenode = pathlib_walknode;
+ doedge = 1;
+ }
+ }
+
+ node = pathlib_nodeatpoint(to);
+ if(node)
+ {
+ LOG_TRACE("NodeAtPoint\n");
+ ++pathlib_merge_cnt;
+
+ if(node.owner == openlist)
+ {
+ h = pathlib_heuristic(node.origin,goal);
+ g = pathlib_cost(parent,node.origin,cost);
+ f = g + h;
+
+ if(node.pathlib_node_g > g)
+ {
+ node.pathlib_node_h = h;
+ node.pathlib_node_g = g;
+ node.pathlib_node_f = f;
+
+ node.path_prev = parent;
+ }
+
+ if (!best_open_node)
+ best_open_node = node;
+ else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
+ best_open_node = node;
+ }
+
+ return 1;
+ }
+
+ where = pathlib_movenode(parent.origin, to, 0);
+
+ if (!pathlib_movenode_goodnode)
+ {
+ //pathlib_showsquare(where, 0 ,30);
+ //pathlib_showsquare(parent.origin, 1 ,30);
+ LOG_TRACE("pathlib_movenode_goodnode = 0\n");
+ return 0;
+ }
+
+ //pathlib_showsquare(where, 1 ,30);
+
+ if(pathlib_nodeatpoint(where))
+ {
+ LOG_TRACE("NAP WHERE :",vtos(where),"\n");
+ LOG_TRACE("not NAP TO:",vtos(to),"\n");
+ LOG_TRACE("NAP-NNAP:",ftos(vlen(to-where)),"\n\n");
+ return 0;
+ }
+
+
+ if(doedge)
+ if (!tile_check(where))
+ {
+ LOG_TRACE("tile_check fail\n");
+ pathlib_showsquare(where, 0 ,30);
+ return 0;
+ }
+
+
+ h = pathlib_heuristic(where,goal);
+ g = pathlib_cost(parent,where,cost);
+ f = g + h;
+
+ /*
+ node = findradius(where,pathlib_gridsize * 0.5);
+ while(node)
+ {
+ if(node.is_path_node == true)
+ {
+ ++pathlib_merge_cnt;
+ if(node.owner == openlist)
+ {
+ if(node.pathlib_node_g > g)
+ {
+ //pathlib_movenode(where,node.origin,0);
+ //if(pathlib_movenode_goodnode)
+ //{
+ //mark_error(node.origin + '0 0 128',30);
+ node.pathlib_node_h = h;
+ node.pathlib_node_g = g;
+ node.pathlib_node_f = f;
+ node.path_prev = parent;
+ //}
+ }
+
+ if (!best_open_node)
+ best_open_node = node;
+ else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
+ best_open_node = node;
+ }
+
+ return 1;
+ }
+ node = node.chain;
+ }
+ */
+
+ node = pathlib_mknode(where, parent);
+ node.pathlib_node_h = h;
+ node.pathlib_node_g = g;
+ node.pathlib_node_f = f;
+
+ if (!best_open_node)
+ best_open_node = node;
+ else if(best_open_node.pathlib_node_f > node.pathlib_node_f)
+ best_open_node = node;
+
+ return 1;
+}
+
+entity pathlib_getbestopen()
+{
+ entity node;
+ entity bestnode;
+
+ if(best_open_node)
+ {
+ ++pathlib_bestcash_hits;
+ pathlib_bestcash_saved += pathlib_open_cnt;
+
+ return best_open_node;
+ }
+
+ node = findchainentity(owner,openlist);
+ if(!node)
+ return world;
+
+ bestnode = node;
+ while(node)
+ {
+ ++pathlib_bestopen_seached;
+ if(node.pathlib_node_f < bestnode.pathlib_node_f)
+ bestnode = node;
+
+ node = node.chain;
+ }
+
+ return bestnode;
+}
+
+void pathlib_close_node(entity node,vector goal)
+{
+
+ if(node.owner == closedlist)
+ {
+ LOG_TRACE("Pathlib: Tried to close a closed node!\n");
+ return;
+ }
+
+ if(node == best_open_node)
+ best_open_node = world;
+
+ ++pathlib_closed_cnt;
+ --pathlib_open_cnt;
+
+ node.owner = closedlist;
+
+ if(vlen(node.origin - goal) <= pathlib_gridsize)
+ {
+ vector goalmove;
+
+ goalmove = pathlib_walknode(node.origin,goal,1);
+ if(pathlib_movenode_goodnode)
+ {
+ goal_node = node;
+ pathlib_foundgoal = true;
+ }
+ }
+}
+
+void pathlib_cleanup()
+{
+ best_open_node = world;
+
+ //return;
+
+ entity node;
+
+ node = findfloat(world,is_path_node, true);
+ while(node)
+ {
+ /*
+ node.owner = openlist;
+ node.pathlib_node_g = 0;
+ node.pathlib_node_h = 0;
+ node.pathlib_node_f = 0;
+ node.path_prev = world;
+ */
+
+ dumpnode(node);
+ node = findfloat(node,is_path_node, true);
+ }
+
+ if(openlist)
+ remove(openlist);
+
+ if(closedlist)
+ remove(closedlist);
+
+ openlist = world;
+ closedlist = world;
+
+}
+
+float Cosine_Interpolate(float a, float b, float x)
+{
+ float ft,f;
+
+ ft = x * 3.1415927;
+ f = (1 - cos(ft)) * 0.5;
+
+ return a*(1-f) + b*f;
+}
+
+float buildpath_nodefilter_directional(vector n,vector c,vector p)
+{
+ vector d1,d2;
+
+ d2 = normalize(p - c);
+ d1 = normalize(c - n);
+
+ if(vlen(d1-d2) < 0.25)
+ {
+ //mark_error(c,30);
+ return 1;
+ }
+ //mark_info(c,30);
+ return 0;
+}
+
+float buildpath_nodefilter_moveskip(vector n,vector c,vector p)
+{
+ pathlib_walknode(p,n,1);
+ if(pathlib_movenode_goodnode)
+ return 1;
+
+ return 0;
+}
+
+float buildpath_nodefilter_none(vector n,vector c,vector p)
+{
+ return 0;
+}
+
+entity path_build(entity next, vector where, entity prev, entity start)
+{
+ entity path;
+
+ if(prev && next)
+ if(buildpath_nodefilter)
+ if(buildpath_nodefilter(next.origin,where,prev.origin))
+ return next;
+
+
+ path = spawn();
+ path.owner = start;
+ path.path_next = next;
+
+ setorigin(path,where);
+
+ if(!next)
+ path.classname = "path_end";
+ else
+ {
+ if(!prev)
+ path.classname = "path_start";
+ else
+ path.classname = "path_node";
+ }
+
+ return path;
+}
+
+entity pathlib_astar(vector from,vector to)
+{SELFPARAM();
+ entity path, start, end, open, n, ln;
+ float ptime, ftime, ctime;
+
+ ptime = gettime(GETTIME_REALTIME);
+ pathlib_starttime = ptime;
+
+ pathlib_cleanup();
+
+ // Select water<->land capable node make/link
+ pathlib_makenode = pathlib_makenode_adaptive;
+
+ // Select XYZ cost estimate
+ //pathlib_heuristic = pathlib_h_diagonal3;
+ pathlib_heuristic = pathlib_h_diagonal;
+
+ // Select distance + waterfactor cost
+ pathlib_cost = pathlib_g_euclidean_water;
+
+ // Select star expander
+ pathlib_expandnode = pathlib_expandnode_star;
+
+ // Select walk simulation movement test
+ pathlib_movenode = pathlib_walknode;
+
+ // Filter final nodes by direction
+ buildpath_nodefilter = buildpath_nodefilter_directional;
+
+ // Filter tiles with cross pattern
+ tile_check = tile_check_cross;
+
+ // If the start is in water we need diffrent settings
+ if(inwater(from))
+ {
+ // Select volumetric node expaner
+ pathlib_expandnode = pathlib_expandnode_box;
+
+ // Water movement test
+ pathlib_movenode = pathlib_swimnode;
+ }
+
+ if (!openlist)
+ openlist = spawn();
+
+ if (!closedlist)
+ closedlist = spawn();
+
+ pathlib_closed_cnt = 0;
+ pathlib_open_cnt = 0;
+ pathlib_made_cnt = 0;
+ pathlib_merge_cnt = 0;
+ pathlib_searched_cnt = 0;
+ pathlib_bestopen_seached = 0;
+ pathlib_bestcash_hits = 0;
+ pathlib_bestcash_saved = 0;
+
+ pathlib_gridsize = 128;
+ pathlib_movecost = pathlib_gridsize;
+ pathlib_movecost_diag = vlen(('1 1 0' * pathlib_gridsize));
+ pathlib_movecost_waterfactor = 25000000;
+ pathlib_foundgoal = 0;
+
+ movenode_boxmax = self.maxs * 1.1;
+ movenode_boxmin = self.mins * 1.1;
+
+ movenode_stepsize = pathlib_gridsize * 0.25;
+
+ tile_check_size = pathlib_gridsize * 0.5;
+ tile_check_up = '0 0 2' * pathlib_gridsize;
+ tile_check_up = '0 0 128';
+ tile_check_down = '0 0 3' * pathlib_gridsize;
+ tile_check_down = '0 0 256';
+
+ //movenode_stepup = '0 0 128';
+ movenode_stepup = '0 0 36';
+ movenode_maxdrop = '0 0 512';
+ //movenode_maxdrop = '0 0 512';
+ movenode_boxup = '0 0 72';
+
+ from.x = fsnap(from.x, pathlib_gridsize);
+ from.y = fsnap(from.y, pathlib_gridsize);
+ //from_z += 32;
+
+ to.x = fsnap(to.x, pathlib_gridsize);
+ to.y = fsnap(to.y, pathlib_gridsize);
+ //to_z += 32;
+
+ LOG_TRACE("AStar init\n");
+ path = pathlib_mknode(from, world);
+ pathlib_close_node(path, to);
+ if(pathlib_foundgoal)
+ {
+ LOG_TRACE("AStar: Goal found on first node!\n");
+
+ open = spawn();
+ open.owner = open;
+ open.classname = "path_end";
+ setorigin(open,path.origin);
+
+ pathlib_cleanup();
+
+ return open;
+ }
+
+ if(pathlib_expandnode(path, from, to) <= 0)
+ {
+ LOG_TRACE("AStar path fail.\n");
+ pathlib_cleanup();
+
+ return world;
+ }
+
+ best_open_node = pathlib_getbestopen();
+ n = best_open_node;
+ pathlib_close_node(best_open_node, to);
+ if(inwater(n.origin))
+ pathlib_expandnode_box(n, from, to);
+ else
+ pathlib_expandnode_star(n, from, to);
+
+ while(pathlib_open_cnt)
+ {
+ if((gettime(GETTIME_REALTIME) - pathlib_starttime) > pathlib_maxtime)
+ {
+ LOG_TRACE("Path took to long to compute!\n");
+ LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt),"\n");
+ LOG_TRACE("Nodes - open: ", ftos(pathlib_open_cnt),"\n");
+ LOG_TRACE("Nodes - merged: ", ftos(pathlib_merge_cnt),"\n");
+ LOG_TRACE("Nodes - closed: ", ftos(pathlib_closed_cnt),"\n");
+
+ pathlib_cleanup();
+ return world;
+ }
+
+ best_open_node = pathlib_getbestopen();
+ n = best_open_node;
+ pathlib_close_node(best_open_node,to);
+
+ if(inwater(n.origin))
+ pathlib_expandnode_box(n,from,to);
+ else
+ pathlib_expandnode(n,from,to);
+
+ if(pathlib_foundgoal)
+ {
+ LOG_TRACE("Target found. Rebuilding and filtering path...\n");
+ ftime = gettime(GETTIME_REALTIME);
+ ptime = ftime - ptime;
+
+ start = path_build(world,path.origin,world,world);
+ end = path_build(world,goal_node.origin,world,start);
+ ln = end;
+
+ open = goal_node;
+ for(open = goal_node; open.path_prev != path; open = open.path_prev)
+ {
+ n = path_build(ln,open.origin,open.path_prev,start);
+ ln.path_prev = n;
+ ln = n;
+ }
+ start.path_next = n;
+ n.path_prev = start;
+ ftime = gettime(GETTIME_REALTIME) - ftime;
+
+ ctime = gettime(GETTIME_REALTIME);
+ pathlib_cleanup();
+ ctime = gettime(GETTIME_REALTIME) - ctime;
+
+
+#ifdef DEBUGPATHING
+ pathlib_showpath2(start);
+
+ LOG_TRACE("Time used - pathfinding: ", ftos(ptime),"\n");
+ LOG_TRACE("Time used - rebuild & filter: ", ftos(ftime),"\n");
+ LOG_TRACE("Time used - cleanup: ", ftos(ctime),"\n");
+ LOG_TRACE("Time used - total: ", ftos(ptime + ftime + ctime),"\n");
+ LOG_TRACE("Time used - # frames: ", ftos(ceil((ptime + ftime + ctime) / sys_frametime)),"\n\n");
+ LOG_TRACE("Nodes - created: ", ftos(pathlib_made_cnt),"\n");
+ LOG_TRACE("Nodes - open: ", ftos(pathlib_open_cnt),"\n");
+ LOG_TRACE("Nodes - merged: ", ftos(pathlib_merge_cnt),"\n");
+ LOG_TRACE("Nodes - closed: ", ftos(pathlib_closed_cnt),"\n");
+ LOG_TRACE("Nodes - searched: ", ftos(pathlib_searched_cnt),"\n");
+ LOG_TRACE("Nodes bestopen searched: ", ftos(pathlib_bestopen_seached),"\n");
+ LOG_TRACE("Nodes bestcash - hits: ", ftos(pathlib_bestcash_hits),"\n");
+ LOG_TRACE("Nodes bestcash - save: ", ftos(pathlib_bestcash_saved),"\n");
+ LOG_TRACE("AStar done.\n");
+#endif
+ return start;
+ }
+ }
+
+ LOG_TRACE("A* Faild to find a path! Try a smaller gridsize.\n");
+
+ pathlib_cleanup();
+
+ return world;
+}
projects / xonotic / xonotic-data.pk3dir.git / blobdiff