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