Use distance instead of distanceSquared for heuristic