Pathfinder algorithm