Path Planning in 3D