Added simple distance between 2 pts, will be needed for a*