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