C++ PriorityQueue::changePriority方法代码示例

/*
 *  Method: aStar
 *  Parameters: BasicGraph graph by reference
 *              Vertex pointer variable for path start
 *              Vertex pointer variable for path end
 *  - - - - - - - - - - - - - - - - - - - - - - - - - -
 *  Returns a Vector of Vertex pointer variables for which
 *  each index corresponds to a step in the path between
 *  the path's start and end points, if any exists.
 *  This function uses A*'s algorithm, which evaluates
 *  the estimated total cost of all neighbors' paths to the end
 *  from the starting vertex before continuing. It differs from Dijkstra's
 *  in its prioritization of location based on their estimated total cost
 *  or distance from the starting point, as opposed to the current cost
 *  up to that point. Because A* also orients subsequent vertices
 *  to their earlier parent, this function uses a helper function to
 *  rewind the shortest route from end to start, if applicable,
 *  returning an empty vector if not.
 */
Vector<Vertex*> aStar(BasicGraph& graph, Vertex* start, Vertex* end) {
    graph.resetData();
    Vector<Vertex*> path;
    PriorityQueue<Vertex*> pqueue;
    pqueue.enqueue(start, heuristicFunction(start, end));
    while(!pqueue.isEmpty()) {
        Vertex* current = pqueue.dequeue();
        visitVertex(current);
        if(BasicGraph::compare(current, end) == 0) break;
        for(Edge* edge : current->edges) {
            double cost = current->cost + edge->cost;
            if(!edge->finish->visited &&
               edge->finish->getColor() != YELLOW) {
                enqueueVertex(edge->finish, current, cost);
                pqueue.enqueue(edge->finish, cost + 
                               heuristicFunction(edge->finish, end));
            }
            if(edge->finish->getColor() == YELLOW &&
               cost < edge->finish->cost) {
                enqueueVertex(edge->finish, current, cost);
                pqueue.changePriority(edge->finish, cost + 
                                      heuristicFunction(edge->finish, end));
            }
        }
    }
    determinePath(start, end, path);
    return path;
}

int main(){
    
    PriorityQueue* pr = new PriorityQueue();

    pr->insertElem(4,13);
    pr->insertElem(3,5);
    pr->insertElem(2,9);
    pr->insertElem(1,2);
    
    for(int i=0 ; i<pr->size() ;i++){
        
        printf("%d\n", pr->minQueue[i]->priority  );

    }


    
    if( pr->contains(4) ) {
        pr->changePriority(4,1);
    }

    while(!pr->isEmpty()){
        Element * elem = pr->minPriority();
        printf("(%d,%d)\n",elem->vertex,elem->priority);
    }

}

Vector<Vertex*> dijkstrasAlgorithm(BasicGraph& graph, Vertex* start, Vertex* end) {
    graph.resetData();
    PriorityQueue<Vertex*> pqueue;
    Vector<Vertex*> path;

    pqueue.enqueue(start, 0);
    start->cost = 0.0;
    start->setColor(YELLOW);

    while(!pqueue.isEmpty())
    {
        Vertex* v = pqueue.dequeue();
        v->setColor(GREEN);

        if (v == end)
            break;

        for (auto edge : v->edges)
        {
            Vertex* next_v = edge->finish;
            if (next_v->getColor() == 0)
            {
                next_v->setColor(YELLOW);
                double next_cost = v->cost + edge->cost;
                next_v->previous = v;
                next_v->cost = next_cost;
                pqueue.enqueue(next_v, next_cost);
            }
            else if (next_v->getColor() == YELLOW)
            {
                double next_cost = v->cost + edge->cost;
                if (next_cost < next_v->cost)
                {
                    next_v->cost = next_cost;
                    next_v->previous = v;
                    pqueue.changePriority(next_v, next_v->cost);
                }
            }
        }
    }

    if (end->getColor() == GREEN)
    {
        Vertex* path_v = end;
        while (path_v->previous)
        {
            path.insert(0, path_v);
            path_v = path_v->previous;
        }
        path.insert(0, path_v);
    }
    return path;
}

/*
 * A* search. Will explore from 'end' until it finds 'start' or can determine that no valid path exists.
 * It explores in reverse to make it easier to build path array.
 */
