C++ shortest path with curvature and torsion taken into account