Added method to calculate distance between points.
Draft of the algorithm that will be implemented today.