public Goal(Predicate <NodeType> generalGoal)
{
SpecificGoal = new OptionalVal <NodeType>();
GeneralGoal = generalGoal;
}
public Goal(NodeType specificGoal, Predicate <NodeType> generalGoal)
{
SpecificGoal = specificGoal;
GeneralGoal = generalGoal;
}
public bool Equals(OptionalVal <T> o)
{
return((o.HasValue == HasValue) &&
(!o.HasValue || o.val.Equals(val)));
}
/// <summary>
/// Finds the shortest path from the given start to a node that satisfies the given goal.
/// Returns whether a full path to an acceptable goal was found.
/// </summary>
/// <param name="outPath">
/// After this method is called, this list contains the path from start to end,
/// NOT including the start node itself.
/// </param>
/// <param name="maxPathLength">
/// The maximum path length this method can search from the start node.
/// "Path length" is the sum of the lengths of each edge in a path.
/// </param>
/// <param name="tryMyBest">
/// If this is true, and a path to an actual goal can't be found,
/// this method will do extra work to attempt to find the closest path possible.
/// </param>
public bool FindPath(NodeType start, Goal <NodeType> goal, float maxPathLength,
bool tryMyBest, List <NodeType> outPath)
{
//Note: edge length is referred to as "search" cost,
// while edge length + A* heuristics is referred to as "traversal" cost.
//Clear out the collections for this run.
pathTree.Clear();
nodesToSearch.Clear();
cost_traversal.Clear();
cost_search.Clear();
considered.Clear();
connections.Clear();
//The final stopping place of this path.
OptionalVal <NodeType> finalDestination = new OptionalVal <NodeType>();
//Start searching from the source node.
considered.Add(start);
pathTree.Add(start, new OptionalVal <NodeType>());
cost_traversal.Add(start, 0.0f);
cost_search.Add(start, 0.0f);
//Start the search frontier with a phony edge that "ends" at the start node.
nodesToSearch.Push(new Edge <NodeType>(default(NodeType), start), 0.0f);
//This is used in case we can't find the actual end node.
NodeType lastNodeChecked = default(NodeType);
//As long as there are more edges to search, keep checking them out.
while (!nodesToSearch.IsEmpty)
{
//Get the edge/node being checked out.
KeyValuePair <float, Edge <NodeType> > edgeToCheck = nodesToSearch.Pop();
NodeType edgeDestination = edgeToCheck.Value.End;
lastNodeChecked = edgeDestination;
float totalTraversalCost = edgeToCheck.Key;
float totalSearchCost = cost_search[edgeDestination];
//Put this edge into the path tree.
//Note that if it was already in the path,
// then a shorter route to it has already been found.
if (!pathTree.ContainsKey(edgeDestination))
{
pathTree.Add(edgeDestination, edgeToCheck.Value.Start);
}
//If a goal has been found, stop here.
if (goal.IsValidEnd(edgeDestination))
{
finalDestination = edgeDestination;
break;
}
//If we've searched too far, discard this edge.
if (totalSearchCost >= maxPathLength)
{
continue;
}
//Now process all the edges coming out of this node.
connections.Clear();
Graph.GetConnections(edgeDestination, connections);
foreach (Edge <NodeType> connection in connections)
{
//Get the total cost of traversing/searching to this node.
float edgeLength, heuristic;
CalcCosts(goal, connection, out edgeLength, out heuristic);
float costToEdgeEnd_search = edgeLength + totalSearchCost,
costToEdgeEnd_traversal = edgeLength + heuristic + totalTraversalCost;
//Only check out this node if it's not too far away.
if (costToEdgeEnd_search <= maxPathLength)
{
//If this node hasn't been found yet, add it to the list of nodes to search.
if (!considered.Contains(connection.End))
{
//Add the edge to the search space.
considered.Add(connection.End);
cost_traversal.Add(connection.End, costToEdgeEnd_traversal);
cost_search.Add(connection.End, costToEdgeEnd_search);
nodesToSearch.Push(connection, costToEdgeEnd_traversal);
}
//If it HAS been found already, this must be a longer path.
else //if (traversalCostToEdgeEnd < costToMoveToNode[connection.End])
{
UnityEngine.Assertions.Assert.IsTrue(costToEdgeEnd_traversal >=
cost_traversal[connection.End]);
//Update the path tree.
//CostToMoveToNode[connections[i].End] = tempCost;
//getToNodeSearchCost[connections[i].End] = tempSearchCost;
//PathTree[connections[i].End] = connections[i].Start;
}
}
}
}
//Now generate the actual path.
//If we didn't find a valid goal node, pick the closest thing to it.
if (!finalDestination.HasValue)
{
if (!tryMyBest || pathTree.Count == 0)
{
return(false);
}
//If we can use heuristics to search towards a goal,
// we can abuse those heuristics to find something close to the goal.
if (goal.SpecificGoal.HasValue)
{
OptionalVal <NodeType> bestEnd = new OptionalVal <NodeType>();
float bestHeuristic = float.PositiveInfinity;
foreach (NodeType n in pathTree.Values)
{
float edgeLength, heuristic;
CalcCosts(goal, new Edge <NodeType>(n, goal.SpecificGoal.Value),
out edgeLength, out heuristic);
if (heuristic < bestHeuristic)
{
bestEnd = n;
bestHeuristic = heuristic;
}
}
finalDestination = bestEnd.Value;
}
//Otherwise, there's no way to know how close we are,
// so just use the last node we checked out.
else
{
finalDestination = lastNodeChecked;
}
}
//Build the path using the path tree.
outPath.Clear();
NodeType counter = finalDestination.Value;
while (!counter.Equals(start))
{
outPath.Add(counter);
counter = pathTree[counter];
}
outPath.Reverse();
return(goal.IsValidEnd(finalDestination.Value));
}
