/// <summary>
/// Find the node with the lowest F score.
/// </summary>
private static Node FindLowestFScore(HashSet <Node> openSet, Node current, DefaultDict fScore)
{
var lowest = float.MaxValue;
Node selectedNode = current;
foreach (var node in openSet)
{
if (fScore.Get(node) < lowest)
{
selectedNode = node;
lowest = fScore.Get(node);
}
}
return(selectedNode);
}
/// <summary>
/// Find the path from start to goal using the A* algorithm. Based on the Wikipedia pseudo code example:
/// https://en.wikipedia.org/wiki/A*_search_algorithm#Pseudocode
/// </summary>
/// <returns><c>true</c>, if path was found, <c>false</c> otherwise.</returns>
public static bool FindPath(Node start, Node goal)
{
var closedSet = new HashSet <Node> ();
var openSet = new HashSet <Node> ();
openSet.Add(start);
var gScore = new DefaultDict();
gScore.Set(start, 0);
var fScore = new DefaultDict();
fScore.Set(start, HeuristicEstimate(start, goal));
var cameFrom = new Dictionary <Node, Node> ();
Node current = start;
while (openSet.Count > 0)
{
current = FindLowestFScore(openSet, current, fScore);
if (current.Type != Node.NodeType.Start && current.Type != Node.NodeType.Goal)
{
current.Type = Node.NodeType.Explored;
}
if (current == goal)
{
ReconstructPath(cameFrom, current);
return(true);
}
openSet.Remove(current);
closedSet.Add(current);
foreach (var neighbor in current.Connections.Keys)
{
if (closedSet.Contains(neighbor))
{
continue;
}
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
var tentativeGScore = gScore.Get(current) + current.Connections [neighbor];
if (tentativeGScore >= gScore.Get(neighbor))
{
continue;
}
cameFrom [neighbor] = current;
gScore.Set(neighbor, tentativeGScore);
fScore.Set(neighbor, gScore.Get(neighbor) + HeuristicEstimate(neighbor, goal));
}
}
return(false);
}
