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