Added new function to calcualte path in 3D