/// <summary>
/// Internal function for A* search.
/// </summary>
/// <param name="pq"></param>
/// <param name="destNode"></param>
/// <returns></returns>
private static AStarSearchProgress AStarFindPath(ref PriorityQueue <double, AStarSearchProgress> pq, Node destNode)
{
PriorityQueueItem <double, AStarSearchProgress> pqItem = pq.Pop();
if (pqItem != null)
{
double curWeight = pqItem.Priority;
AStarSearchProgress progress = pqItem.Item;
Node curNode = progress.CurNode;
if (object.ReferenceEquals(curNode, destNode))
{
return(progress);
}
else
{
List <Edge> edgeList = curNode.EdgeList;
foreach (Edge edge in edgeList)
{
if (!progress.IsNodeVisited(edge.Node))
{
double weight = curWeight + edge.Weight;
AStarSearchProgress newProgress = progress.Clone();
newProgress.Visit(edge);
newProgress.CurNode = edge.Node;
pq.Put(weight, newProgress);
}
}
}
}
return(null);
}
/// <summary>
/// Performs an A* search from srcNode to destNode. Throws <c>PathNotFoundException</c> if no path can be found.
/// </summary>
/// <param name="srcNode"></param>
/// <param name="destNode"></param>
/// <returns></returns>
public static LinkedList <Edge> AStarSearch(Node srcNode, Node destNode)
{
PriorityQueue <double, AStarSearchProgress> pathEvalPQ = new PriorityQueue <double, AStarSearchProgress>();
AStarSearchProgress progress = new AStarSearchProgress()
{
CurNode = srcNode
};
progress.Visit(new Edge(0, srcNode));
pathEvalPQ.Put(0, progress);
while (pathEvalPQ.Count != 0)
{
AStarSearchProgress finalProgress = AStarFindPath(ref pathEvalPQ, destNode);
if (finalProgress != null)
{
return(finalProgress.VisitedEdges);
}
}
throw new PathNotFoundException(string.Format("Path from {0} to {1} cannot be found.", srcNode, destNode));
}