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;
                }
            }
        }
    }
    //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);
    }
Ejemplo n.º 3
0
    //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);
    }
    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;
                }
            }
        }
    }