Shortest path C++