Implement more effective priority queue

[hra4h03]

First off, thank you for making the ground stone library, which is currently in use in famous libraries and utilities. I think that the usage of this library must encourage implementing the algorithm more effectively. Therefore, I created a pull request with the improvements.

Thank you.