Exemple #1
0
        //-------------------------- 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);
 }
Exemple #3
0
        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;
        }
Exemple #5
0
        //------------ 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);
                        }
                    }
                }
            }
        }
Exemple #6
0
        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;
 }
Exemple #8
0
        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);
        }
Exemple #9
0
        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);
        }