Distance util function and test