Moved distance algorithm into pathfinding.