Slightly improved shortest_path