/// <summary>
/// convenince method for fetching a WorldState object
/// </summary>
/// <returns>The world state.</returns>
public WorldState CreateWorldState()
{
return(WorldState.Create(this));
}
/* from: http://theory.stanford.edu/~amitp/GameProgramming/ImplementationNotes.html
* OPEN = priority queue containing START
* CLOSED = empty set
* while lowest rank in OPEN is not the GOAL:
* current = remove lowest rank item from OPEN
* add current to CLOSED
* for neighbors of current:
* cost = g(current) + movementcost(current, neighbor)
* if neighbor in OPEN and cost less than g(neighbor):
* remove neighbor from OPEN, because new path is better
* if neighbor in CLOSED and cost less than g(neighbor): **
* remove neighbor from CLOSED
* if neighbor not in OPEN and neighbor not in CLOSED:
* set g(neighbor) to cost
* add neighbor to OPEN
* set priority queue rank to g(neighbor) + h(neighbor)
* set neighbor's parent to current
*/
/// <summary>
/// Make a plan of actions that will reach desired world state
/// </summary>
/// <param name="ap">Ap.</param>
/// <param name="start">Start.</param>
/// <param name="goal">Goal.</param>
/// <param name="storage">Storage.</param>
public static Stack <Action> Plan(ActionPlanner ap, WorldState start, WorldState goal,
List <AStarNode> selectedNodes = null)
{
storage.Clear();
var currentNode = Pool <AStarNode> .Obtain();
currentNode.WorldState = start;
currentNode.ParentWorldState = start;
currentNode.CostSoFar = 0; // g
currentNode.HeuristicCost = CalculateHeuristic(start, goal); // h
currentNode.CostSoFarAndHeuristicCost = currentNode.CostSoFar + currentNode.HeuristicCost; // f
currentNode.Depth = 1;
storage.AddToOpenList(currentNode);
while (true)
{
// nothing left open so we failed to find a path
if (!storage.HasOpened())
{
storage.Clear();
return(null);
}
currentNode = storage.RemoveCheapestOpenNode();
storage.AddToClosedList(currentNode);
// all done. we reached our goal
if (goal.Equals(currentNode.WorldState))
{
var plan = ReconstructPlan(currentNode, selectedNodes);
storage.Clear();
return(plan);
}
var neighbors = ap.GetPossibleTransitions(currentNode.WorldState);
for (var i = 0; i < neighbors.Count; i++)
{
var cur = neighbors[i];
var opened = storage.FindOpened(cur);
var closed = storage.FindClosed(cur);
var cost = currentNode.CostSoFar + cur.CostSoFar;
// if neighbor in OPEN and cost less than g(neighbor):
if (opened != null && cost < opened.CostSoFar)
{
// remove neighbor from OPEN, because new path is better
storage.RemoveOpened(opened);
opened = null;
}
// if neighbor in CLOSED and cost less than g(neighbor):
if (closed != null && cost < closed.CostSoFar)
{
// remove neighbor from CLOSED
storage.RemoveClosed(closed);
}
// if neighbor not in OPEN and neighbor not in CLOSED:
if (opened == null && closed == null)
{
var nb = Pool <AStarNode> .Obtain();
nb.WorldState = cur.WorldState;
nb.CostSoFar = cost;
nb.HeuristicCost = CalculateHeuristic(cur.WorldState, goal);
nb.CostSoFarAndHeuristicCost = nb.CostSoFar + nb.HeuristicCost;
nb.Action = cur.Action;
nb.ParentWorldState = currentNode.WorldState;
nb.Parent = currentNode;
nb.Depth = currentNode.Depth + 1;
storage.AddToOpenList(nb);
}
}
// done with neighbors so release it back to the pool
ListPool <AStarNode> .Free(neighbors);
}
}
/// <summary>
/// convenince method for fetching a WorldState object
/// </summary>
/// <returns>The world state.</returns>
public WorldState createWorldState()
{
return(WorldState.create(this));
}