Pathfinding improvements