Fixed calculating distance function