/* 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 });
}
