add distance function to utilities

takes parameters: p1x,p1y,p2x,p2y and returns distance between