Refactor fork handling in guidance (#3264)

refactor fork handler

include/extractor/guidance/intersection.hpp

@@ -9,6 +9,7 @@
 #include <vector>
 
 #include "util/bearing.hpp"
+#include "util/log.hpp"
 #include "util/node_based_graph.hpp"
 #include "util/typedefs.hpp" // EdgeID
 
@@ -26,6 +27,8 @@ namespace guidance
 {
 
 // the shape of an intersection only knows about edge IDs and bearings
+// `bearing`    is the direction in clockwise angle from true north after taking the turn:
+//              0 = heading north, 90 = east, 180 = south, 270 = west
 struct IntersectionShapeData
 {
     EdgeID eid;
@@ -180,6 +183,12 @@ template <typename Self> struct EnableIntersectionOps
         auto comp = makeCompareAngularDeviation(angle);
         return boost::range::min_element(*self(), comp);
     }
+    // returns a non-const_interator
+    auto findClosestTurn(double angle)
+    {
+        auto comp = makeCompareAngularDeviation(angle);
+        return std::min_element(self()->begin(), self()->end(), comp);
+    }
 
     /* Check validity of the intersection object. We assume a few basic properties every set of
      * connected roads should follow throughout guidance pre-processing. This utility function
@@ -259,6 +268,18 @@ template <typename Self> struct EnableIntersectionOps
         return filter(*candidate) ? self()->end() : candidate;
     }
 
+    // check if all roads between begin and end allow entry
+    template <typename InputIt>
+    bool hasAllValidEntries(const InputIt begin, const InputIt end) const
+    {
+        static_assert(
+            std::is_base_of<std::input_iterator_tag,
+                            typename std::iterator_traits<InputIt>::iterator_category>::value,
+            "hasAllValidEntries() only accepts input iterators");
+        return std::all_of(
+            begin, end, [](const IntersectionViewData &road) { return road.entry_allowed; });
+    }
+
   private:
     auto self() { return static_cast<Self *>(this); }
     auto self() const { return static_cast<const Self *>(this); }
@@ -271,6 +292,30 @@ struct IntersectionView final : std::vector<IntersectionViewData>,      //
     using Base = std::vector<IntersectionViewData>;
 };
 
+// `Intersection` is a relative view of an intersection by an incoming edge.
+// `Intersection` are streets at an intersection ordered from from sharp right counter-clockwise to
+// sharp left where `intersection[0]` is _always_ a u-turn
+
+// An intersection is an ordered list of connected roads ordered from from sharp right
+// counter-clockwise to sharp left where `intersection[0]` is always a u-turn
+//
+//                                           |
+//                                           |
+//                                     (intersec[3])
+//                                           |
+//                                           |
+//                                           |
+//  nid ---(via_eid/intersec[0])--- nbg.GetTarget(via)  ---(intersec[2])---
+//                                           |
+//                                           |
+//                                           |
+//                                     (intersec[1])
+//                                           |
+//                                           |
+//
+// intersec := intersection
+// nbh := node_based_graph
+//
 struct Intersection final : std::vector<ConnectedRoad>,         //
                             EnableShapeOps<Intersection>,       //
                             EnableIntersectionOps<Intersection> //

include/extractor/guidance/intersection_handler.hpp

@@ -45,7 +45,7 @@ class IntersectionHandler
     virtual bool
     canProcess(const NodeID nid, const EdgeID via_eid, const Intersection &intersection) const = 0;
 
-    // process the intersection
+    // handle and process the intersection
     virtual Intersection
     operator()(const NodeID nid, const EdgeID via_eid, Intersection intersection) const = 0;
 

include/extractor/guidance/turn_handler.hpp

@@ -10,6 +10,8 @@
 #include "util/name_table.hpp"
 #include "util/node_based_graph.hpp"
 
+#include <boost/optional.hpp>
+
 #include <cstddef>
 #include <utility>
 #include <vector>
@@ -45,9 +47,24 @@ class TurnHandler : public IntersectionHandler
                             Intersection intersection) const override final;
 
   private:
+    struct Fork
+    {
+        const Intersection::iterator right;
+        const Intersection::iterator left;
+        const std::size_t size;
+        Fork(const Intersection::iterator right, const Intersection::iterator left);
+    };
+
     bool isObviousOfTwo(const EdgeID via_edge,
                         const ConnectedRoad &road,
                         const ConnectedRoad &other) const;
+
+    bool hasObvious(const EdgeID &via_edge, const Fork &fork) const;
+
+    boost::optional<Fork> findForkCandidatesByGeometry(Intersection &intersection) const;
+
+    bool isCompatibleByRoadClass(const Intersection &intersection, const Fork fork) const;
+
     // Dead end.
     OSRM_ATTR_WARN_UNUSED
     Intersection handleOneWayTurn(Intersection intersection) const;
@@ -68,8 +85,7 @@ class TurnHandler : public IntersectionHandler
     handleDistinctConflict(const EdgeID via_edge, ConnectedRoad &left, ConnectedRoad &right) const;
 
     // Classification
-    std::pair<std::size_t, std::size_t> findFork(const EdgeID via_edge,
-                                                 const Intersection &intersection) const;
+    boost::optional<Fork> findFork(const EdgeID via_edge, Intersection &intersection) const;
 
     OSRM_ATTR_WARN_UNUSED
     Intersection assignLeftTurns(const EdgeID via_edge,