public bool Search() { var Q = new IndexedPriorityQueueLow <double>(FCosts, Graph.NodeCount()); if (SourceNodeIndex > Graph.NodeCount()) { return(false); } Q.Insert(SourceNodeIndex); while (!Q.IsEmpty()) { int NextClosestNode = Q.Pop(); ShortestPathTree[NextClosestNode] = SearchFrontier[NextClosestNode]; if (NextClosestNode == TargetNodeIndex) { bFound = true; return(true); } foreach (var Edge in Graph.Edges[NextClosestNode]) { double NewCost = CostToThisNode[NextClosestNode] + Edge.EdgeCost; double HCost = Heuristic.Calculate(Graph, TargetNodeIndex, Edge.ToNodeIndex); double GCost = GCosts[NextClosestNode] + Edge.EdgeCost; if (SearchFrontier[Edge.ToNodeIndex] == null) { FCosts[Edge.ToNodeIndex] = GCost + HCost; GCosts[Edge.ToNodeIndex] = GCost; Q.Insert(Edge.ToNodeIndex); SearchFrontier[Edge.ToNodeIndex] = Edge; } //else if ( (GCost < GCosts[Edge.ToNodeIndex]) && // (ShortestPathTree[Edge.ToNodeIndex] == null) ) //{ else if (GCost < GCosts[Edge.ToNodeIndex]) { FCosts[Edge.ToNodeIndex] = GCost + HCost; GCosts[Edge.ToNodeIndex] = GCost; Q.ChangePriority(Edge.ToNodeIndex); SearchFrontier[Edge.ToNodeIndex] = Edge; } } } return(false); }
public Search_AStar(SparseGraph <TNode, TEdge> Graph, IHeuristic <SparseGraph <TNode, TEdge> > Heuristic, int Source, int Target) { this.Graph = Graph; this.bFound = false; this.Heuristic = Heuristic; SourceNodeIndex = Source; TargetNodeIndex = Target; int ActiveNodeCount = Graph.ActiveNodeCount(); int NodeCount = Graph.NodeCount(); ShortestPathTree = new List <TEdge>(NodeCount); SearchFrontier = new List <TEdge>(NodeCount); CostToThisNode = new List <double>(NodeCount); GCosts = new List <double>(NodeCount); FCosts = new List <double>(NodeCount); // not sure i need to initialize these...nt); for (int i = 0; i < NodeCount; i++) { ShortestPathTree.Insert(i, null); SearchFrontier.Insert(i, null); CostToThisNode.Insert(i, 0); FCosts.Insert(i, 0); GCosts.Insert(i, 0); } TimeSlicedQ = new IndexedPriorityQueueLow <double>(FCosts, Graph.NodeCount()); TimeSlicedQ.Insert(SourceNodeIndex); }
public bool Search() { var Q = new IndexedPriorityQueueLow <double>(CostToThisNode, Graph.NodeCount()); Q.Insert(SourceNodeIndex); while (!Q.IsEmpty()) { int NextClosestNode = Q.Pop(); ShortestPathTree[NextClosestNode] = SearchFrontier[NextClosestNode]; if (NextClosestNode == TargetNodeIndex) { bFound = true; return(true); } foreach (var Edge in Graph.Edges[NextClosestNode]) { double NewCost = CostToThisNode[NextClosestNode] + Edge.EdgeCost; if (SearchFrontier[Edge.ToNodeIndex] == null) { CostToThisNode[Edge.ToNodeIndex] = NewCost; Q.Insert(Edge.ToNodeIndex); SearchFrontier[Edge.ToNodeIndex] = Edge; } else if ((NewCost < CostToThisNode[Edge.ToNodeIndex]) && (ShortestPathTree[Edge.ToNodeIndex] == null)) { CostToThisNode[Edge.ToNodeIndex] = NewCost; Q.ChangePriority(Edge.ToNodeIndex); SearchFrontier[Edge.ToNodeIndex] = Edge; } } } return(false); }
public ESearchStatus CycleOnce() { if (SourceNodeIndex > Graph.NodeCount()) { return(ESearchStatus.TargetNotFound); } if (TimeSlicedQ.IsEmpty()) { return(ESearchStatus.TargetNotFound); } int NextClosestNode = TimeSlicedQ.Pop(); ShortestPathTree[NextClosestNode] = SearchFrontier[NextClosestNode]; if (NextClosestNode == TargetNodeIndex) { return(ESearchStatus.TargetFound); } foreach (var Edge in Graph.Edges[NextClosestNode]) { double HCost = Heuristic.Calculate(Graph, TargetNodeIndex, Edge.ToNodeIndex); double GCost = GCosts[NextClosestNode] + Edge.EdgeCost; if (SearchFrontier[Edge.ToNodeIndex] == null) { FCosts[Edge.ToNodeIndex] = GCost + HCost; GCosts[Edge.ToNodeIndex] = GCost; TimeSlicedQ.Insert(Edge.ToNodeIndex); SearchFrontier[Edge.ToNodeIndex] = Edge; } else if (GCost < GCosts[Edge.ToNodeIndex] && (ShortestPathTree[Edge.ToNodeIndex] == null)) { FCosts[Edge.ToNodeIndex] = GCost + HCost; GCosts[Edge.ToNodeIndex] = GCost; TimeSlicedQ.ChangePriority(Edge.ToNodeIndex); SearchFrontier[Edge.ToNodeIndex] = Edge; } } return(ESearchStatus.SearchIncomplete); }