3D distance

Added method to calculate the distance between two 3D coordinates.