From ef8f3d7508dbdcaeac990d1b891d2e0775c594e6 Mon Sep 17 00:00:00 2001
From: Michael Bell <moke.byll@gmail.com>
Date: Tue, 27 Sep 2022 13:49:12 +0100
Subject: [PATCH] Ensure u-turn exists in intersection view (#6376)

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                                  |  1 +
 features/foot/intersection.feature            | 31 +++++++++++++++++++
 .../intersection/intersection_analysis.cpp    | 26 +++++++++++++---
 3 files changed, 54 insertions(+), 4 deletions(-)
 create mode 100644 features/foot/intersection.feature

diff --git a/CHANGELOG.md b/CHANGELOG.md
index b6ec2d73ede..1d775480fb2 100644
--- a/CHANGELOG.md
+++ b/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)
 
diff --git a/features/foot/intersection.feature b/features/foot/intersection.feature
new file mode 100644
index 00000000000..f1ef3f6a549
--- /dev/null
+++ b/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 |
diff --git a/src/extractor/intersection/intersection_analysis.cpp b/src/extractor/intersection/intersection_analysis.cpp
index 542bbaafae3..9a3ad1f108f 100644
--- a/src/extractor/intersection/intersection_analysis.cpp
+++ b/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,22 @@ IntersectionView convertToIntersectionView(const util::NodeBasedDynamicGraph &gr
         }
     }
 
+    auto no_uturn = std::none_of(pre_intersection_view.begin(),
+                                 pre_intersection_view.end(),
+                                 [&is_uturn](const 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());