public AStarNode(IAStarState value, IAStarState goal, float cost, AStarMap map)
{
this.Cost = cost;
this.GScore = float.MaxValue;
this.Heuristic = float.MaxValue;
this.Value = value;
this.CameFrom = null;
this.Goal = goal;
this.Heuristic = map.GetHeuristic(this);
}
public IEnumerable <IAStarState> FindSolution(IAStarState start, IAStarState goal, AStarMap map)
{
//It should add to the goal state the preconditions of the action
closedSet.Clear();
openSet.Clear();
AStarNode startNode = new AStarNode(start, goal, 0, map);
startNode.Goal = goal;
startNode.GScore = 0;
openSet.Add(startNode);
AStarStateComparer comparer = new AStarStateComparer();
FScoreComparer fscoreComparer = new FScoreComparer();
while (openSet.Count > 0)
{
openSet.Sort(fscoreComparer);
AStarNode current = openSet.First();
openSet.Remove(current);
if (current.Value.SatisfiedBy(current.Goal))
{
return(reconstructPath(current));
}
closedSet.Add(current);
foreach (AStarNode neighbour in map.GetNeighbours(current))
{
if (closedSet.Contains(neighbour, comparer))
{
continue;
}
float tentativeGScore = current.GScore + neighbour.Cost;
if (openSet.Contains(neighbour, comparer) == false)
{
openSet.Add(neighbour);
}
else if (tentativeGScore >= neighbour.GScore)
{
continue;
}
neighbour.CameFrom = current;
neighbour.GScore = tentativeGScore;
//FScore updates automatically
}
}
return(null);
}