Comment by Ono-Sendai

I think I reinvented and started implementing something like this, but then just ended up using std::priority_queue (the C++ standard library priority queue) which is pretty fast.