// Outputs the optimal path
public Path ComputeBestPath(BlockWorld blockWorld)
{
HeapPriorityQueue <Path> frontier = new HeapPriorityQueue <Path>(MaxNodes);
HashSet <BlockWorld> explored = new HashSet <BlockWorld>();
// Initial state, path
BlockWorld initialState = blockWorld;
Path initialPath = new Path(0, new List <BlockWorld>()
{
initialState
});
// Add the initial path to the frontier
frontier.Enqueue(initialPath, 0 + Heuristic(initialState));
// Find paths
int expansions = 0;
while (frontier.Count > 0)
{
Path path = frontier.Dequeue();
BlockWorld lastWorld = path.Last();
// Return the no path
if (expansions > MaxExpansions)
{
return(null);
}
// Check goal
if (GoalFunction(lastWorld))
{
return(path);
}
// Mark as explored
explored.Add(lastWorld);
// Iterate over possible actions
List <BlockWorldAction> possibleActions = lastWorld.ApplicableActions();
foreach (BlockWorldAction action in possibleActions)
{
// Try the action on a cloned block world
BlockWorld newWorld = lastWorld.Clone();
newWorld.Advance(action);
// Check if explored already
bool alreadyExplored = false;
foreach (BlockWorld exploredWorld in explored)
{
if (exploredWorld.PropertiesEqual(newWorld))
{
alreadyExplored = true;
break;
}
}
if (!alreadyExplored)
{
// Extend path
Path newPath = path.ExtendedPath(newWorld, CostFunction(newWorld));
// Add to frontier
frontier.Enqueue(newPath, Heuristic(newWorld) + newPath.Cost);
}
}
expansions++;
}
// No solution exists
return(null);
}