initial version of intersection classification

include/util/guidance/bearing_class.hpp

@@ -0,0 +1,87 @@
+#ifndef OSRM_UTIL_GUIDANCE_BEARING_CLASS_HPP_
+#define OSRM_UTIL_GUIDANCE_BEARING_CLASS_HPP_
+
+#include <cstddef>
+#include <cstdint>
+#include <functional>
+#include <vector>
+
+namespace osrm
+{
+namespace util
+{
+namespace guidance
+{
+class BearingClass;
+} // namespace guidance
+} // namespace util
+} // namespace osrm
+
+namespace std
+{
+template <> struct hash<::osrm::util::guidance::BearingClass>
+{
+    inline std::size_t operator()(const ::osrm::util::guidance::BearingClass &bearing_class) const;
+};
+} // namespace std
+
+namespace osrm
+{
+namespace util
+{
+namespace guidance
+{
+
+class BearingClass
+{
+  public:
+    using FlagBaseType = std::uint32_t;
+    const static constexpr double discrete_angle_step_size = 360. / 24;
+
+    BearingClass();
+
+    // add a continuous angle to the, returns true if no item existed that uses the same discrete
+    // angle
+    bool addContinuous(const double bearing);
+    // add a discrete ID, returns true if no item existed that uses the same discrete angle
+    bool addDiscreteID(const std::uint8_t id);
+
+    // hashing
+    bool operator==(const BearingClass &other) const;
+
+    // sorting
+    bool operator<(const BearingClass &other) const;
+
+    std::vector<double> getAvailableBearings() const;
+
+    // get a discrete representation of an angle. Required to map a bearing/angle to the discrete
+    // ones stored within the class
+    static std::uint8_t discreteBearingID(double angle);
+
+    // we are hiding the access to the flags behind a protection wall, to make sure the bit logic
+    // isn't tempered with
+  private:
+    // given a list of possible discrete angles, the available angles flag indicates the presence of
+    // a given turn at the intersection
+    FlagBaseType available_bearings_mask;
+
+    // allow hash access to internal representation
+    friend std::size_t std::hash<BearingClass>::operator()(const BearingClass &) const;
+};
+
+} // namespace guidance
+} // namespace util
+} // namespace osrm
+
+// make Bearing Class hasbable
+namespace std
+{
+inline size_t hash<::osrm::util::guidance::BearingClass>::
+operator()(const ::osrm::util::guidance::BearingClass &bearing_class) const
+{
+    return hash<::osrm::util::guidance::BearingClass::FlagBaseType>()(
+        bearing_class.available_bearings_mask);
+}
+} // namespace std
+
+#endif /* OSRM_UTIL_GUIDANCE_BEARING_CLASS_HPP_ */

include/util/guidance/entry_class.hpp

@@ -0,0 +1,78 @@
+#ifndef OSRM_UTIL_GUIDANCE_ENTRY_CLASS_HPP_
+#define OSRM_UTIL_GUIDANCE_ENTRY_CLASS_HPP_
+
+#include <cstddef>
+#include <cstdint>
+#include <functional>
+
+namespace osrm
+{
+namespace util
+{
+namespace guidance
+{
+class EntryClass;
+} // namespace guidance
+} // namespace util
+} // namespace osrm
+
+namespace std
+{
+template <> struct hash<::osrm::util::guidance::EntryClass>
+{
+    inline std::size_t operator()(const ::osrm::util::guidance::EntryClass &entry_class) const;
+};
+} // namespace std
+
+namespace osrm
+{
+namespace util
+{
+namespace guidance
+{
+
+class EntryClass
+{
+    using FlagBaseType = std::uint32_t;
+
+  public:
+    EntryClass();
+
+    // we are hiding the access to the flags behind a protection wall, to make sure the bit logic
+    // isn't tempered with. zero based indexing
+    void activate(std::uint32_t index);
+
+    // check whether a certain turn allows entry
+    bool allowsEntry(std::uint32_t index) const;
+
+    // required for hashing
+    bool operator==(const EntryClass &) const;
+
+    // sorting
+    bool operator<(const EntryClass &) const;
+
+  private:
+    // given a list of possible discrete angles, the available angles flag indicates the presence of
+    // a given turn at the intersection
+    FlagBaseType enabled_entries_flags;
+
+    // allow hash access to internal representation
+    friend std::size_t std::hash<EntryClass>::operator()(const EntryClass &) const;
+};
+
+} // namespace guidance
+} // namespace utilr
+} // namespace osrm
+
+// make Entry Class hasbable
+namespace std
+{
+inline size_t hash<::osrm::util::guidance::EntryClass>::
+operator()(const ::osrm::util::guidance::EntryClass &entry_class) const
+{
+    return hash<::osrm::util::guidance::EntryClass::FlagBaseType>()(
+        entry_class.enabled_entries_flags);
+}
+} // namespace std
+
+#endif /* OSRM_UTIL_GUIDANCE_ENTRY_CLASS_HPP_ */

