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