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