include/util/guidance/toolkit.hpp

@@ -4,6 +4,12 @@
 /* A set of tools required for guidance in both pre and post-processing */
 
 #include "extractor/guidance/turn_instruction.hpp"
+#include "util/guidance/bearing_class.hpp"
+#include "util/guidance/entry_class.hpp"
+#include "util/simple_logger.hpp"
+
+#include <algorithm>
+#include <vector>
 
 namespace osrm
 {
@@ -41,8 +47,76 @@ inline extractor::guidance::DirectionModifier getTurnDirection(const double angl
     return extractor::guidance::DirectionModifier::UTurn;
 }
 
-} /* namespace guidance */
-} /* namespace util */
-} /* namespace osrm */
+inline double getMatchingDiscreteBearing(const bool requires_entry,
+                                  const double bearing,
+                                  const EntryClass entry_class,
+                                  const std::vector<double> existing_bearings)
+{
+    if (existing_bearings.empty())
+        return 0;
+
+    const double discrete_bearing =
+        BearingClass::discreteBearingID(bearing) * BearingClass::discrete_angle_step_size;
+    // it they are very close to the turn, the discrete bearing should be fine
+    if (std::abs(bearing - discrete_bearing) < 0.25 * BearingClass::discrete_angle_step_size)
+    {
+        const auto isValidEntry = [&]() {
+            const auto bound = std::upper_bound(existing_bearings.begin(), existing_bearings.end(),
+                                                (discrete_bearing - 0.001));
+            const auto index = bound == existing_bearings.end()
+                                   ? 0
+                                   : std::distance(existing_bearings.begin(), bound);
+
+            return entry_class.allowsEntry(index);
+        };
+        BOOST_ASSERT(!requires_entry || isValidEntry());
+        return discrete_bearing;
+    }
+    else
+    {
+        // the next larger bearing or first if we are wrapping around at zero
+        const auto next_index =
+            std::distance(existing_bearings.begin(),
+                          std::lower_bound(existing_bearings.begin(), existing_bearings.end(),
+                                           discrete_bearing)) %
+            existing_bearings.size();
+
+        // next smaller bearing or last if we are wrapping around at zero
+        const auto previous_index =
+            (next_index + existing_bearings.size() - 1) % existing_bearings.size();
+
+        const auto difference = [](const double first, const double second) {
+            return std::min(std::abs(first - second), 360.0 - std::abs(first - second));
+        };
+
+        const auto next_bearing = existing_bearings[next_index];
+        const auto previous_bearing = existing_bearings[previous_index];
+
+        const auto decideOnBearing = [&](
+            const std::size_t preferred_index, const double preferred_bearing,
+            const std::size_t alternative_index, const double alternative_bearing) {
+            if (!requires_entry || entry_class.allowsEntry(preferred_index))
+                return preferred_bearing;
+            else if (entry_class.allowsEntry(alternative_index))
+                return alternative_bearing;
+            else
+            {
+                SimpleLogger().Write(logDEBUG)
+                    << "Cannot find a valid entry for a discrete Bearing in Turn Classificiation";
+                return 0.;
+            }
+        };
+
+        if (difference(bearing, next_bearing) < difference(bearing, previous_index))
+            return decideOnBearing(next_index, next_bearing, previous_index, previous_bearing);
+        else
+            return decideOnBearing(previous_index, previous_bearing, next_index, next_bearing);
+    }
+
+    return 0;
+}
+} // namespace guidance
+} // namespace util
+} // namespace osrm
 
 #endif /* OSRM_UTIL_GUIDANCE_TOOLKIT_HPP_ */