matlab code to find shortest path visiting set of points in 3D space