Pathfinding implementation