Pathfinding: Update and improvement