// Use this for initialization
private void Start()
{
var a = new Node("A");
var b = new Node("B");
var c = new Node("C");
var d = new Node("D");
var e = new Node("E");
var f = new Node("F");
a.AddOutgoingEdge(new Edge(a, b, 4));
a.AddOutgoingEdge(new Edge(a, c, 2));
a.AddOutgoingEdge(new Edge(a, f, 50));
b.AddOutgoingEdge(new Edge(b, c, 5));
b.AddOutgoingEdge(new Edge(b, d, 10));
c.AddOutgoingEdge(new Edge(c, e, 3));
e.AddOutgoingEdge(new Edge(e, d, 4));
d.AddOutgoingEdge(new Edge(d, f, 11));
var pathfinder = new AStarPathfinder <Node>(Heuristic);
var timer = new StopwatchExecutionTimer();
var path = pathfinder.FindPath(a, f);
print(string.Format("In {1} seconds found the following path with cost {0} from A to F:", path.Cost, timer.ElapsedSeconds));
print(path.Edges.Aggregate("", (soFar, edge) => soFar + (soFar.Length > 0 ? " -> " : "") + edge));
}
//Find a plan for the provided goal
private Plan GetPlanFor(Goal goal)
{
var timer = new StopwatchExecutionTimer();
var plan = env.Planner.FormulatePlan(env.KnowledgeProvider, env.SupportedPlanningActions, goal);
UnityEngine.Debug.Log(
$"{env.Planner.GetType()} has found a plan of length {plan.Length} and cost {plan.Cost} to satisfy \"{goal.Name}\" in {timer.ElapsedSeconds} seconds"
);
return(plan);
}
public Plan FormulatePlan(
IKnowledgeProvider knowledgeProvider,
HashSet <PlanningAction> availableActions,
Goal goal
)
{
var timer = new StopwatchExecutionTimer();
var allActions = new HashSet <PlanningAction>();
allActions.UnionWith(availableActions);
allActions.UnionWith(experienceActions.Where(experienceAction =>
availableActions.IsSupersetOf(experienceAction.Actions)));
var initialWorldState = new RelevantSymbolsPopulator(allActions, goal)
.PopulateWorldState(knowledgeProvider);
var goalConstraints = new RegressiveStatePopulator().Populate(goal);
try
{
var path = pathfinder.FindPath(
RegressiveNode.MakeRegular(
goalConstraints,
initialWorldState,
new RegressiveNodeExpander(allActions)
),
RegressiveNode.MakeTarget(initialWorldState)
);
if (learning)
{
var beforeLearning = timer.ElapsedSeconds;
Learn(path, initialWorldState);
UnityEngine.Debug.Log(
$"Learning time: {timer.ElapsedSeconds-beforeLearning}");
}
return(new Plan(from edge in path.Edges.Reverse() select((RegressiveEdge)edge).Action, goal));
}
catch (PathNotFoundException e)
{
throw new PlanNotFoundException(this, maxPlanLength, goal, e);
}
}
public Path <TGraphNode> FindPath(
TGraphNode sourceNode,
TGraphNode targetNode,
IEqualityComparer <TGraphNode> targetEqualityComparer,
IEqualityComparer <TGraphNode> nodeEqualityComparer
)
{
// If there is a limit on search time, we need to initialize the timer
var timer = new StopwatchExecutionTimer();
// Set of already visited nodes
var closed = new HashSet <TGraphNode>(nodeEqualityComparer);
// Priority queue of partial and potentially complete paths
var open = new SmartPriorityQueue <PartialPath, double> {
new PartialPath(heuristic(sourceNode, targetNode))
};
// Best known path to target in the open priority queue
PartialPath bestPathToTarget = null;
// Cost of the cheapest known path to target
var minTotalCost = double.PositiveInfinity;
// While there are unexplored nodes
while (open.Count > 0)
{
// Pop partial path with the lowest estimated cost from the priority queue
var currentPartialPath = open.PopFront();
// Node to explore
// If partial path is empty, we are at the source of our search.
var currentNode = currentPartialPath.IsEmpty ? sourceNode : currentPartialPath.LastNode;
if (closed.Contains(currentNode))
{
continue;
}
if (targetEqualityComparer.Equals(currentNode, targetNode))
{
UnityEngine.Debug.Log(
$"Path found of cost {currentPartialPath.CostSoFar} : {currentPartialPath.EdgeCount} edges; {closed.Count} nodes expanded"
);
return(currentPartialPath.ToPath());
}
// If there is a time limit, check the timer.
// If the time limit has been exceeded, return the best known path to target or,
// otherwise break and report a failure.
if (maxSecondsPerSearch < float.PositiveInfinity && timer.ElapsedSeconds > maxSecondsPerSearch)
{
if (bestPathToTarget != null)
{
// Time out, best path so far
UnityEngine.Debug.Log(
$"Time out! Path found of cost {currentPartialPath.CostSoFar} : {currentPartialPath.EdgeCount} edges"
);
return(bestPathToTarget.ToPath());
}
throw new PathfindingTimeoutException(GetType(), maxSecondsPerSearch);
}
// If we have a limit on max search depth, and current path is at max depth limit,
// don't add paths to currentNode's neighbours to the open priority queue
if (maxSearchDepth >= 0 && currentPartialPath.EdgeCount >= maxSearchDepth)
{
continue;
}
if (assumeNonNegativeCosts && currentPartialPath.CostSoFar > minTotalCost)
{
continue;
}
// Mark currentNode as visited by placing it into closed set.
closed.Add(currentNode);
// Insert paths from sourceNode to currentNode's neighbours into the open priority queue
foreach (var outgoingEdge in currentNode.OutgoingEdges)
{
var neighbour = outgoingEdge.TargetNode;
var pathToNeighbour = new PartialPath(currentPartialPath);
pathToNeighbour.AppendEdge(outgoingEdge, heuristic(neighbour, targetNode));
open.Add(pathToNeighbour);
// If there is a time limit, check if the neighbour is the target node
// and update bestPathToTarget, if needed.
// This allows us to keep track of the best path to target, found so far.
if (maxSecondsPerSearch < float.PositiveInfinity &&
targetEqualityComparer.Equals(neighbour, targetNode) &&
(bestPathToTarget == null ||
pathToNeighbour.EstimatedTotalCost < bestPathToTarget.CostSoFar))
{
bestPathToTarget = pathToNeighbour;
}
// If the assumption of non-negative costs is in effect, check if the neighbour
// is the target node and update minTotalCost, if needed.
if (assumeNonNegativeCosts && targetEqualityComparer.Equals(neighbour, targetNode))
{
minTotalCost = Math.Min(minTotalCost, pathToNeighbour.CostSoFar);
}
}
}
UnityEngine.Debug.Log("No plan found");
throw new NoPathExistsException(
GetType(),
maxSearchDepth
);
}
/// <inheritdoc />
public Path <GraphNode> FindPath(
GraphNode sourceNode,
GraphNode targetNode,
IEqualityComparer <GraphNode> targetEqualityComparer,
IEqualityComparer <GraphNode> nodeEqualityComparer
)
{
// If there is a limit on search time, we need to initialize the timer
var timer = new StopwatchExecutionTimer();
// Set of already visited nodes
var closed = new HashSet <GraphNode>(nodeEqualityComparer);
// Priority queue of partial and potentially complete paths
var open = new ListBasedPriorityQueue <PartialPath>();
open.Add(new PartialPath(this.heuristic(sourceNode, targetNode)));
// Best known path to target in the open priority queue
// (Used as a fallback, if time limit is exceeded before the search ends)
PartialPath bestPathToTarget = null;
// Cost of the cheapest known path to target
// (Used to discard costlier paths, if the assumption of non-negative edge costs is in effect)
// double minTotalCost = double.PositiveInfinity;
// While there are unexplored nodes
while (open.Count > 0)
{
// Pop partial path with the lowest estimated cost from the priority queue
var currentPartialPath = open.PopFront();
// Node to explore
GraphNode currentNode;
// If partial path is empty (and therefore has no last node to speak of),
// it means we are at the source of our search.
if (currentPartialPath.IsEmpty)
{
// Explore the source node
currentNode = sourceNode;
}
else
{
// Explore the last node of the current partial path
currentNode = currentPartialPath.LastNode;
}
// If the currently examined node has been examined previously, don't consider it further.
// This has the effect of discarding the partial path, which led to it, from the open priority queue.
// FIXME: As far as I understand, this is correct behaviour only if the heuristic is admissable!
// In general case (i.e. with inadmissable heuristics), closed nodes may need to be reopened.
// To control this behaviour an additional flag may be added to the constructor and checked here.
if (closed.Contains(currentNode))
{
continue;
}
// Will only be called if ASTAR_VALIDATE_NODE_HASH_IMPLEMENTATION is defined
ValidateNodeHashImplementation(closed, currentNode);
// If the currently examined node is the target
if (targetEqualityComparer.Equals(currentNode, targetNode))
{
// Hurray!
return(currentPartialPath.ToPath());
}
// If there is a time limit, check the timer.
// If the time limit has been exceeded, return the best known path to target or,
// if there has been no such path discovered, break and report a failure.
if (this.maxSecondsPerSearch < float.PositiveInfinity && timer.ElapsedSeconds > this.maxSecondsPerSearch)
{
if (bestPathToTarget != null)
{
// "Good enough"
return(bestPathToTarget.ToPath());
}
else
{
throw new PathfindingTimeoutException(this.GetType(), this.maxSecondsPerSearch);
}
}
// If we have a limit on max search depth, and current path is at max depth limit,
// don't add paths to currentNode's neighbours to the open priority queue,
// since those paths will exceed the max search depth, and don't place currentNode
// into closed set, since there might be a more expensive path with smaller edge count,
// which leads to it without exceeding the max seatch depth limit.
if (this.maxSearchDepth >= 0 && currentPartialPath.EdgeCount >= this.maxSearchDepth)
{
continue;
}
// FIXME: Remove the commented out code.
// The following check makes no sense: priority queue already guarantees that
// the current path is at most as expensive as the best known path to target.
// Maybe if it checked for >=, instead of >, i.e. assumed positive and not merely non-negative costs,
// it would have miniscule effect, but even so the benefits would likely be negligible.
// If the assumption of non-negative costs is in effect, we can discard the current path
// from further consideration, if it's already costlier than the cheapest known path
// that reaches the target node, because if all the edges have non-negative cost,
// we can be certain that it will not get any cheaper.
// (At this point we know that the current path does not reach the target,
// otherwise we would've already returned it earlier in this loop iteration.)
// if(this.assumeNonNegativeCosts && currentPartialPath.CostSoFar > minTotalCost)
// {
// continue;
// }
// Mark currentNode as visited by placing it into closed set.
closed.Add(currentNode);
// Insert paths from sourceNode to currentNode's neighbours into the open priority queue
foreach (var outgoingEdge in currentNode.OutgoingEdges)
{
var neighbour = outgoingEdge.TargetNode;
var pathToNeighbour = new PartialPath(currentPartialPath);
pathToNeighbour.AppendEdge(outgoingEdge, this.heuristic(neighbour, targetNode));
open.Add(pathToNeighbour);
// If there is a time limit, check if the neighbour is the target node
// and update bestPathToTarget, if needed.
// This allows us to keep track of the best path to target, found so far.
if (this.maxSecondsPerSearch < float.PositiveInfinity &&
targetEqualityComparer.Equals(neighbour, targetNode)
// Theoretically, EstimatedTotalCost could be used here, instead of CostSoFar,
// since heuristic SHOULD return 0 for target node.
// However, this behaviour is not enforced, and so CostSoFar is used for reliability.
&& (bestPathToTarget == null || pathToNeighbour.CostSoFar < bestPathToTarget.CostSoFar))
{
bestPathToTarget = pathToNeighbour;
}
// If the assumption of non-negative costs is in effect, check if the neighbour
// is the target node and update minTotalCost, if needed.
// if(this.assumeNonNegativeCosts && targetEqualityComparer.Equals(neighbour, targetNode))
// {
// // Theoretically, EstimatedTotalCost could be used here, instead of CostSoFar,
// // since heuristic SHOULD return 0 for target node.
// // However, this behaviour is not enforced, and so CostSoFar is used for reliability.
// minTotalCost = Math.Min(minTotalCost, pathToNeighbour.CostSoFar);
// }
}
}
// Found no path to targetNode
throw new NoPathExistsException(
this.GetType(),
this.maxSearchDepth != AstarPathfinderConfiguration <GraphNode> .UNLIMITED_SEARCH_DEPTH
? (int?)this.maxSearchDepth
: (int?)null
);
}
