public void solve()
{
int total = width * height;
Node[,] prev = new Node[width, height];
int infinity = int.MaxValue - 1;
int[,] distances = new int[width, height];
for (int y = 0; y < height; y++)
{
for (int x = 0; x < width; x++)
{
distances[x, y] = infinity;
}
}
FibHeap unvisited = new FibHeap();
HeapNode[,] nodeindex = new HeapNode[width, height];
distances[Start.X, Start.Y] = 0;
HeapNode startnode = new HeapNode(0, Start);
nodeindex[Start.X, Start.Y] = startnode;
unvisited.insert(startnode);
count = 0;
while (unvisited.count > 0)
{
count++;
HeapNode n = unvisited.minimum();
unvisited.removeminimum();
Node u = n.value;
if (distances[u.X, u.Y] == infinity)
{
break;
}
if (u.X == End.X && u.Y == End.Y)
{
break;
}
foreach (Node v in u.neighbors)
{
if (v.X != 0)
{
if (!visited[v.X, v.Y])
{
int d = MathExt.abs(v.X - u.X) + MathExt.abs(v.Y - u.Y);
int newdistance = distances[u.X, u.Y] + d;
int remaining = MathExt.abs(v.X - End.X) + MathExt.abs(v.Y - End.Y);
if (newdistance < distances[v.X, v.Y])
{
HeapNode vnode = nodeindex[v.X, v.Y];
if (vnode == null)
{
vnode = new HeapNode(newdistance + remaining, v);
unvisited.insert(vnode);
nodeindex[v.X, v.Y] = vnode;
distances[v.X, v.Y] = newdistance;
prev[v.X, v.Y] = u;
}
else
{
unvisited.decreasekey(vnode, newdistance + remaining);
distances[v.X, v.Y] = newdistance;
prev[v.X, v.Y] = u;
}
}
}
}
}
visited[u.X, u.Y] = true;
}
result = new List <Node>();
Node current = End;
while (current.X != 0)
{
result.Insert(0, current);
current = prev[current.X, current.Y];
}
}