//-------------------------- AddEdge ------------------------------------------
//
// Use this to add an edge to the graph. The method will ensure that the
// edge passed as a parameter is valid before adding it to the graph. If the
// graph is a digraph then a similar edge connecting the nodes in the opposite
// direction will be automatically added.
//-----------------------------------------------------------------------------
public void AddEdge(NavGraphEdge edge)
{
//first make sure the from and to nodes exist within the graph
Debug.Assert((edge.From < m_iNextNodeIndex) && (edge.To < m_iNextNodeIndex), "<SparseGraph::AddEdge>: invalid node index");
//make sure both nodes are active before adding the edge
if ((m_Nodes[edge.To].Index != invalid_node_index) && (m_Nodes[edge.From].Index != invalid_node_index))
{
//add the edge, first making sure it is unique
if (UniqueEdge(edge.From, edge.To))
{
m_Edges[edge.From].Add(edge);
}
//if the graph is undirected we must add another connection in the opposite direction
if (!m_bDigraph)
{
//check to make sure the edge is unique before adding
if (UniqueEdge(edge.To, edge.From))
{
NavGraphEdge NewEdge = new NavGraphEdge(edge.To, edge.From, edge.Cost);
m_Edges[edge.To].Add(NewEdge);
}
}
}
}
public static bool IsNull(NavGraphEdge o)
{
if ((object)o == null)
{
return(true);
}
return(false);
}
public override bool Search()
{
//create a std stack of edges
Queue <NavGraphEdge> Q = new Queue <NavGraphEdge>();
//create a dummy edge and put on the Queue
NavGraphEdge Dummy = new NavGraphEdge(m_iSource, m_iSource, 0);
Q.Enqueue(Dummy);
//mark the source node as visited
m_Visited[m_iSource] = (int)NodeState.visited;
//while there are edges in the stack keep searching
while (Q.Count > 0)
{
//grab and remove the next edge
NavGraphEdge NextEdge = Q.Dequeue();
//make a note of the parent of the node this edge points to
m_Route[NextEdge.To] = NextEdge.From;
//put it on the tree. (making sure the dummy edge is not placed on the tree)
if (NextEdge != Dummy)
{
m_SpanningTree.Add(NextEdge);
}
//if the target has been found the method can return success
if (NextEdge.To == m_iTarget)
{
return(true);
}
//push the edges leading from the node this edge points to onto
//the queue
SparseGraph.EdgeIterator EdgeItr = new SparseGraph.EdgeIterator(m_Graph, NextEdge.To);
while (EdgeItr.MoveNext())
{
if (m_Visited[EdgeItr.Current.To] == (int)NodeState.unvisited)
{
Q.Enqueue(EdgeItr.Current);
//and mark it visited
m_Visited[EdgeItr.Current.To] = (int)NodeState.visited;
}
}
}
//no path to target
return(false);
}
public override bool Search()
{
//create a std stack of edges
Stack<NavGraphEdge> stack = new Stack<NavGraphEdge>();
//create a dummy edge and put on the stack
NavGraphEdge Dummy = new NavGraphEdge(m_iSource, m_iSource, 0);
stack.Push(Dummy);
//while there are edges in the stack keep searching
while (stack.Count > 0)
{
//grab and remove the next edge
NavGraphEdge NextEdge = stack.Pop();
//make a note of the parent of the node this edge points to
m_Route[NextEdge.To] = NextEdge.From;
//put it on the tree. (making sure the dummy edge is not placed on the tree)
if (NextEdge != Dummy)
{
m_SpanningTree.Add(NextEdge);
}
//and mark it visited
m_Visited[NextEdge.To] = (int)NodeState.visited;
//if the target has been found the method can return success
if (NextEdge.To == m_iTarget)
{
return true;
}
//push the edges leading from the node this edge points to onto
//the stack (provided the edge does not point to a previously
//visited node)
SparseGraph.EdgeIterator EdgeItr = new SparseGraph.EdgeIterator(m_Graph, NextEdge.To);
while (EdgeItr.MoveNext())
{
if (m_Visited[EdgeItr.Current.To] == (int)NodeState.unvisited)
{
stack.Push(EdgeItr.Current);
}
}
}
//no path to target
return false;
}
//------------ Helper_AddAllNeighboursToGridNode ------------------
//
// use to add the eight neighboring edges of a graph node that
// are positioned in a grid layout
//------------------------------------------------------------------------
public static void Helper_AddAllNeighboursToGridNode(SparseGraph graph,
int row,
int col,
int NumCellsX,
int NumCellsY)
{
for (int i = -1; i < 2; ++i)
{
for (int j = -1; j < 2; ++j)
{
int nodeX = col + j;
int nodeY = row + i;
//skip if equal to this node
if ((i == 0) && (j == 0))
{
continue;
}
//check to see if this is a valid neighbour
if (ValidNeighbour(nodeX, nodeY, NumCellsX, NumCellsY))
{
//calculate the distance to this node
Vector2D PosNode = graph.GetNode(row * NumCellsX + col).Pos;
Vector2D PosNeighbour = graph.GetNode(nodeY * NumCellsX + nodeX).Pos;
double dist = PosNode.Distance(PosNeighbour);
NavGraphEdge NewEdge;
//this neighbour is okay so it can be added
NewEdge = new NavGraphEdge(row * NumCellsX + col,
nodeY * NumCellsX + nodeX,
dist);
graph.AddEdge(NewEdge);
//if graph is not a diagraph then an edge needs to be added going
//in the other direction
if (!graph.isDigraph())
{
NewEdge = new NavGraphEdge(nodeY * NumCellsX + nodeX,
row * NumCellsX + col,
dist);
graph.AddEdge(NewEdge);
}
}
}
}
}
public override List <int> GetPathToTarget()
{
List <int> path = new List <int>();
//just return an empty path if no path to target found or if
//no target has been specified
if (!m_bFound || m_iTarget < 0)
{
return(path);
}
int nd = m_iTarget;
path.Add(nd);
while (nd != m_iSource && !(NavGraphEdge.IsNull(m_ShortestPathTree[nd])))
{
nd = m_ShortestPathTree[nd].From;
path.Insert(0, nd); // Adds an element to the beginning of a list.
}
return(path);
}
public static bool IsNull(NavGraphEdge o)
{
if ((object)o == null) return true;
return false;
}
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);
}
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);
}
