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;
}
};
Using std::set<std::pair<cost_t, Node *>>
to store key value pairs and then deleting and reinserting on key updates solved my issue.