Pathfinding improved