/* 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 = Utilities.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 = Utilities.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
Utilities.ListPool <AStarNode> .free(neighbors);
}
}
