Massive performance improvement by using
a heap structure to find the node with
lowest f_score. Sadly the stl heap algorithms
lack the ability to move a already inserted
element so that functionality had to be
reimplemented.