public List <NodeSquare> a_star(NodeSquare start, NodeSquare goal)
{
List <NodeSquare> closedSet = new List <NodeSquare> ();
List <NodeSquare> openSet = new List <NodeSquare> ();
openSet.Add(start);
Dictionary <NodeSquare, NodeSquare> cameFrom = new Dictionary <NodeSquare, NodeSquare> ();
Dictionary <NodeSquare, float> gScore = new Dictionary <NodeSquare, float> ();
Dictionary <NodeSquare, float> fScore = new Dictionary <NodeSquare, float> ();
foreach (NodeSquare node in nodes)
{
gScore.Add(node, float.MaxValue);
fScore.Add(node, float.MaxValue);
}
gScore [start] = 0;
fScore [start] = heuristic_cost_estimate(start, goal);
while (openSet.Count > 0)
{
NodeSquare current = getLowest(fScore, openSet);
current.makeRobot();
if (current.Equals(goal))
{
return(reconstruct_path(cameFrom, current));
}
openSet.Remove(current);
closedSet.Add(current);
foreach (NodeSquare neighbor in adjacent(current))
{
if (closedSet.Contains(neighbor))
{
continue;
}
float tentative_gScore = gScore [current] + 1;
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
else if (tentative_gScore >= gScore [neighbor])
{
continue;
}
cameFrom.Add(neighbor, current);
gScore [neighbor] = tentative_gScore;
fScore [neighbor] = gScore [neighbor] + heuristic_cost_estimate(neighbor, goal);
}
}
return(null);
}
