Finding euclidean path (trail), C++, STL implementation