Ensure u-turn exists in intersection view

Due to some rather complex logic that tries to calculate intersection
angles by looking further up the road, it's possible to return
an intersection view that is missing a u-turn - something which
is assumed to exist in later guidance calculations.

We apply a fix here by ensuring the u-turn is always included in
the returned view.

CHANGELOG.md

@@ -59,6 +59,7 @@
       - ADDED: Support snapping to multiple ways at an input location. [#5953](https://github.com/Project-OSRM/osrm-backend/pull/5953)
       - FIXED: Fix snapping target locations to ways used in turn restrictions. [#6339](https://github.com/Project-OSRM/osrm-backend/pull/6339)
       - ADDED: Support OSM traffic signal directions. [#6153](https://github.com/Project-OSRM/osrm-backend/pull/6153)
+      - FIXED: Ensure u-turn exists in intersection view. [#6376](https://github.com/Project-OSRM/osrm-backend/pull/6376)
     - Profile:
       - CHANGED: Bicycle surface speeds [#6212](https://github.com/Project-OSRM/osrm-backend/pull/6212)
 

features/foot/intersection.feature

@@ -0,0 +1,31 @@
+@routing @foot
+Feature: Foot - Intersections
+
+    Background:
+        Given the profile "foot"
+        Given a grid size of 2 meters
+
+    # https://github.com/Project-OSRM/osrm-backend/issues/6218
+    Scenario: Foot - Handles non-planar intersections
+        Given the node map
+
+            """
+                f
+                |
+                a
+                |
+            b---c---d
+                |
+                e
+            """
+
+        And the ways
+            | nodes | highway | foot | layer |
+            | ac    | footway | yes  | 0     |
+            | bc    | footway | yes  | 0     |
+            | cd    | footway | yes  | 0     |
+            | cef   | footway | yes  | 1     |
+
+        When I route I should get
+            | from | to | route    |
+            | a    | d  | ac,cd,cd |

src/extractor/intersection/intersection_analysis.cpp

@@ -617,12 +617,13 @@ IntersectionView convertToIntersectionView(const util::NodeBasedDynamicGraph &gr
     std::vector<IntersectionViewDataWithAngle> pre_intersection_view;
     IntersectionViewData uturn{{SPECIAL_EDGEID, 0., 0., 0.}, false, 0.};
     std::size_t allowed_uturns_number = 0;
+
+    const auto is_uturn = [](const auto angle) {
+        return std::fabs(angle) < std::numeric_limits<double>::epsilon();
+    };
+
     for (const auto &outgoing_edge : outgoing_edges)
     {
-        const auto is_uturn = [](const auto angle) {
-            return std::fabs(angle) < std::numeric_limits<double>::epsilon();
-        };
-
         const auto edge_it = findEdge(edge_geometries, outgoing_edge.edge);
         const auto is_merged = merged_edges.count(outgoing_edge.edge) != 0;
         const auto is_turn_allowed = intersection::isTurnAllowed(graph,
@@ -678,6 +679,7 @@ IntersectionView convertToIntersectionView(const util::NodeBasedDynamicGraph &gr
     BOOST_ASSERT(uturn.eid != SPECIAL_EDGEID);
     if (uturn.entry_allowed || allowed_uturns_number == 0)
     { // Add the true U-turn if it is allowed or no other U-turns found
+        BOOST_ASSERT(uturn.angle == 0.);
         pre_intersection_view.insert(pre_intersection_view.begin(), {uturn, 0});
     }
 
@@ -706,6 +708,21 @@ IntersectionView convertToIntersectionView(const util::NodeBasedDynamicGraph &gr
         }
     }
 
+    auto no_uturn = std::none_of(
+        pre_intersection_view.begin(),
+        pre_intersection_view.end(),
+        [&is_uturn](IntersectionViewDataWithAngle road) { return is_uturn(road.first.angle); });
+    // After all of this, if we now don't have a u-turn, let's add one to the intersection.
+    // This is a hack to fix the triggered assertion ( see:
+    // https://github.com/Project-OSRM/osrm-backend/issues/6218 ). Ideally we would fix this more
+    // robustly, but this will require overhauling all of the intersection logic.
+    if (no_uturn)
+    {
+        BOOST_ASSERT(!uturn.entry_allowed && allowed_uturns_number > 0);
+        BOOST_ASSERT(uturn.angle == 0.);
+        pre_intersection_view.insert(pre_intersection_view.begin(), {uturn, 0});
+    }
+
     // Copy intersection view data
     IntersectionView intersection_view;
     intersection_view.reserve(pre_intersection_view.size());