vector<Node *> aStar(BasicGraph& graph, Vertex* start, Vertex* end) {
    graph.resetData();
    vector<Vertex*> path;
    PriorityQueue<Vertex*> openQueue;

    //Setup starting state
    end->cost = 0;
    end->visited = true;
    openQueue.enqueue(end, end->cost);
    Vertex* current = nullptr;

    //While there are still reachable nodes to explore
    while(!openQueue.isEmpty()){

        //Set current node to the next node in the queue
        current = openQueue.dequeue();
        current->setColor(GREEN);

        //If current node is target, build path and return
        if(current == start){

            rewind(end, start, path);
            return path;
        }

        //Add any unvisited neighbor to queue, adjust cost for already visited neighbor nodes
        for(Vertex* neighbor : graph.getNeighbors(current)){

            if(!neighbor->visited){

                neighbor->previous = current;
                neighbor->visited = true;
                neighbor->cost = current->cost + graph.getArc(current, neighbor)->cost;
                openQueue.enqueue(neighbor, neighbor->cost + neighbor->heuristic(start));
                neighbor->setColor(YELLOW);
            }else{

                double newCost = current->cost + graph.getArc(current, neighbor)->cost;
                if(neighbor->cost > newCost){

                    neighbor->previous = current;
                    neighbor->cost = newCost;
                    openQueue.changePriority(neighbor, neighbor->cost + neighbor->heuristic(start));
                    neighbor->setColor(YELLOW);
                }
            }
        }
    }
    return path;
}

/*
 * Function: aStar
 * --------------------
 * This function implements aStar search (variation of dijkstras with a heuristic)
 *
 * Preconditions:
 *
 *  @param: graph: The graph to be searched
 *          start: The start vertex
 *          end:The end vertex
 *  @return: returns a vector of vertexes containing the path (empty if no path)
 */
Vector<Vertex*> aStar(BasicGraph& graph, Vertex* start, Vertex* end) {

    graph.resetData(); // reset
    Vector<Vertex*> path; // the return path
    Set<Vertex*> inQueue; // A vector holding the values that we have already put into the queue

    // INitialize every vertex to have infinite cost
    for(Vertex* s:graph.getVertexSet()){
        s->cost = POSITIVE_INFINITY;
    }

    PriorityQueue<Vertex*> pq; // A pq to see where to search next (cost+heuristic = priority)
    // Set start's cost to 0 + heuristicFxn val
    start->cost = 0 + heuristicFunction(start,end);
    // Enqueue start vertex
    pq.enqueue(start,0);
    start->setColor(YELLOW);
    inQueue.add(start);

    // While there are still elements in the pq to try
    while(!pq.isEmpty()){
        Vertex* v = pq.dequeue();
        inQueue.remove(v);

        v->visited = true;
        v->setColor(GREEN);

        // We can stop (reached end)
        if(v == end){
            // We have reached the end, deconstruct
            path.clear();

            Vertex* temp = v;
            path.insert(0,temp);

            while(temp->previous!=NULL){ // deconstruct
                temp = temp->previous;
                path.insert(0,temp);
            }
            break;
        }
        // For each unvisited neighbor of v vertex
        for (Vertex* n: graph.getNeighbors(v)){
            // And it's unvisited
            if(n->visited == false){
                // v's cost plus weight of edge
                Edge* e = graph.getEdge(v,n);
                double edgeCost = e->cost;
                double costToVertexN = v->cost + edgeCost;
                // If the costToVertexN < n->cost, we know this is a better way to get to
                // vertex n
                if(costToVertexN < n->cost){
                    n->cost = costToVertexN;
                    n->previous = v;
                    n->setColor(YELLOW);

                    // Check to see if pq already contains n
                    if(inQueue.contains(n)){ // Priority now includes heuristic
                        pq.changePriority(n,costToVertexN + heuristicFunction(n,end));
                    } else {
                        pq.enqueue(n,costToVertexN + heuristicFunction(n,end));
                    }
                }

            }
        }

    }
    return path;
}

