Dynamically planning the shortest path.cpp