public override PathFinderResult FindPath()
{
GraphNodeHeuristic tempX, tempY;
bool updateDist;
int i, conn, distTemp;
var numberOfSteps = 0;
Distances[Graph.StartNode.NodeCode] = 0; // distance in firts node is zero
OpenSet.Add(new GraphNodeHeuristic(Graph.StartNode, 0, 0)); //O(log n)
while (OpenSet.Count > 0) //O(1)
{
numberOfSteps++;
tempX = OpenSet.ExtractMin(); // get first suitable key and remove it (O(log n))
if (tempX.GraphNode.Equals(Graph.EndNode))
{
return new PathFinderResult
{
Path = Path,
PathDirections = PathDirections,
Distances = Distances
}
}
;
ClosedSet[tempX.NodeCode] = true;
for (i = 0; i < 4; i++) // four directions
{
conn = tempX.NodeConnections[i];
if (conn == int.MaxValue || ClosedSet[conn])
{
continue; //skip value, no connections
}
distTemp = Distances[tempX.NodeCode] + Graph.GraphBody[conn].NodeValue;
tempY = new GraphNodeHeuristic(Graph.GraphBody[conn], distTemp,
HeuristicComparer.ComputeHeuristic(Graph.GraphBody[conn]));
if (!OpenSet.Contains(tempY)) //O(log 1)
{
OpenSet.Add(tempY); //O(log n)
updateDist = true;
}
else if (distTemp < Distances[tempY.NodeCode])
{
updateDist = true;
}
else
{
updateDist = false;
}
if (updateDist)
{
OpenSet.UpdateDistance(tempY); // O(log n)
Path[tempY.NodeCode] = tempX.GraphNode;
PathDirections[tempY.NodeCode] = GraphDirections.GetDirectionCharCode((byte)i);
Distances[tempY.NodeCode] = distTemp;
}
}
}
// no path found
return(new PathFinderResult
{
Path = null,
PathDirections = null,
Distances = null
});
}
}
