Improved pathfinding algorithm.