Improved pathfinding algorithm