//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
DebugUtils.Assert ( ( edge.From() < nextNodeIndex_ ) &&
( edge.To() < nextNodeIndex_ ),
"<NavGraph::AddEdge>: invalid node index, from: " + edge.From() + ", to: " + edge.To() );
//make sure both nodes are active before adding the edge
if ( (nodes_[edge.To()].Index() != NavGraphNode.invalid_node_index) &&
(nodes_[edge.From()].Index() != NavGraphNode.invalid_node_index)) {
//add the edge, first making sure it is unique
if ( isUniqueEdge(edge.From(), edge.To()) ) {
edges_[edge.From()].Add(edge);
}
//if the graph is undirected we must add another connection in the opposite
//direction
/*
if (!isDigraph_) {
//check to make sure the edge is unique before adding
if ( isUniqueEdge(edge.To(), edge.From()) ) {
NavGraphEdge newEdge = edge;
newEdge.SetTo(edge.From());
newEdge.SetFrom(edge.To());
edges_[edge.To()].Add(newEdge);
}
}
*/
}
}
//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
DebugUtils.Assert((edge.From() < nextNodeIndex_) &&
(edge.To() < nextNodeIndex_),
"<NavGraph::AddEdge>: invalid node index, from: " + edge.From() + ", to: " + edge.To());
//make sure both nodes are active before adding the edge
if ((nodes_[edge.To()].Index() != NavGraphNode.invalid_node_index) &&
(nodes_[edge.From()].Index() != NavGraphNode.invalid_node_index))
{
//add the edge, first making sure it is unique
if (isUniqueEdge(edge.From(), edge.To()))
{
edges_[edge.From()].Add(edge);
}
//if the graph is undirected we must add another connection in the opposite
//direction
/*
* if (!isDigraph_) {
* //check to make sure the edge is unique before adding
* if ( isUniqueEdge(edge.To(), edge.From()) ) {
* NavGraphEdge newEdge = edge;
* newEdge.SetTo(edge.From());
* newEdge.SetFrom(edge.To());
*
* edges_[edge.To()].Add(newEdge);
* }
* }
*/
}
}
//removes the edge connecting from and to from the graph (if present). If
//a digraph then the edge connecting the nodes in the opposite direction
//will also be removed.
public void RemoveEdge(int fromNodeIdx, int toNodeIdx)
{
DebugUtils.Assert((fromNodeIdx < nodes_.Count) &&
(toNodeIdx < nodes_.Count),
"<NavGraph::RemoveEdge>:invalid node index");
NavGraphEdge currEdge = null;
/*
* if (!isDigraph_) {
* for ( int i=0; i<edges_[toNodeIdx].Count; ++i ) {
* currEdge = edges_[toNodeIdx][i];
* if ( currEdge.To() == fromNodeIdx ) {
* edges_[toNodeIdx].RemoveAt(i);
* break;
* }
* }
* }
*/
for (int i = 0; i < edges_[fromNodeIdx].Count; ++i)
{
currEdge = edges_[fromNodeIdx][i];
if (currEdge.To() == toNodeIdx)
{
edges_[fromNodeIdx].RemoveAt(i);
break;
}
}
}
public NavGraph(int cell_count, NavGrid nav_grid)
{
grid = new HandleVector <NavGraphEdge> .Handle[nav_grid.maxLinksPerCell * cell_count];
for (int i = 0; i < grid.Length; i++)
{
grid[i] = HandleVector <NavGraphEdge> .InvalidHandle;
}
edges = new HandleVector <NavGraphEdge>(cell_count);
for (int j = 0; j < cell_count; j++)
{
int num = j * nav_grid.maxLinksPerCell;
NavGrid.Link link = nav_grid.Links[num];
while (link.link != NavGrid.InvalidHandle)
{
NavGraphEdge item = new NavGraphEdge
{
startNavType = link.startNavType,
endNavType = link.endNavType,
endCell = link.link,
startCell = j
};
HandleVector <NavGraphEdge> .Handle handle = edges.Add(item);
grid[num] = handle;
num++;
link = nav_grid.Links[num];
}
grid[num] = HandleVector <NavGraphEdge> .InvalidHandle;
}
}
//returns true if an edge is not already present in the graph. Used
//when adding edges to make sure no duplicates are created.
protected bool isUniqueEdge(int fromIdx, int toIdx)
{
NavGraphEdge currEdge = null;
for (int i = 0; i < edges_[fromIdx].Count; ++i)
{
currEdge = edges_[fromIdx][i];
if (currEdge.To() == toIdx)
{
return(false);
}
}
return(true);
}
//iterates through all the edges in the graph and removes any that point
//to an invalidated node
protected void CullInvalidEdges()
{
List <NavGraphEdge> currEdgeList = null;
NavGraphEdge currEdge = null;
for (int i = 0; i < edges_.Count; ++i)
{
currEdgeList = edges_[i];
for (int j = 0; j < currEdgeList.Count; ++j)
{
currEdge = currEdgeList[j];
if (nodes_[currEdge.To()].Index() == NavGraphNode.invalid_node_index ||
nodes_[currEdge.From()].Index() == NavGraphNode.invalid_node_index)
{
currEdgeList.RemoveAt(j);
}
}
}
}
//sets the cost of an edge
public void SetEdgeCost(int fromNodeIdx, int toNodeIdx, float cost)
{
//make sure the nodes given are valid
DebugUtils.Assert((fromNodeIdx < nodes_.Count) && (toNodeIdx < nodes_.Count),
"<NavGraph::SetEdgeCost>: invalid index, fromNodeIdx: " + fromNodeIdx + ", toNodeIdx: " + toNodeIdx);
NavGraphEdge currEdge = null;
//visit each neighbour and erase any edges leading to this node
for (int i = 0; i < edges_[fromNodeIdx].Count; ++i)
{
currEdge = edges_[fromNodeIdx][i];
if (currEdge.To() == toNodeIdx)
{
currEdge.SetCost(cost);
break;
}
}
}
public PathEdge( Vector2 source, Vector2 destination, NavGraphEdge.EdgeType edgeType=NavGraphEdge.EdgeType.normal )
{
source_ = source;
destination_ = destination;
edgeType_ = edgeType;
}
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 void Render(Graphics objGraphics)
{
//render all the cells
for (int nd = 0; nd < m_Graph.NumNodes(); ++nd)
{
int left = (int)(m_Graph.GetNode(nd).Pos.X - m_dCellWidth / 2.0);
int top = (int)(m_Graph.GetNode(nd).Pos.Y - m_dCellHeight / 2.0);
int right = (int)(1 + m_Graph.GetNode(nd).Pos.X + m_dCellWidth / 2.0);
int bottom = (int)(1 + m_Graph.GetNode(nd).Pos.Y + m_dCellHeight / 2.0);
Rectangle rect = Rectangle.FromLTRB(left, top, right, bottom);
SolidBrush fillBrush = new SolidBrush(Color.Gray);
switch (m_TerrainType[nd])
{
case (int)brush_type.normal:
fillBrush.Color = Color.White;
break;
case (int)brush_type.obstacle:
fillBrush.Color = Color.Black;
break;
case (int)brush_type.water:
fillBrush.Color = Color.DodgerBlue;
break;
case (int)brush_type.mud:
fillBrush.Color = Color.Brown;
break;
default:
fillBrush.Color = Color.White;
break;
}//end switch
objGraphics.FillRectangle(fillBrush, rect);
if (m_bShowTiles)
{
objGraphics.DrawLine(Pens.Gray, rect.Location, new Point(rect.Right, rect.Top));
objGraphics.DrawLine(Pens.Gray, rect.Location, new Point(rect.Left, rect.Bottom));
}
if (nd == m_iTargetCell)
{
rect.Inflate(-4, -4);
objGraphics.FillRectangle(Brushes.Red, rect);
objGraphics.DrawString("T", m_font, Brushes.Black, rect.Right - (int)(rect.Width * 0.75), rect.Y);
}
else if (nd == m_iSourceCell)
{
rect.Inflate(-4, -4);
objGraphics.FillRectangle(Brushes.LightGreen, rect);
objGraphics.DrawString("S", m_font, Brushes.Black, rect.Right - (int)(rect.Width * 0.75), rect.Y);
}
//render dots at the corners of the cells
objGraphics.DrawLine(m_ThickBlack, left, top, left + 1, top + 1);
}
//draw the graph nodes and edges if rqd
if (m_bShowGraph)
{
if (m_Graph.NumNodes() > 0)
{
SparseGraph.NodeIterator NodeItr = new SparseGraph.NodeIterator(m_Graph);
while (NodeItr.MoveNext())
{
objGraphics.DrawEllipse(Pens.LightGray, (int)(NodeItr.Current.Pos.X - 2), (int)(NodeItr.Current.Pos.Y - 2), (int)2 * 2, (int)2 * 2);
SparseGraph.EdgeIterator EdgeItr = new SparseGraph.EdgeIterator(m_Graph, NodeItr.Current.Index);
while (EdgeItr.MoveNext())
{
objGraphics.DrawLine(Pens.LightGray, (int)NodeItr.Current.Pos.X, (int)NodeItr.Current.Pos.Y, (int)m_Graph.GetNode(EdgeItr.Current.To).Pos.X, (int)m_Graph.GetNode(EdgeItr.Current.To).Pos.Y);
}
}
}
}
//draw any tree retrieved from the algorithms
for (int e = 0; e < m_SubTree.Count; ++e)
{
if (!NavGraphEdge.IsNull(m_SubTree[e]))
{
Vector2D from = m_Graph.GetNode(m_SubTree[e].From).Pos;
Vector2D to = m_Graph.GetNode(m_SubTree[e].To).Pos;
objGraphics.DrawLine(Pens.Red, (int)from.X, (int)from.Y, (int)to.X, (int)to.Y);
}
}
//draw the path (if any)
if (m_Path.Count > 0)
{
for (int i = 0; i < m_Path.Count; ++i)
{
if (i > 0)
{
Point start = new Point((int)m_Graph.GetNode(m_Path[i - 1]).Pos.X, (int)m_Graph.GetNode(m_Path[i - 1]).Pos.Y);
Point end = new Point((int)m_Graph.GetNode(m_Path[i]).Pos.X, (int)m_Graph.GetNode(m_Path[i]).Pos.Y);
objGraphics.DrawLine(m_ThickBlue, start, end);
}
}
}
}
//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);
}
