Add helper functions to calculate distance between 2 points