Update pathfinder to use distance rather than distanceSquared