From dd1ace87c74c20f8d24ace654d7c6a6406d00a3a Mon Sep 17 00:00:00 2001
From: Michael Bell <michael@mjjbell.com>
Date: Sat, 24 Sep 2022 20:36:06 +0100
Subject: [PATCH] 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                                  |  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());