public override bool Search()
{
//create an indexed priority queue that sorts smallest to largest
//(front to back).Note that the maximum number of elements the iPQ
//may contain is N. This is because no node can be represented on the
//queue more than once.
IndexedPriorityQLow pq = new IndexedPriorityQLow(m_FCosts, m_Graph.NumNodes());
//put the source node on the queue
pq.insert(m_iSource);
//while the queue is not empty
while (!pq.empty())
{
//get lowest cost node from the queue.
int NextClosestNode = pq.Pop();
//move this node from the frontier to the spanning tree
m_ShortestPathTree[NextClosestNode] = m_SearchFrontier[NextClosestNode];
//if the target has been found exit
if (NextClosestNode == m_iTarget)
{
return(true);
}
//now to relax the edges.
SparseGraph.EdgeIterator EdgeItr = new SparseGraph.EdgeIterator(m_Graph, NextClosestNode);
while (EdgeItr.MoveNext())
{
//calculate the heuristic cost from this node to the target (H)
double HCost = funcPointer(m_Graph, m_iTarget, EdgeItr.Current.To);
//calculate the 'real' cost to this node from the source (G)
double GCost = m_GCosts[NextClosestNode] + EdgeItr.Current.Cost;
//if the node has not been added to the frontier, add it and update
//the G and F costs
if (NavGraphEdge.IsNull(m_SearchFrontier[EdgeItr.Current.To]))
{
m_FCosts[EdgeItr.Current.To] = GCost + HCost;
m_GCosts[EdgeItr.Current.To] = GCost;
pq.insert(EdgeItr.Current.To);
m_SearchFrontier[EdgeItr.Current.To] = EdgeItr.Current;
}
//if this node is already on the frontier but the cost to get here
//is cheaper than has been found previously, update the node
//costs and frontier accordingly.
else if ((GCost < m_GCosts[EdgeItr.Current.To]) && NavGraphEdge.IsNull(m_ShortestPathTree[EdgeItr.Current.To]))
{
m_FCosts[EdgeItr.Current.To] = GCost + HCost;
m_GCosts[EdgeItr.Current.To] = GCost;
pq.ChangePriority(EdgeItr.Current.To);
m_SearchFrontier[EdgeItr.Current.To] = EdgeItr.Current;
}
}
}
return(false);
}
protected void Search()
{
//create an indexed priority queue that sorts smallest to largest
//(front to back).Note that the maximum number of elements the iPQ
//may contain is N. This is because no node can be represented on the
//queue more than once.
IndexedPriorityQLow pq = new IndexedPriorityQLow(costToThisNode_, navGraph_.NumNodes());
//put the source node on the queue
pq.Insert(sourceNodeID_);
//while the queue is not empty
while (!pq.Empty())
{
//get lowest cost node from the queue. Don't forget, the return value
//is a *node index*, not the node itself. This node is the node not already
//on the SPT that is the closest to the source node
int nextClosestNode = pq.Pop();
//move this edge from the frontier to the shortest path tree
shortestPathTree_[nextClosestNode] = searchFrontier_[nextClosestNode];
//if the target has been found exit
if (nextClosestNode == targetNodeID_)
{
return;
}
//now to relax the edges.
List <NavGraphEdge> edges = navGraph_.GetEdgeListList()[nextClosestNode];
NavGraphEdge currEdge = null;
for (int i = 0; i < edges.Count; ++i)
{
currEdge = edges[i];
//the total cost to the node this edge points to is the cost to the
//current node plus the cost of the edge connecting them.
float newCost = costToThisNode_[nextClosestNode] + currEdge.Cost();
//if this edge has never been on the frontier make a note of the cost
//to get to the node it points to, then add the edge to the frontier
//and the destination node to the PQ.
if (searchFrontier_[currEdge.To()] == null)
{
costToThisNode_[currEdge.To()] = newCost;
pq.Insert(currEdge.To());
searchFrontier_[currEdge.To()] = currEdge;
}
//else test to see if the cost to reach the destination node via the
//current node is cheaper than the cheapest cost found so far. If
//this path is cheaper, we assign the new cost to the destination
//node, update its entry in the PQ to reflect the change and add the
//edge to the frontier
else if ((newCost < costToThisNode_[currEdge.To()]) &&
(shortestPathTree_[currEdge.To()] == null))
{
costToThisNode_[currEdge.To()] = newCost;
//because the cost is less than it was previously, the PQ must be
//re-sorted to account for this.
pq.ChangePriority(currEdge.To());
searchFrontier_[currEdge.To()] = currEdge;
}
}
}
}
public override bool Search()
{
//create an indexed priority queue that sorts smallest to largest
//(front to back).Note that the maximum number of elements the iPQ
//may contain is N. This is because no node can be represented on the
//queue more than once.
IndexedPriorityQLow pq = new IndexedPriorityQLow(m_CostToThisNode, m_Graph.NumNodes());
//put the source node on the queue
pq.insert(m_iSource);
//while the queue is not empty
while (!pq.empty())
{
//get lowest cost node from the queue. Don't forget, the return value
//is a *node index*, not the node itself. This node is the node not already
//on the SPT that is the closest to the source node
int NextClosestNode = pq.Pop();
//move this edge from the frontier to the shortest path tree
m_ShortestPathTree[NextClosestNode] = m_SearchFrontier[NextClosestNode];
//if the target has been found exit
if (NextClosestNode == m_iTarget)
{
return(true);
}
//now to relax the edges.
SparseGraph.EdgeIterator EdgeItr = new SparseGraph.EdgeIterator(m_Graph, NextClosestNode);
while (EdgeItr.MoveNext())
{
//the total cost to the node this edge points to is the cost to the
//current node plus the cost of the edge connecting them.
double NewCost = m_CostToThisNode[NextClosestNode] + EdgeItr.Current.Cost;
//if this edge has never been on the frontier make a note of the cost
//to get to the node it points to, then add the edge to the frontier
//and the destination node to the PQ.
if (NavGraphEdge.IsNull(m_SearchFrontier[EdgeItr.Current.To]))
{
m_CostToThisNode[EdgeItr.Current.To] = NewCost;
pq.insert(EdgeItr.Current.To);
m_SearchFrontier[EdgeItr.Current.To] = EdgeItr.Current;
}
//else test to see if the cost to reach the destination node via the
//current node is cheaper than the cheapest cost found so far. If
//this path is cheaper, we assign the new cost to the destination
//node, update its entry in the PQ to reflect the change and add the
//edge to the frontier
else if ((NewCost < m_CostToThisNode[EdgeItr.Current.To]) &&
NavGraphEdge.IsNull(m_ShortestPathTree[EdgeItr.Current.To]))
{
m_CostToThisNode[EdgeItr.Current.To] = NewCost;
//because the cost is less than it was previously, the PQ must be
//re-sorted to account for this.
pq.ChangePriority(EdgeItr.Current.To);
m_SearchFrontier[EdgeItr.Current.To] = EdgeItr.Current;
}
}
}
return(false);
}
public bool findPath(Vector2f startPosition,
Vector2f targetPosition,
out List <Vector2f> path)
{
path = new List <Vector2f>();
if (mGraph == null)
{
return(false);
}
if (mSessionId == 0xFFFFFFFF)
{
reset();
}
mSessionId++;
mPriorityQueue.Reset();
uint startCellIndex = mGraph.getCellIndex(targetPosition);
uint endCellIndex = mGraph.getCellIndex(startPosition);
if (startCellIndex == (uint)PAHT_FINDER_ENUM.INVAILD_INDEX || endCellIndex == (uint)PAHT_FINDER_ENUM.INVAILD_INDEX)
{
return(false);
}
if (startCellIndex == endCellIndex)
{
path.Add(startPosition);
path.Add(targetPosition);
return(true);
}
if (!mGraph.sameArea(startCellIndex, endCellIndex))
{
return(false);
}
mShortestPathTree[(int)startCellIndex] = (uint)PAHT_FINDER_ENUM.INVAILD_INDEX;
mGValues[(int)startCellIndex] = 0;
mFValues[(int)startCellIndex] = 0;
mOpenTable[(int)startCellIndex] = mSessionId;
mPriorityQueue.insert((int)startCellIndex);
while (!mPriorityQueue.empty())
{
uint currentIndex = (uint)mPriorityQueue.Pop();
mCloseTable[(int)currentIndex] = mSessionId;
if (currentIndex == endCellIndex)
{
List <uint> cells = new List <uint>();
uint cellIndex = (uint)currentIndex;
uint maxCycleTime = mGraph.getCellCount();
for (uint i = 0; i < maxCycleTime; i++)
{
cells.Add(cellIndex);
if (mShortestPathTree[(int)cellIndex] == (uint)PAHT_FINDER_ENUM.INVAILD_INDEX)
{
return(mGraph.smoothPath(startPosition, targetPosition, cells, ref path));
}
else
{
cellIndex = mShortestPathTree[(int)cellIndex];
}
}
return(false);
}
else
{
List <uint> adjanceCells = mGraph.getAdjanceCells(currentIndex);
for (int i = 0; adjanceCells != null && i < adjanceCells.Count; i++)
{
uint adjanceIndex = adjanceCells[i];
if (mShortestPathTree[(int)currentIndex] == adjanceIndex)
{
continue;
}
float hValue = mGraph.getHValue(adjanceIndex, startPosition);
float gValue = mGValues[(int)currentIndex] + mGraph.getGValue(mShortestPathTree[(int)currentIndex], currentIndex, adjanceIndex);
if (mOpenTable[(int)adjanceIndex] != mSessionId)
{
mGValues[(int)adjanceIndex] = gValue;
mFValues[(int)adjanceIndex] = gValue + hValue;
mShortestPathTree[(int)adjanceIndex] = currentIndex;
mOpenTable[(int)adjanceIndex] = mSessionId;
mPriorityQueue.insert((int)adjanceIndex);
}
else if (mCloseTable[(int)adjanceIndex] != mSessionId && gValue < mGValues[(int)adjanceIndex])
{
mGValues[(int)adjanceIndex] = gValue;
mFValues[(int)adjanceIndex] = gValue + hValue;
mShortestPathTree[(int)adjanceIndex] = currentIndex;
mPriorityQueue.ChangePriority((int)adjanceIndex);
}
}
}
}
return(false);
}
//When called, this method pops the next node off the PQ and examines all
//its edges. The method returns an enumerated value (target_found,
//target_not_found, search_incomplete) indicating the status of the search
override public SearchResult CycleOnce()
{
//if the PQ is empty the target has not been found
if (pq_.Empty())
{
return(Graph_SearchTimeSliced.SearchResult.target_not_found);
}
//get lowest cost node from the queue
int nextClosestNode = pq_.Pop();
//move this node from the frontier to the spanning tree
shortestPathTree_[nextClosestNode] = searchFrontier_[nextClosestNode];
//if the target has been found exit
if (isSatisfied(navGraph_, targetNodeIndex_, nextClosestNode))
{
//make a note of the node index that has satisfied the condition. This
//is so we can work backwards from the index to extract the path from
//the shortest path tree.
targetNodeIndex_ = nextClosestNode;
return(Graph_SearchTimeSliced.SearchResult.target_found);
}
//now to test all the edges attached to this node
List <NavGraphEdge> edges = navGraph_.GetEdgeListList()[nextClosestNode];
NavGraphEdge currEdge = null;
for (int i = 0; i < edges.Count; ++i)
{
currEdge = edges[i];
//the total cost to the node this edge points to is the cost to the
//current node plus the cost of the edge connecting them.
float newCost = costToThisNode_[nextClosestNode] + currEdge.Cost();
//if this edge has never been on the frontier make a note of the cost
//to get to the node it points to, then add the edge to the frontier
//and the destination node to the PQ.
if (searchFrontier_[currEdge.To()] == null)
{
costToThisNode_[currEdge.To()] = newCost;
pq_.Insert(currEdge.To());
searchFrontier_[currEdge.To()] = currEdge;
}
//else test to see if the cost to reach the destination node via the
//current node is cheaper than the cheapest cost found so far. If
//this path is cheaper, we assign the new cost to the destination
//node, update its entry in the PQ to reflect the change and add the
//edge to the frontier
else if ((newCost < costToThisNode_[currEdge.To()]) &&
(shortestPathTree_[currEdge.To()] == null))
{
costToThisNode_[currEdge.To()] = newCost;
//because the cost is less than it was previously, the PQ must be
//re-sorted to account for this.
pq_.ChangePriority(currEdge.To());
searchFrontier_[currEdge.To()] = currEdge;
}
}
//there are still nodes to explore
return(Graph_SearchTimeSliced.SearchResult.search_incomplete);
}
//When called, this method pops the next node off the PQ and examines all
//its edges. The method returns an enumerated value (target_found,
//target_not_found, search_incomplete) indicating the status of the search
override public SearchResult CycleOnce()
{
//if the PQ is empty the target has not been found
if (pq_.Empty())
{
return(SearchResult.target_not_found);
}
//get lowest cost node from the queue
int NextClosestNode = pq_.Pop();
//put the node on the SPT
shortestPathTree_[NextClosestNode] = searchFrontier_[NextClosestNode];
//if the target has been found exit
if (NextClosestNode == targetIdx_)
{
return(SearchResult.target_found);
}
//now to test all the edges attached to this node
List <NavGraphEdge> edgeList = graph_.GetEdgeListList()[NextClosestNode];
NavGraphEdge edge = null;
for (int i = 0; i < edgeList.Count; ++i)
{
edge = edgeList[i];
//calculate the heuristic cost from this node to the target (H)
float HCost = Calculate(graph_, targetIdx_, edge.To());
//calculate the 'real' cost to this node from the source (G)
float GCost = gCosts_[NextClosestNode] + edge.Cost();
//if the node has not been added to the frontier, add it and update
//the G and F costs
if (searchFrontier_[edge.To()] == null)
{
fCosts_[edge.To()] = GCost + HCost;
gCosts_[edge.To()] = GCost;
pq_.Insert(edge.To());
searchFrontier_[edge.To()] = edge;
}
//if this node is already on the frontier but the cost to get here
//is cheaper than has been found previously, update the node
//costs and frontier accordingly.
else if ((GCost < gCosts_[edge.To()]) &&
(shortestPathTree_[edge.To()] == null))
{
fCosts_[edge.To()] = GCost + HCost;
gCosts_[edge.To()] = GCost;
pq_.ChangePriority(edge.To());
searchFrontier_[edge.To()] = edge;
}
}
//there are still nodes to explore
return(SearchResult.search_incomplete);
}
protected void Search()
{
//create an indexed priority queue that sorts smallest to largest
//(front to back).Note that the maximum number of elements the iPQ
//may contain is N. This is because no node can be represented on the
//queue more than once.
IndexedPriorityQLow pq = new IndexedPriorityQLow ( costToThisNode_, navGraph_.NumNodes() );
//put the source node on the queue
pq.Insert(sourceNodeID_);
//while the queue is not empty
while ( !pq.Empty() ) {
//get lowest cost node from the queue. Don't forget, the return value
//is a *node index*, not the node itself. This node is the node not already
//on the SPT that is the closest to the source node
int nextClosestNode = pq.Pop();
//move this edge from the frontier to the shortest path tree
shortestPathTree_[nextClosestNode] = searchFrontier_[nextClosestNode];
//if the target has been found exit
if ( nextClosestNode == targetNodeID_ ) {
return;
}
//now to relax the edges.
List<NavGraphEdge> edges = navGraph_.GetEdgeListList()[nextClosestNode];
NavGraphEdge currEdge = null;
for ( int i=0; i<edges.Count; ++i ) {
currEdge = edges[i];
//the total cost to the node this edge points to is the cost to the
//current node plus the cost of the edge connecting them.
float newCost = costToThisNode_[nextClosestNode] + currEdge.Cost();
//if this edge has never been on the frontier make a note of the cost
//to get to the node it points to, then add the edge to the frontier
//and the destination node to the PQ.
if ( searchFrontier_[currEdge.To()] == null ) {
costToThisNode_[currEdge.To()] = newCost;
pq.Insert(currEdge.To());
searchFrontier_[currEdge.To()] = currEdge;
}
//else test to see if the cost to reach the destination node via the
//current node is cheaper than the cheapest cost found so far. If
//this path is cheaper, we assign the new cost to the destination
//node, update its entry in the PQ to reflect the change and add the
//edge to the frontier
else if ( (newCost < costToThisNode_[currEdge.To()]) &&
(shortestPathTree_[currEdge.To()] == null) ) {
costToThisNode_[currEdge.To()] = newCost;
//because the cost is less than it was previously, the PQ must be
//re-sorted to account for this.
pq.ChangePriority(currEdge.To());
searchFrontier_[currEdge.To()] = currEdge;
}
}
}
}
