488 lines
11 KiB
C
488 lines
11 KiB
C
/* Tricky */
|
|
|
|
#include <astar.h>
|
|
|
|
/* ([ mappos: ({ parent, x, y, g_cost, h_cost, f_cost }), ... ]) */
|
|
static mapping data;
|
|
|
|
/* ([ id: ([ "pos": mappos, "fcost": f ]), ... ]) */
|
|
static mapping binaryHeap;
|
|
|
|
/* Number of items on the binary heap. */
|
|
static int numItems;
|
|
static int maxItems;
|
|
|
|
static int l1, l2;
|
|
|
|
void initHeap()
|
|
{
|
|
l1 = l2 = 0;
|
|
maxItems = numItems = 0;
|
|
binaryHeap = ([ ]);
|
|
}
|
|
|
|
int getHeadHeap() { return binaryHeap[1]["pos"]; }
|
|
|
|
#undef LOG_FIND
|
|
|
|
int findHeap(int d)
|
|
{
|
|
int *nodes;
|
|
int u, v, f;
|
|
int lc, rc;
|
|
|
|
/* Sanity check. */
|
|
if(!numItems) return 0; /* Heap is empty, return 0. */
|
|
|
|
if(member_array(d, keys(data)) == -1) return 0; /* Not come across this item before. */
|
|
|
|
if(numItems == 1)
|
|
{
|
|
|
|
if(d == binaryHeap[1]["pos"]) return 1;
|
|
|
|
return 0;
|
|
}
|
|
|
|
nodes = allocate(numItems + 1);
|
|
nodes[0] = 1;
|
|
|
|
f = data[d][AS_FCOST];
|
|
v = 1;
|
|
|
|
while (v <= numItems)
|
|
{
|
|
nodes[v]++;
|
|
|
|
if(d == binaryHeap[v]["pos"]) break;
|
|
|
|
u = v;
|
|
rc = (lc = u << 1) + 1;
|
|
|
|
if(nodes[u] < 3)
|
|
{
|
|
|
|
if(rc <= numItems)
|
|
{
|
|
|
|
if(nodes[u] == 1 && f >= binaryHeap[lc]["fcost"]) v = lc;
|
|
else
|
|
if(f >= binaryHeap[rc]["fcost"])
|
|
{
|
|
nodes[u] = 2;
|
|
v = rc;
|
|
}
|
|
|
|
nodes[0]++;
|
|
}
|
|
else
|
|
if(nodes[u] == 1 && lc <= numItems)
|
|
{
|
|
|
|
if(f >= binaryHeap[lc]["fcost"]) v = lc;
|
|
|
|
nodes[0]++;
|
|
}
|
|
|
|
}
|
|
|
|
if(nodes[u] == 4)
|
|
{
|
|
v = 0;
|
|
break;
|
|
}
|
|
|
|
if(u == v)
|
|
{
|
|
v >>= 1;
|
|
|
|
if(!v) break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if(v > numItems) v = 0;
|
|
|
|
#ifdef LOG_FIND
|
|
if(l1 < 64 && !v)
|
|
{
|
|
int i, dx;
|
|
|
|
l1++;
|
|
|
|
log_file("HEAP", sprintf("Finding %d(%d). Checks made: %d ... Not Found!\n", d, f, nodes[0]));
|
|
/* log_file("HEAP", sprintf("binaryHeap = %O\nnodes = %O\n", binaryHeap, nodes)); */
|
|
|
|
i = 1;
|
|
|
|
while(i <= numItems)
|
|
{
|
|
string *tmp = ({ });
|
|
string str = "";
|
|
|
|
dx = (80.0 / (float)i);
|
|
|
|
for(int j = i; j < i << 1; j++)
|
|
{
|
|
|
|
if(j <= numItems) tmp += ({ "|" });
|
|
else tmp += ({ "" });
|
|
|
|
}
|
|
|
|
log_file("HEAP", sprintf("%@|"+dx+"s\n", tmp));
|
|
|
|
tmp = ({ });
|
|
tmp = allocate(i);
|
|
|
|
for(int j = i; j < i << 1; j++)
|
|
{
|
|
|
|
if(j <= numItems)
|
|
tmp[j - i] = sprintf("%|"+(dx >> 1)+"'_'d", binaryHeap[j]["fcost"]);
|
|
else tmp[j - i] = "";
|
|
|
|
}
|
|
|
|
i <<= 1;
|
|
|
|
log_file("HEAP", sprintf("%@|"+dx+"s\n", tmp));
|
|
}
|
|
|
|
log_file("HEAP", "\n");
|
|
}
|
|
|
|
if(l2 < 64 && v && numItems > 7)
|
|
{
|
|
int i, dx;
|
|
|
|
l2++;
|
|
|
|
log_file("HEAP", sprintf("Finding %d(%d). Checks made: %d ... Result = %d\n", d, f, nodes[0], v));
|
|
/* log_file("HEAP", sprintf("binaryHeap = %O\nnodes = %O\n", binaryHeap, nodes)); */
|
|
|
|
i = 1;
|
|
|
|
while(i <= numItems)
|
|
{
|
|
string *tmp = ({ });
|
|
string str = "";
|
|
|
|
dx = (80.0 / (float)i);
|
|
|
|
for(int j = i; j < i << 1; j++)
|
|
{
|
|
|
|
if(j <= numItems) tmp += ({ "|" });
|
|
else tmp += ({ "" });
|
|
|
|
}
|
|
|
|
log_file("HEAP", sprintf("%@|"+dx+"s\n", tmp));
|
|
|
|
tmp = ({ });
|
|
tmp = allocate(i);
|
|
|
|
for(int j = i; j < i << 1; j++)
|
|
{
|
|
|
|
if(j <= numItems)
|
|
{
|
|
|
|
if(j == v)
|
|
tmp[j - i] = sprintf("%|"+(dx >> 1)+"'_'s", "["+binaryHeap[j]["fcost"]+"]");
|
|
else
|
|
tmp[j - i] = sprintf("%|"+(dx >> 1)+"'_'d", binaryHeap[j]["fcost"]);
|
|
|
|
}
|
|
else tmp[j - i] = "";
|
|
|
|
}
|
|
|
|
i <<= 1;
|
|
|
|
log_file("HEAP", sprintf("%@|"+dx+"s\n", tmp));
|
|
}
|
|
|
|
log_file("HEAP", "\n");
|
|
}
|
|
#endif
|
|
|
|
return v;
|
|
}
|
|
|
|
void resortHeapDown(int v)
|
|
{
|
|
|
|
while(v <= numItems)
|
|
{
|
|
mapping parent, lchild, rchild;
|
|
int u, lc, rc;
|
|
|
|
u = v;
|
|
|
|
rc = (lc = u << 1) + 1;
|
|
|
|
parent = ([ ]) + binaryHeap[u];
|
|
|
|
if(rc <= numItems)
|
|
{
|
|
lchild = ([ ]) + binaryHeap[lc];
|
|
rchild = ([ ]) + binaryHeap[rc];
|
|
|
|
if(parent["fcost"] > lchild["fcost"] &&
|
|
parent["fcost"] > rchild["fcost"])
|
|
{
|
|
|
|
if(lchild["fcost"] > rchild["fcost"]) v = rc;
|
|
else v = lc;
|
|
|
|
}
|
|
else
|
|
if(parent["fcost"] > lchild["fcost"]) v = lc;
|
|
else
|
|
if(parent["fcost"] > rchild["fcost"]) v = rc;
|
|
|
|
}
|
|
else
|
|
if(lc <= numItems)
|
|
{
|
|
|
|
if(parent["fcost"] > binaryHeap[lc]["fcost"]) v = lc;
|
|
|
|
}
|
|
|
|
if(u == v) break;
|
|
|
|
binaryHeap[u] = ([ ]) + binaryHeap[v];
|
|
binaryHeap[v] = ([ ]) + parent;
|
|
}
|
|
|
|
}
|
|
|
|
void resortHeapUp(int m)
|
|
{
|
|
mapping tmp;
|
|
|
|
while(m > 1)
|
|
{
|
|
int m2 = m >> 1;
|
|
|
|
if (binaryHeap[m]["fcost"] >= binaryHeap[m2]["fcost"]) break;
|
|
|
|
tmp = ([ ]) + binaryHeap[m];
|
|
binaryHeap[m] = ([ ]) + binaryHeap[m2];
|
|
binaryHeap[m2] = ([ ]) + tmp;
|
|
|
|
m = m2;
|
|
}
|
|
|
|
}
|
|
|
|
void addHeap(int d)
|
|
{
|
|
numItems++;
|
|
|
|
if(numItems > maxItems) maxItems = numItems;
|
|
|
|
binaryHeap[numItems] = ([ "pos": d, "fcost": data[d][AS_FCOST] ]);
|
|
|
|
if(numItems > 1) resortHeapUp(numItems);
|
|
|
|
}
|
|
|
|
void removeHeadHeap()
|
|
{
|
|
|
|
if(!numItems) return 0; /* Heap is empty, return 0. */
|
|
|
|
/* Replace the head item with the tail item. */
|
|
if(numItems > 1) binaryHeap[1] = ([ ]) + binaryHeap[numItems];
|
|
|
|
/* Chop off the tail. */
|
|
map_delete(binaryHeap, numItems);
|
|
|
|
if(--numItems < 2) return;
|
|
|
|
resortHeapDown(1);
|
|
|
|
return;
|
|
}
|
|
|
|
int calc_h(int sx, int sy, int ex, int ey)
|
|
{
|
|
int dx, dy;
|
|
|
|
dx = sx - ex;
|
|
dy = sy - ey;
|
|
|
|
return (sqrt(dx * dx + dy * dy) * 0.975);
|
|
|
|
/* Manhattan method. */
|
|
/* return (abs(dx) + abs(dy)); */
|
|
}
|
|
|
|
string make_path(int start, int goal, int goalx, int goaly)
|
|
{
|
|
mapping x_dir, y_dir;
|
|
string *dirs;
|
|
string path;
|
|
int i, oldx, oldy, newx, newy;
|
|
|
|
path = "";
|
|
i = goal;
|
|
oldx = goalx; oldy = goaly;
|
|
x_dir = ([ -1: ({ 3, 4, 5 }), 0: ({ 2, 9, 6 }), 1: ({ 1, 0, 7 }) ]);
|
|
y_dir = ([ -1: ({ 3, 2, 1 }), 0: ({ 4, 9, 0 }), 1: ({ 5, 6, 7 }) ]);
|
|
dirs = ({ "e", "ne", "n", "nw", "w", "sw", "s", "se", "*" });
|
|
|
|
while(i != start)
|
|
{
|
|
int *xd, *yd;
|
|
|
|
i = data[i][AS_PARENT];
|
|
newx = data[i][AS_X];
|
|
newy = data[i][AS_Y];
|
|
|
|
xd = x_dir[oldx - newx];
|
|
yd = y_dir[oldy - newy];
|
|
path = dirs[(xd & yd)[0]] + path;
|
|
|
|
oldx = newx;
|
|
oldy = newy;
|
|
}
|
|
|
|
return path;
|
|
}
|
|
|
|
mixed *find_path(string map, int startx, int starty, int goalx, int goaly, mapping costs)
|
|
{
|
|
string *split_map;
|
|
int *closed;
|
|
int map_width, start, goal;
|
|
int currpos;
|
|
int newx, newy, newpos;
|
|
int oldf, g, h;
|
|
int id;
|
|
|
|
/* Process the map into an easier-to-use format. */
|
|
split_map = explode(map, "\n");
|
|
map_width = 0;
|
|
|
|
/* Find the length of the longest line in the "map" */
|
|
for(int i = sizeof(split_map); i-- ;)
|
|
if(map_width < strlen(split_map[i]))
|
|
map_width = strlen(split_map[i]);
|
|
|
|
/* Make all the lines that length by padding with escape characters.
|
|
* (Note: I use escapes because they are an unlikely character to be
|
|
* chosen for walking over, and unused characters are 'walls') */
|
|
for(int i = sizeof(split_map); i-- ;)
|
|
split_map[i] += sprintf("%" + (map_width - strlen(split_map[i])) + "'\27's", "");
|
|
|
|
/* Sanity checks */
|
|
if(goalx < 0 || goalx >= map_width || goaly < 0 || goaly >= sizeof(split_map))
|
|
return ({ });
|
|
|
|
if(member_array(split_map[starty][startx], keys(costs)) == -1 ||
|
|
member_array(split_map[goaly][goalx], keys(costs)) == -1)
|
|
return ({ });
|
|
|
|
/* Setup initial state. */
|
|
start = startx + starty * map_width;
|
|
goal = goalx + goaly * map_width;
|
|
|
|
data = ([ ]);
|
|
closed = ({ });
|
|
|
|
g = 0;
|
|
h = calc_h(startx, starty, goalx, goaly);
|
|
write("Distance estimate: " + h + "\n");
|
|
|
|
data[start] = ({ -1, startx, starty, g, h, g + h });
|
|
|
|
/* Initialise the binary heap. */
|
|
initHeap();
|
|
addHeap(start);
|
|
|
|
while(numItems && member_array(goal, closed) == -1)
|
|
{
|
|
currpos = getHeadHeap();
|
|
closed += ({ currpos });
|
|
removeHeadHeap();
|
|
|
|
for(int ineighbour = 0; ineighbour < 8; ineighbour += 2)
|
|
{
|
|
newx = (currpos % map_width) + ({ 1, 1, 0, -1, -1, -1, 0, 1 })[ineighbour];
|
|
newy = (currpos / map_width) + ({ 0, -1, -1, -1, 0, 1, 1, 1 })[ineighbour];
|
|
|
|
/* Out of bounds, ignore it. */
|
|
if(newx < 0 || newx >= map_width ||
|
|
newy < 0 || newy >= sizeof(split_map))
|
|
continue;
|
|
|
|
/* Solid wall, ignore it. */
|
|
if(member_array(split_map[newy][newx], keys(costs)) == -1) continue;
|
|
|
|
newpos = newy * map_width + newx;
|
|
/* Already checked, ignore it. */
|
|
if(member_array(newpos, closed) != -1) continue;
|
|
|
|
g = data[currpos][AS_GCOST] + costs[split_map[newy][newx]];
|
|
|
|
if(!(id = findHeap(newpos)))
|
|
{
|
|
h = calc_h(newx, newy, goalx, goaly);
|
|
|
|
data[newpos] = allocate(AS_DATA_SZ);
|
|
data[newpos][AS_PARENT] = currpos;
|
|
data[newpos][AS_X] = newx;
|
|
data[newpos][AS_Y] = newy;
|
|
data[newpos][AS_GCOST] = g;
|
|
data[newpos][AS_HCOST] = h;
|
|
data[newpos][AS_FCOST] = g + h;
|
|
|
|
addHeap(newpos);
|
|
}
|
|
else
|
|
{
|
|
|
|
if(g < data[newpos][AS_GCOST])
|
|
{
|
|
oldf = data[newpos][AS_FCOST];
|
|
|
|
h = calc_h(newx, newy, goalx, goaly);
|
|
|
|
data[newpos][AS_PARENT] = currpos;
|
|
data[newpos][AS_X] = newx;
|
|
data[newpos][AS_Y] = newy;
|
|
data[newpos][AS_GCOST] = g;
|
|
data[newpos][AS_HCOST] = h;
|
|
data[newpos][AS_FCOST] = g + h;
|
|
|
|
if(g + h != oldf)
|
|
{
|
|
binaryHeap[id]["fcost"] = g + h;
|
|
|
|
if(g + h < oldf) resortHeapUp(id);
|
|
else
|
|
if(g + h > oldf) resortHeapDown(id);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
write("Maximum binary heap size: " + maxItems + "\n");
|
|
|
|
if(member_array(goal, closed) == -1) return ({ });
|
|
|
|
return ({ make_path(start, goal, goalx, goaly), data });
|
|
}
|