public Node(Node parent, Action action, State state, State target)
{
this.State = state;
this.Parent = parent;
this.Action = action;
if (this.Parent != null && this.Action != null)
this.PathCost = this.Parent.PathCost + action.Cost;
this.EstimatedTotalPathCost = this.PathCost + Math.Sqrt(Math.Pow(this.State.X - target.X,2) + Math.Pow(this.State.Y - target.Y,2));
}
public static INode Search(List<State> states, List<Action> actions, State start, State end)
{
PriorityQueue<Node> frontier = new PriorityQueue<Node>();
List<State> explored = new List<State>();
frontier.Add(new Node(start, end));
while (frontier.Count > 0)
{
// Chooses the lowest-cost node in the frontier
Node currentNode = frontier.Pop();
// Win condition
if (currentNode.State.Equals(end))
return currentNode;
// Add currentNode to list of explored
explored.Add(currentNode.State);
// Filter actions to the ones connected to the current node
foreach (Action action in actions.Where(a => a.StateA.Equals(currentNode.State) || a.StateB.Equals(currentNode.State)))
{
// One of A or B will be the currentNode's action
// but it won't be added to the frontier since it
// is already in explored
var childA = new Node(currentNode, action, action.StateA, end);
if (!explored.Contains(childA.State))
frontier.Add(childA);
var childB = new Node(currentNode, action, action.StateB, end);
if (!explored.Contains(childB.State))
frontier.Add(childB);
}
}
return null;
}
public Node(Node parent, Action action, State target)
: this(parent, action, action.StateA, target)
{
}