/// <summary>
/// Finds path between From and To points
/// </summary>
/// <param name="from">The start point</param>
/// <param name="to">The destination point</param>
/// <param name="length">Length of the path in meters</param>
/// <returns>Path as list of PathSegments beteewn two point. If path wasn't found returns null.</returns>
public IList<PathSegment> FindPath(CandidatePoint from, CandidatePoint to, ref double length)
{
Initialize(from, to);
while (_open.Count > 0) {
PartialPath current = _open.RemoveTop();
_close.Add(current.End, current);
// Path found
if(current.End.MapPoint == to.MapPoint) {
length = current.Length;
var result = BuildPath(current, from);
Finalize(to);
return result;
}
foreach (var link in current.End.Connections) {
if (link.From != current.End) continue;
double distance;
if (link.To.MapPoint == to.MapPoint)
distance = current.Length + Calculations.GetPathLength(current.End.MapPoint, to.MapPoint, link.Geometry);
else
distance = current.Length + link.Geometry.Length;
if (_open.Contains(link.To)) {
// Update previously found path in the open list (if this one is shorter)
PartialPath p = _open[link.To];
if (p.Length > distance) {
p.PreviousNode = current.End;
p.PathFromPrevious = link.Geometry;
_open.Update(p, distance);
}
}
else if (_close.ContainsKey(link.To)) {
// Update previously found path in the close list (if this one is shorter)
if (_close[link.To].Length > distance) {
_close[link.To].Length = distance;
_close[link.To].End = current.End;
_close[link.To].PathFromPrevious = link.Geometry;
}
}
else {
// Expand path to new node
PartialPath expanded = new PartialPath() {
Length = distance,
EstimationToEnd = Calculations.GetDistance2D(link.To.MapPoint, to.MapPoint),
End = link.To, PreviousNode = current.End, PathFromPrevious = link.Geometry };
_open.Add(expanded);
}
}
}
Finalize(to);
return null;
}
/// <summary>
/// Builds result path
/// </summary>
/// <param name="lastPathPart"></param>
/// <param name="from"></param>
/// <returns></returns>
IList<PathSegment> BuildPath(PartialPath lastPathPart, CandidatePoint from)
{
List<PathSegment> result = new List<PathSegment>();
while (lastPathPart.PreviousNode != null) {
result.Add(new PathSegment() { From = lastPathPart.PreviousNode, To = lastPathPart.End, Road = lastPathPart.PathFromPrevious });
lastPathPart = _close[lastPathPart.PreviousNode];
}
result.Add(new PathSegment() { From = new Node(from.MapPoint), To = lastPathPart.End, Road = lastPathPart.PathFromPrevious });
result.Reverse();
return result;
}
/// <summary>
/// Initializes internal properties before search
/// </summary>
/// <param name="from">The start point</param>
/// <param name="to">The destination point</param>
void Initialize(CandidatePoint from, CandidatePoint to)
{
_open.Clear();
_close.Clear();
// Add nodes reachable from the From point to the open list
foreach (var connection in from.Road.Connections) {
PartialPath path = new PartialPath() {End = connection.To, PathFromPrevious = connection.Geometry,
Length = Calculations.GetPathLength(from.MapPoint, connection.To.MapPoint, connection.Geometry),
EstimationToEnd = Calculations.GetDistance2D(connection.To.MapPoint, to.MapPoint)
};
if (_open.Contains(path)) {
if (_open[path.End].Length > path.Length) {
_open.Remove(_open[path.End]);
_open.Add(path);
}
}
else {
_open.Add(path);
}
}
_temporaryConnections.Clear();
// Add temporary connections to the TO point
foreach (var targetConnections in to.Road.Connections) {
Connection destinationConnection = new Connection(targetConnections.From, new Node(to.MapPoint)) { Geometry = to.Road };
_temporaryConnections.Add(destinationConnection);
}
}
/// <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
);
}
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
);
}
public void Pattern()
{
var partialPath = new PartialPath(@"di");
Assert.AreEqual("di*", partialPath.Pattern);
}
public void PatternPathDeep()
{
var partialPath = new PartialPath(@"dir1\subdir");
Assert.AreEqual(@"dir1\subdir*", partialPath.Pattern);
}
public void NoQuotesBase()
{
var partialPath = new PartialPath(@"'T:\src'\dotne");
Assert.AreEqual(@"T:\src\", partialPath.Base);
}
public void NoQuotesPattern()
{
var partialPath = new PartialPath(@"'T:\src'\dotne");
Assert.AreEqual(@"T:\src\dotne*", partialPath.Pattern);
}
public void CompletionTargetIsRawTextEvenWhenQuoted()
{
var partialPath = new PartialPath(@"'T:\src'\dotne");
Assert.AreEqual(@"'T:\src'\dotne", partialPath.CompletionTarget);
}
public void CompletionTargetIsRawText()
{
var partialPath = new PartialPath(@"T:\src\dotne");
Assert.AreEqual(@"T:\src\dotne", partialPath.CompletionTarget);
}
public void CompletionTargetIsEqualToTextIfNoQuotes()
{
var partialPath = new PartialPath(@"T:\src");
Assert.AreEqual(partialPath.Text, partialPath.CompletionTarget);
}
public void BasePathTwoDeep()
{
var partialPath = new PartialPath(@"dir1\subdir1\Tes");
Assert.AreEqual(@"dir1\subdir1\", partialPath.Base);
}
public void BasePathNone()
{
var partialPath = new PartialPath(@"di");
Assert.AreEqual(String.Empty, partialPath.Base);
}
