コード例 #1
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;
        }
コード例 #2
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;
        }