Added method to calculate the 3D distance between two points
Added unit test for the new 3D distance method