Support snapping to multiple ways at an input location (#5953)

This PR improves routing results by adding support for snapping to
multiple ways at input locations.

This means all edges at the snapped location can act as source/target
candidates for routing search, ensuring we always find the best route,
and not the one dependent on the edge selected.

src/engine/plugins/viaroute.cpp

@@ -5,12 +5,10 @@
 
 #include "util/for_each_pair.hpp"
 #include "util/integer_range.hpp"
-#include "util/json_container.hpp"
 
 #include <cstdlib>
 
 #include <algorithm>
-#include <memory>
 #include <string>
 #include <vector>
 
@@ -95,19 +93,10 @@ Status ViaRoutePlugin::HandleRequest(const RoutingAlgorithmsInterface &algorithm
     }
     BOOST_ASSERT(phantom_node_pairs.size() == route_parameters.coordinates.size());
 
-    auto snapped_phantoms = SnapPhantomNodes(phantom_node_pairs);
-
-    std::vector<PhantomNodes> start_end_nodes;
-    auto build_phantom_pairs = [&start_end_nodes](const PhantomNode &first_node,
-                                                  const PhantomNode &second_node) {
-        start_end_nodes.push_back(PhantomNodes{first_node, second_node});
-    };
-    util::for_each_pair(snapped_phantoms, build_phantom_pairs);
+    auto snapped_phantoms = SnapPhantomNodes(std::move(phantom_node_pairs));
 
     api::RouteAPI route_api{facade, route_parameters};
 
-    InternalManyRoutesResult routes;
-
     // TODO: in v6 we should remove the boolean and only keep the number parameter.
     // For now just force them to be in sync. and keep backwards compatibility.
     const auto wants_alternatives =
@@ -115,20 +104,23 @@ Status ViaRoutePlugin::HandleRequest(const RoutingAlgorithmsInterface &algorithm
         (route_parameters.alternatives || route_parameters.number_of_alternatives > 0);
     const auto number_of_alternatives = std::max(1u, route_parameters.number_of_alternatives);
 
+    InternalManyRoutesResult routes;
     // Alternatives do not support vias, only direct s,t queries supported
     // See the implementation notes and high-level outline.
     // https://github.com/Project-OSRM/osrm-backend/issues/3905
-    if (1 == start_end_nodes.size() && algorithms.HasAlternativePathSearch() && wants_alternatives)
+    if (2 == snapped_phantoms.size() && algorithms.HasAlternativePathSearch() && wants_alternatives)
     {
-        routes = algorithms.AlternativePathSearch(start_end_nodes.front(), number_of_alternatives);
+        routes = algorithms.AlternativePathSearch({snapped_phantoms[0], snapped_phantoms[1]},
+                                                  number_of_alternatives);
     }
-    else if (1 == start_end_nodes.size() && algorithms.HasDirectShortestPathSearch())
+    else if (2 == snapped_phantoms.size() && algorithms.HasDirectShortestPathSearch())
     {
-        routes = algorithms.DirectShortestPathSearch(start_end_nodes.front());
+        routes = algorithms.DirectShortestPathSearch({snapped_phantoms[0], snapped_phantoms[1]});
     }
     else
     {
-        routes = algorithms.ShortestPathSearch(start_end_nodes, route_parameters.continue_straight);
+        routes =
+            algorithms.ShortestPathSearch(snapped_phantoms, route_parameters.continue_straight);
     }
 
     // The post condition for all path searches is we have at least one route in our result.
@@ -160,18 +152,29 @@ Status ViaRoutePlugin::HandleRequest(const RoutingAlgorithmsInterface &algorithm
             }
         }
 
-        route_api.MakeResponse(routes, start_end_nodes, result);
+        route_api.MakeResponse(routes, snapped_phantoms, result);
     }
     else
     {
-        auto first_component_id = snapped_phantoms.front().component.id;
-        auto not_in_same_component = std::any_of(snapped_phantoms.begin(),
-                                                 snapped_phantoms.end(),
-                                                 [first_component_id](const PhantomNode &node) {
-                                                     return node.component.id != first_component_id;
-                                                 });
+        const auto all_in_same_component =
+            [](const std::vector<PhantomNodeCandidates> &waypoint_candidates) {
+                return std::any_of(waypoint_candidates.front().begin(),
+                                   waypoint_candidates.front().end(),
+                                   // For each of the first possible phantoms, check if all other
+                                   // positions in the list have a phantom from the same component.
+                                   [&](const PhantomNode &phantom) {
+                                       const auto component_id = phantom.component.id;
+                                       return std::all_of(
+                                           std::next(waypoint_candidates.begin()),
+                                           std::end(waypoint_candidates),
+                                           [component_id](const PhantomNodeCandidates &candidates) {
+                                               return candidatesHaveComponent(candidates,
+                                                                              component_id);
+                                           });
+                                   });
+            };
 
-        if (not_in_same_component)
+        if (!all_in_same_component(snapped_phantoms))
         {
             return Error("NoRoute", "Impossible route between points", result);
         }