public List <Node> Solve(Maze maze)
{
int width = maze.width;
int total = maze.width * maze.height;
var start = maze.GetStart();
var startPos = start.Position();
var end = maze.GetEnd();
var endPos = end.Position();
bool[] visited = new bool[total];
Node[] prev = new Node[total];
var infinity = float.PositiveInfinity;
float[] distances = new float[total];
var unvisited = new FibPQ();
FibNode[] nodeIndex = new FibNode[total];
for (var i = 0; i < total; i++)
{
visited[i] = false;
distances[i] = infinity;
}
distances[start.Position().Y *width + start.Position().X] = 0;
var startNode = new FibNode(0, start);
nodeIndex[start.Position().Y *width + start.Position().X] = startNode;
unvisited.insert(startNode);
completed = false;
while (unvisited.len() > 0)
{
var n = unvisited.RemoveMinimum();
var u = n.value;
var uPos = u.Position();
var uPosIndex = uPos.Y * width + uPos.X;
if (distances[uPosIndex] == infinity)
{
break;
}
if (uPos == endPos)
{
completed = true;
break;
}
foreach (var v in u.neighbours)
{
if (v != null)
{
var vPos = v.Position();
var vPosIndex = vPos.Y * width + vPos.X;
if (visited[vPosIndex] == false)
{
var d = (vPos.Y - uPos.Y) + (vPos.X - uPos.X);
var newDistance = distances[uPosIndex] + d;
var remaining = (vPos.Y - endPos.Y) + (vPos.X - endPos.X);
if (newDistance < distances[vPosIndex])
{
var vNode = nodeIndex[vPosIndex];
if (vNode == null)
{
vNode = new FibNode((int)newDistance + remaining, v);
unvisited.insert(vNode);
nodeIndex[vPosIndex] = vNode;
distances[vPosIndex] = newDistance;
prev[vPosIndex] = u;
}
else
{
unvisited.decreaseKey(vNode, (int)newDistance + remaining);
distances[vPosIndex] = newDistance;
prev[vPosIndex] = u;
}
}
}
}
}
visited[uPosIndex] = true;
}
List <Node> path = new List <Node>();
Node current = end;
path.Add(current);
while (current != null)
{
path = DirectionFactory.AppendLeft(path, current);
current = prev[current.Position().Y *width + current.Position().X];
}
return(path);
}
public List <Node> Solve()
{
var width = maze.width;
var total = maze.width * maze.height;
var start = maze.GetStart();
var end = maze.GetEnd();
var endPos = end.Position();
bool[] visited = new bool[total];
Node[] prev = new Node[total];
var infinity = float.PositiveInfinity;
float[] distances = new float[total];
FibNode[] nodeIndex = new FibNode[total];
for (var i = 0; i < total; i++)
{
visited[i] = false;
distances[i] = infinity;
}
var unvisited = new FibPQ();
distances[start.Position().Y *width + start.Position().X] = 0;
FibNode startNode = new FibNode(0, start);
nodeIndex[start.Position().Y *width + start.Position().X] = startNode;
unvisited.insert(startNode);
int count = 0;
completed = false;
while (unvisited.len() > 0)
{
count += 1;
FibNode n = unvisited.RemoveMinimum();
//
Node u = n.value;
Point uPos = u.Position();
int uPosIndex = uPos.Y * width + uPos.X;
if (distances[uPosIndex] == infinity)
{
break;
}
if (uPos == endPos)
{
completed = true;
break;
}
foreach (var v in u.neighbours)
{
if (v != null)
{
Point vPos = v.Position();
int vPosIndex = vPos.Y * width + vPos.X;
if (visited[vPosIndex] == false)
{
int d = (vPos.Y - uPos.Y) + (vPos.X - uPos.X);
int newDistance = (int)distances[uPosIndex] + d;
if (newDistance < distances[vPosIndex])
{
var vNode = nodeIndex[vPosIndex];
//v isn't alread in the queue - add it
if (vNode == null)
{
vNode = new FibNode(newDistance, v);
unvisited.insert(vNode);
nodeIndex[vPosIndex] = vNode;
distances[vPosIndex] = newDistance;
prev[vPosIndex] = u;
}
//v is already in the queue - decrease it's key
else
{
unvisited.decreaseKey(vNode, newDistance);
distances[vPosIndex] = newDistance;
prev[vPosIndex] = u;
}
}
}
}
}
visited[uPosIndex] = true;
}
//Recontruct the path.
List <Node> path = new List <Node>();
Node current = end;
path.Add(current);
while (current != null)
{
path = DirectionFactory.AppendLeft(path, current);
current = prev[current.Position().Y *width + current.Position().X];
}
return(path);
}