//this is the same as dijkstrasAlgorithm except for one thing which is commented
vector<Node *> aStar(BasicGraph& graph, Vertex* start, Vertex* end) {
    graph.resetData();
    Vertex* unDefined;
    Vertex*  currentNode;
    start->cost=0.0;
    PriorityQueue<Vertex*> vertexPrioQueue;
    map<Vertex*,Vertex*> predecessor;
    for (Node *node :graph.getNodeSet()){
        if (node!=start){
            node->cost=INFINITY;
            predecessor[node]=unDefined;
        }
        vertexPrioQueue.enqueue(node,node->cost);;

    }
    double alt;
    while (!vertexPrioQueue.isEmpty()){
        currentNode= vertexPrioQueue.front();

        vertexPrioQueue.dequeue();
        currentNode->setColor(YELLOW);
        currentNode->visited=true;
        if (currentNode==end){
            break;
        }
        for(Node *node :graph.getNeighbors(currentNode)){
            if (!node->visited){
                alt=currentNode->cost+graph.getArc(currentNode,node)->cost;
                if (alt<node->cost){
                    node->cost=alt;
                    predecessor[node]=currentNode;
                    //this is the change, the queuepriority comes from the node cost + the heuristic
                    vertexPrioQueue.changePriority(node,node->cost+node->heuristic((end)));

                }
            }
        }
        currentNode->setColor(GREEN);

    }

    if(predecessor[end]==unDefined){
        vector<Vertex*> path;
        return path;
    }
    else{
        stack<Vertex*> vertexStack;
        vector<Vertex*> path;
        currentNode=end;
        while (currentNode!=start){
            vertexStack.push(currentNode);
            currentNode=predecessor[currentNode];

        }
        vertexStack.push(start);
        while (!vertexStack.empty()){
            path.push_back(vertexStack.top());
            vertexStack.pop();
        }
        return path;
    }
}

vector<Node *> dijkstrasAlgorithm(BasicGraph& graph, Vertex* start, Vertex* end) {
    graph.resetData();
    //set as predescessor if that is undefined
    Vertex* unDefined;

    //the current node we are checking
    Vertex*  currentNode;

    //sets startnode cost to zero
    start->cost=0.0;

    //create prioqueue
    PriorityQueue<Vertex*> vertexPrioQueue;

    //used to keep track of all predeccesors
    map<Vertex*,Vertex*> predecessor;

    //set all costs, sets predecessor and adds the to the queue
    for (Node *node :graph.getNodeSet()){
        //all nodes but start should have infinity cost
        if (node!=start){
            node->cost=INFINITY;
            predecessor[node]=unDefined;
        }
        //add all nodes to queue
        vertexPrioQueue.enqueue(node,node->cost);

    }
    //keep track of the alternative cost
    double alt;
    //while the queue is not empty
    while (!vertexPrioQueue.isEmpty()){
        //put current node to the one with highest priority
        currentNode= vertexPrioQueue.front();
        vertexPrioQueue.dequeue();
        currentNode->setColor(YELLOW);
        currentNode->visited=true;
        if (currentNode==end){
            break;
        }

        //check all the node's neighbors
        for(Node *node :graph.getNeighbors(currentNode)){
            //if we have not visited that node
            if (!node->visited){
                //we check the alternative cost
                alt=currentNode->cost+graph.getArc(currentNode,node)->cost;
                //if the alternative cost is lower then we set that to our new cost
                if (alt<node->cost){
                    node->cost=alt;
                    predecessor[node]=currentNode;
                    vertexPrioQueue.changePriority(node,alt);

                }
            }
        }
        currentNode->setColor(GREEN);

    }
    //if we havent found end
    if(predecessor[end]==unDefined){
        vector<Vertex*> path;
        return path;
    }
    else{
        //if we have found end we trace through the predecessor map to find the path
        stack<Vertex*> vertexStack;
        vector<Vertex*> path;
        currentNode=end;
        while (currentNode!=start){
            vertexStack.push(currentNode);
            currentNode=predecessor[currentNode];

        }
        vertexStack.push(start);
        while (!vertexStack.empty()){
            path.push_back(vertexStack.top());
            vertexStack.pop();
        }
        return path;
    }
}