/// <summary>
/// Expands the current coverage area with all neighbors of the specified <see
/// cref="Graph"/> node that can be reached by the current <see cref="Agent"/>.</summary>
/// <param name="node">
/// The <see cref="Graph"/> node whose neighbors to examine.</param>
/// <param name="cost">
/// The total path cost to reach <paramref name="node"/>, measured as the sum of all <see
/// cref="IGraphAgent{T}.GetStepCost"/> results for each movement step between neighboring
/// nodes.</param>
/// <remarks><para>
/// <b>ExpandArea</b> recursively computes all possible movement paths for the current <see
/// cref="Agent"/>, adding all valid nodes in any affordable path to the <see cref="Nodes"/>
/// collection.
/// </para><para>
/// <b>ExpandArea</b> never revisits nodes that were already reached by a better path. The
/// source node specified in the <see cref="FindReachable"/> call is never added to the
/// <b>Nodes</b> collection.</para></remarks>
private void ExpandArea(T node, double cost)
{
// get valid neighbors of current node
IList <T> neighbors = Graph.GetNeighbors(node);
// recurse into all valid neighbors
for (int i = 0; i < neighbors.Count; i++)
{
T neighbor = neighbors[i];
double minCost;
// skip nodes with better path
_pathCosts.TryGetValue(neighbor, out minCost);
if (minCost != 0 && minCost <= cost)
{
continue;
}
// skip unreachable nodes
if (!_agent.CanMakeStep(node, neighbor))
{
continue;
}
// get cost for next movement step
double stepCost = _agent.GetStepCost(node, neighbor);
Debug.Assert(stepCost > 0);
// skip unaffordable nodes
if (!_agent.RelaxedRange && cost + stepCost > _maxCost)
{
continue;
}
// skip nodes with better path
if (minCost != 0 && minCost <= cost + stepCost)
{
continue;
}
// add newly reached neighbor if possible
if (minCost == 0 && _agent.CanOccupy(neighbor))
{
_nodes.Add(neighbor);
}
// store new minimum path cost
_pathCosts[neighbor] = cost + stepCost;
// visit neighbors if still affordable
if (cost + stepCost < _maxCost)
{
ExpandArea(neighbor, cost + stepCost);
}
}
}
/// <summary>
/// Returns the last <see cref="PathNode{T}"/> in the best path whose total path cost does
/// not exceed the specified maximum cost.</summary>
/// <param name="maxCost">
/// The maximum total path cost of the returned <see cref="PathNode{T}"/>.</param>
/// <returns>
/// The <see cref="PathNode{T}"/> that is the last parent of <see cref="BestNode"/> whose
/// total path cost does not exceed the specified <paramref name="maxCost"/>.</returns>
/// <exception cref="ArgumentOutOfRangeException">
/// <paramref name="maxCost"/> is zero or negative.</exception>
/// <exception cref="PropertyValueException">
/// <see cref="BestNode"/> is a null reference.</exception>
/// <remarks><para>
/// <b>GetLastNode</b> always returns a <see cref="PathNode{T}"/> whose <see
/// cref="PathNode{T}.Node"/> is an element of <see cref="Nodes"/>. The exact element
/// depends on the specified <paramref name="maxCost"/>.
/// </para><para>
/// <b>GetLastNode</b> searches for the <see cref="PathNode{T}"/> that is the last <see
/// cref="PathNode{T}.Parent"/> of <see cref="BestNode"/> whose <see cref="PathNode{T}.G"/>
/// value does not exceed the specified <paramref name="maxCost"/>, and for which <see
/// cref="IGraphAgent{T}.CanOccupy"/> succeeds with the moving <see cref="Agent"/>.
/// </para><para>
/// If <see cref="IGraphAgent{T}.RelaxedRange"/> is <c>true</c> for the moving <see
/// cref="Agent"/>, the <see cref="PathNode{T}.G"/> value of the returned <b>PathNode</b>
/// may exceed <paramref name="maxCost"/> if the <see cref="PathNode{T}.G"/> value of its
/// <see cref="PathNode{T}.Parent"/> node is strictly less than <paramref name="maxCost"/>.
/// </para><para>
/// If the specified <paramref name="maxCost"/> exceeds the cost of all nodes, or if
/// <b>CanOccupy</b> fails for all affordable nodes, <b>GetLastNode</b> returns the <see
/// cref="PathNode{T}"/> that corresponds to the first <see cref="Nodes"/> element, i.e. the
/// source node of the path search.</para></remarks>
public PathNode <T> GetLastPathNode(double maxCost)
{
if (maxCost <= 0)
{
ThrowHelper.ThrowArgumentOutOfRangeException(
"maxCost", maxCost, Strings.ArgumentNotPositive);
}
if (_bestNode == null)
{
ThrowHelper.ThrowPropertyValueException("BestNode", Strings.PropertyNull);
}
/*
* Go backward starting at BestNode and check these conditions:
*
* 1. cursor.Parent is null -- we have arrived at the source node and have
* nowhere else to go, so we return the source node.
*
* 2. cursor.G <= maxCost -- the current node is no more expensive than maxCost.
*
* 3. cursor.Parent.G < maxCost -- Agent.RelaxedRange means we can enter any node
* if we have even one movement point left, so we return the current node.
*
* 4. Agent.CanOccupy(cursor.Node) -- always check that the Agent’s movement
* can end on the current node.
*/
PathNode <T> cursor = _bestNode;
bool relaxed = _agent.RelaxedRange;
while (true)
{
PathNode <T> parent = cursor._parent;
if (parent == null)
{
return(cursor);
}
if ((cursor._g <= maxCost || (relaxed && parent._g < maxCost)) &&
_agent.CanOccupy(cursor.Node))
{
return(cursor);
}
cursor = parent;
}
}
/// <summary>
/// Finds the best path to move the specified agent from one specified <see cref="Graph"/>
/// node to another.</summary>
/// <param name="agent">
/// The <see cref="IGraphAgent{T}"/> that performs the movement.</param>
/// <param name="source">
/// The source node within <see cref="Graph"/>.</param>
/// <param name="target">
/// The target node within <see cref="Graph"/>.</param>
/// <returns>
/// <c>true</c> if a best path between <paramref name="source"/> and <paramref
/// name="target"/> could be found; otherwise, <c>false</c>.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="agent"/>, <paramref name="source"/>, or <paramref name="target"/> is a
/// null reference.</exception>
/// <remarks><para>
/// <b>FindBestPath</b> returns <c>false</c> if either of the specified <paramref
/// name="source"/> and <paramref name="target"/> nodes is invalid, or if no connecting path
/// could be found.
/// </para><para>
/// Otherwise, <b>FindBestPath</b> returns <c>true</c> and sets the <see cref="BestNode"/>,
/// <see cref="Nodes"/>, and <see cref="TotalCost"/> properties to the results of the path
/// search.
/// </para><para>
/// <b>FindBestPath</b> calls <see cref="IGraphAgent{T}.CanOccupy"/> and <see
/// cref="IGraphAgent{T}.IsNearTarget"/> on the specified <paramref name="agent"/> to
/// determine whether a given <see cref="BestNode"/> candidate is acceptable. Depending on
/// the implementation of <b>IsNearTarget</b>, the <see cref="PathNode{T}.Node"/> of the
/// final <see cref="BestNode"/> may differ from the specified <paramref name="target"/>,
/// and possibly equal the specified <paramref name="source"/>. <b>CanOccupy</b> is never
/// called on path nodes that match the <paramref name="source"/> node.
/// </para><para>
/// <b>FindBestPath</b> operates with a <em>restricted search radius</em> if <see
/// cref="RelativeLimit"/> is greater than zero. In this case, <see cref="AbsoluteLimit"/>
/// is set to the product (rounded up) of <b>RelativeLimit</b> and the distance between
/// <paramref name="source"/> and <paramref name="target"/>. Whenever a node is considered
/// for inclusion in the search path, its distances from <paramref name="source"/> and
/// <paramref name="target"/> are calculated, and the node is ignored if the sum exceeds
/// <b>AbsoluteLimit</b>.</para></remarks>
public bool FindBestPath(IGraphAgent <T> agent, T source, T target)
{
if (agent == null)
{
ThrowHelper.ThrowArgumentNullException("agent");
}
if (source == null)
{
ThrowHelper.ThrowArgumentNullException("source");
}
if (target == null)
{
ThrowHelper.ThrowArgumentNullException("target");
}
_agent = agent;
_source = source;
_target = target;
// clear previous results
_absoluteLimit = 0;
_bestNode = null;
_nodes.Clear();
_targetWorld = PointD.Empty;
// fail if either node is invalid
if (!Graph.Contains(source) || !Graph.Contains(target))
{
return(false);
}
// compute absolute distance limit if desired
double distance = Graph.GetDistance(source, target);
if (_relativeLimit > 0)
{
_absoluteLimit = distance * _relativeLimit;
}
// compute world distance to target if desired
if (UseWorldDistance)
{
_targetWorld = Graph.GetWorldLocation(target);
}
// initialize search list
_openList = new PathNode <T>(source, Graph.Connectivity);
_openList._h = distance;
bool success = false;
while (SetBestNode())
{
T node = _bestNode.Node;
// succeed if occupation target is in range
if (_agent.IsNearTarget(node, target, _bestNode._h) &&
(ComparerCache <T> .EqualityComparer.Equals(source, node) ||
_agent.CanOccupy(node)))
{
success = true;
break;
}
// add children to search space
CreateChildren(_bestNode);
}
// clear intermediate data
_source = _target = default(T);
Debug.Assert(_parents.Count == 0);
_openList = null;
_openTable.Clear();
_closedTable.Clear();
return(success);
}
