// Returns a stack of nodes representing the path from start to goal.
// Popping elements off the stack yields the correct sequence of moves
// in order.
public Stack<MazeNode> AStarSearch(MazeNode start, MazeNode goal)
{
HashSet<MazeNode> closed = new HashSet<MazeNode>();
HashSet<MazeNode> open = new HashSet<MazeNode>();
MazeNode current;
ArrayList children;
int GScoreEstimate;
start.g = 0;
start.h = start.manhattanDistance(goal);
start.f = start.g + start.h;
start.a_star_parent = start;
open.Add(start);
//While there are adjacent, unexplored nodes to search
while (open.Count > 0)
{
current = open.Min();
//If we've reached our goal
if (current == goal)
{
//Retrieve the path we took via a stack
Stack<MazeNode> path_to_goal = new Stack<MazeNode>();
while (current.a_star_parent != current)
{
path_to_goal.Push(current);
current = current.a_star_parent;
}
path_to_goal.Push(current);
return path_to_goal;
}
//Else continue our search
open.Remove(current);
closed.Add(current);
children = current.getAdjacentEdges();
foreach (MazeNode child in children)
{
// If we've visited this node already, skip it.
if (closed.Contains(child))
{
continue;
}
// g is computed as the cost it took us to get here, plus the distance between the
// current node and the child we're considering.
GScoreEstimate = (int)(current.g) + current.manhattanDistance(child);
// If we haven't considered this node already, or our current estimate is more optimistic
// than our prior estimation
if (!open.Contains(child) || GScoreEstimate < child.g)
{
child.g = GScoreEstimate;
child.f = child.g + child.manhattanDistance(goal);
if (!open.Contains(child))
{
open.Add(child);
}
child.a_star_parent = current;
}
}
closed.Add(current);
}
//Search failed, no more nodes to find on open list
return null;
}