/// <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); }