Search code examples
c++stlpriority-queue

STL priority_queue using reinsertion for key decrease not working


I'm trying to implement an A*-like algorithm, and I am having trouble implementing key decrease using the STL priority_queue container. I am trying to reinsert elements into the queue when I decrease the key, but they are still not getting popped before elements with higher key.

Here is the main A*-like loop with irrelevant details removed:

while (!(open.empty() || (goalExp && switchToEA(firstTimeGoal, reopen)) )) {
        Node *nextParent = open.top();
        open.pop(); // Where the node gets popped
        if (closed.find(nextParent) != closed.end())
            continue;
        closed.insert(nextParent);
        nextParent->expand((*graph)[nextParent->getState()]);
        
        for (auto &i: nextParent->getAdjacent()) {
            T state = i.first;
            cost_t edgeCost = i.second;
            Node *child;
            if (state_to_node.find(state) != state_to_node.end()) {
                child = state_to_node[state];

                if (nextParent->getGCost() + edgeCost < child->getGCost()) {
                    child->setGCost(nextParent->getGCost() + edgeCost);
                    child->setFCost(child->getGCost() + (*h)(state));
                    child->setBack(nextParent);
                    open.push(child); // Key decrease
                } 
            } else {
                child = new Node(nextParent->getGCost() + edgeCost, nextParent->getGCost() + edgeCost + (*h)(state),
                                 state, nextParent);
                open.push(child); // New node insertion
                state_to_node[state] = child;
            }
        }
}

Priority queue definition:

std::priority_queue<Node *, std::vector<Node *>, typename Node::Compare> open;

Comparator class:

class Compare {
    public:
        bool operator() (BaseNode * a,BaseNode * b) {
            return a->fcost > b->fcost;
        }

        bool operator() (const BaseNode& a, const BaseNode& b) {
            return a.fcost > b.fcost;
        }
};

Solution

  • Using std::set<std::pair<cost_t, Node *>> to store key value pairs and then deleting and reinserting on key updates solved my issue.