Add distance calculation to ClosestPair algorithm, with tests