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