version using discrete values stored directly at intersection

include/util/guidance/bearing_class.hpp

@@ -36,7 +36,7 @@ class BearingClass
 {
   public:
     using FlagBaseType = std::uint32_t;
-    const static constexpr double discrete_angle_step_size = 360. / 24;
+    const static constexpr double discrete_angle_step_size = 360. / 32;
 
     BearingClass();
 
@@ -46,6 +46,14 @@ class BearingClass
     // add a discrete ID, returns true if no item existed that uses the same discrete angle
     bool addDiscreteID(const std::uint8_t id);
 
+    //remove a bearing from the list
+    void resetContinuous(const double bearing);
+    void resetDiscreteID(const std::uint8_t id);
+
+    //is available
+    bool hasContinuous(const double bearing) const;
+    bool hasDiscrete(const std::uint8_t id) const;
+
     // hashing
     bool operator==(const BearingClass &other) const;
 
@@ -56,7 +64,8 @@ class BearingClass
 
     // 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);
+    static std::uint8_t angleToDiscreteID(double angle);
+    static double discreteIDToAngle( const std::uint8_t );
 
     // we are hiding the access to the flags behind a protection wall, to make sure the bit logic
     // isn't tempered with

include/util/guidance/toolkit.hpp

@@ -56,7 +56,7 @@ inline double getMatchingDiscreteBearing(const bool requires_entry,
         return 0;
 
     const double discrete_bearing =
-        BearingClass::discreteBearingID(bearing) * BearingClass::discrete_angle_step_size;
+        BearingClass::discreteIDToAngle(BearingClass::angleToDiscreteID(bearing));
     // 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)
     {

src/extractor/guidance/turn_classification.cpp

@@ -2,6 +2,10 @@
 
 #include "util/simple_logger.hpp"
 
+#include <cstddef>
+#include <cstdint>
+#include <iomanip>
+
 namespace osrm
 {
 namespace extractor
@@ -11,17 +15,234 @@ namespace guidance
 
 struct TurnPossibility
 {
-    TurnPossibility(bool entry_allowed, double bearing)
+    TurnPossibility(bool entry_allowed, double bearing, std::uint8_t discrete_id)
-        : entry_allowed(entry_allowed), bearing(std::move(bearing))
+        : entry_allowed(entry_allowed), bearing(std::move(bearing)),
+          discrete_id(std::move(discrete_id))
     {
     }
 
-    TurnPossibility() : entry_allowed(false), bearing(0) {}
+    TurnPossibility() : entry_allowed(false), bearing(0), discrete_id(0) {}
 
     bool entry_allowed;
     double bearing;
+    std::uint8_t discrete_id;
 };
 
+namespace
+{
+
+bool hasConflicts(const std::vector<TurnPossibility> &turns)
+{
+    if (turns.size() <= 1)
+        return false;
+    for (std::size_t pos = 0; pos < turns.size(); ++pos)
+    {
+        if (turns[pos].discrete_id == turns[(pos + 1) % turns.size()].discrete_id)
+            return true;
+    }
+    return false;
+}
+
+// Fix cases with nearly identical turns. If the difference between turns is just to small to be
+// visible, we will not report the angle twice
+std::vector<TurnPossibility> fixIdenticalTurns(std::vector<TurnPossibility> intersection)
+{
+    BOOST_ASSERT(intersection.size() > 1);
+    for (auto itr = intersection.begin(); itr != intersection.end(); ++itr)
+    {
+        const auto next = [&]() {
+            auto next_itr = std::next(itr);
+            if (next_itr != intersection.end())
+                return next_itr;
+            return intersection.begin();
+        }();
+
+        // conflict here?
+        if (itr->discrete_id == next->discrete_id)
+        {
+            if (angularDeviation(itr->bearing, next->bearing) < 0.5) // very small angular difference
+            {
+                if (!itr->entry_allowed || next->entry_allowed)
+                {
+                    itr = intersection.erase(itr);
+                }
+                else
+                {
+                    intersection.erase(next);
+                }
+                if (itr == intersection.end())
+                    break;
+            }
+        }
+    }
+    return intersection;
+}
+
+std::vector<TurnPossibility> fixAroundBorder(std::vector<TurnPossibility> intersection)
+{
+    BOOST_ASSERT(intersection.size() > 1);
+    // We can solve a conflict by reporting different bearing availabilities, as long as
+    // both conflicting turns are on different sides of the bearing separator.
+    //
+    // Consider this example:
+    //      ID(0)
+    //         . b
+    // a ....   (ID 1)
+    //         . c
+    //      ID(2)
+    //
+    // Both b and c map to the ID 1. Due to the split, we can set ID 0 and 2. In
+    // deducing the best available bearing for a turn, we can now find 0 to be closest
+    // to b and 2 to be closest to c. This only works when there are no other bearings
+    // close to the conflicting assignment, though.
+    for (std::size_t current_index = 0; current_index < intersection.size(); ++current_index)
+    {
+        const auto next_index = (current_index + 1) % intersection.size();
+        if (intersection[current_index].discrete_id == intersection[next_index].discrete_id)
+        {
+            const double border = util::guidance::BearingClass::discreteIDToAngle(
+                util::guidance::BearingClass::angleToDiscreteID(
+                    intersection[current_index].bearing));
+
+            // if both are on different sides of the separation, we can check for possible
+            // resolution
+            if (intersection[current_index].bearing < border &&
+                intersection[next_index].bearing > border)
+            {
+                const auto shift_angle = [](const double bearing, const double delta) {
+                    auto shifted_angle = bearing + delta;
+                    if (shifted_angle < 0)
+                        return shifted_angle + 360.;
+                    if (shifted_angle > 360)
+                        return shifted_angle - 360.;
+                    return shifted_angle;
+                };
+
+                // conflict resolution is possible, if both bearings are available
+                const auto left_id = util::guidance::BearingClass::angleToDiscreteID(
+                    shift_angle(intersection[current_index].bearing,
+                                -util::guidance::BearingClass::discrete_angle_step_size));
+                const auto right_id = util::guidance::BearingClass::angleToDiscreteID(
+                    shift_angle(intersection[next_index].bearing,
+                                util::guidance::BearingClass::discrete_angle_step_size));
+
+                const bool resolvable = [&]() {
+                    if (intersection.size() == 2)
+                        return true;
+
+                    // cannot shift to the left without generating another conflict
+                    if (intersection[current_index + intersection.size() - 1].discrete_id ==
+                        left_id)
+                        return false;
+
+                    // cannot shift to the right without generating another conflict
+                    if (intersection[(next_index + 1) % intersection.size()].discrete_id ==
+                        right_id)
+                        return false;
+
+                    return true;
+                }();
+
+                if (resolvable)
+                {
+                    intersection[current_index].discrete_id = left_id;
+                    intersection[next_index].discrete_id = right_id;
+                }
+            }
+        }
+    }
+    return intersection;
+}
+
+// return an empty set of turns, if the conflict is not possible to be handled
+std::vector<TurnPossibility> handleConflicts(std::vector<TurnPossibility> intersection)
+{
+    intersection = fixIdenticalTurns(std::move(intersection));
+
+    if (!hasConflicts(intersection))
+        return intersection;
+
+    intersection = fixAroundBorder(intersection);
+
+    // if the intersection still has conflicts, we cannot handle it correctly
+    if (hasConflicts(intersection))
+        intersection.clear();
+
+    return intersection;
+#if 0
+    const auto border = util::guidance::BearingClass::discreteIDToAngle(
+        util::guidance::BearingClass::angleToDiscreteID(intersection[0].bearing));
+
+        // at least two turns
+        auto previous_id = util::guidance::BearingClass::angleToDiscreteID(turns.back().bearing);
+        for (std::size_t i = 0; i < turns.size(); ++i)
+        {
+            if (turns[i].entry_allowed)
+                entry_class.activate(turn_index);
+            const auto discrete_id =
+                util::guidance::BearingClass::angleToDiscreteID(turns[i].bearing);
+
+            const auto prev_index = (i + turns.size() - 1) % turns.size();
+            if (discrete_id != previous_id)
+            {
+                // if we go back in IDs, conflict resolution has to deal with multiple conflicts
+                if (previous_id > discrete_id &&
+                    previous_id != (util::guidance::BearingClass::angleToDiscreteID(
+                                       turns[prev_index].bearing)))
+                {
+                    std::cout << "Previous ID conflict " << (int)previous_id << " "
+                              << (int)discrete_id << std::endl;
+                    break;
+                }
+                ++turn_index;
+                bearing_class.addDiscreteID(discrete_id);
+                previous_id = discrete_id;
+            }
+            else
+            {
+                // the previous turn was handled into a conflict. Such a conflict cannot be
+                // correctly expressed.
+                // We have to report a unclassified setting.
+                if (util::guidance::BearingClass::angleToDiscreteID(turns[prev_index].bearing) !=
+                    previous_id)
+                    break;
+
+                if (turns[i].bearing >= border && turns[prev_index].bearing < border)
+                {
+                    const auto shift_angle = [](const double bearing, const double delta) {
+                        auto shifted_angle = bearing + delta;
+                        if (shifted_angle < 0)
+                            return shifted_angle + 360.;
+                        if (shifted_angle > 360)
+                            return shifted_angle - 360.;
+                        return shifted_angle;
+                    };
+
+                    // conflict resolution is possible, if both bearings are available
+                    const auto left_id = util::guidance::BearingClass::angleToDiscreteID(
+                        shift_angle(turns[prev_index].bearing,
+                                    -util::guidance::BearingClass::discrete_angle_step_size));
+                    const auto right_id = util::guidance::BearingClass::angleToDiscreteID(
+                        shift_angle(turns[i].bearing,
+                                    util::guidance::BearingClass::discrete_angle_step_size));
+                    if (!bearing_class.hasDiscrete(left_id) && !bearing_class.hasDiscrete(right_id))
+                    {
+                        bearing_class.resetDiscreteID(discrete_id);
+                        bearing_class.addDiscreteID(left_id);
+                        bearing_class.addDiscreteID(right_id);
+                        ++turn_index;
+                        previous_id = right_id;
+                    }
+                }
+            }
+        }
+#endif
+
+    return intersection;
+};
+
+} // namespace
+
 std::pair<util::guidance::EntryClass, util::guidance::BearingClass>
 classifyIntersection(NodeID nid,
                      const Intersection &intersection,
@@ -29,6 +250,8 @@ classifyIntersection(NodeID nid,
                      const extractor::CompressedEdgeContainer &compressed_geometries,
                      const std::vector<extractor::QueryNode> &query_nodes)
 {
+    if (intersection.empty())
+        return {};
 
     std::vector<TurnPossibility> turns;
 
@@ -43,37 +266,43 @@ classifyIntersection(NodeID nid,
 
         const double bearing =
             util::coordinate_calculation::bearing(node_coordinate, edge_coordinate);
-        turns.push_back({road.entry_allowed, bearing});
+        turns.push_back({road.entry_allowed, bearing,
+                         util::guidance::BearingClass::angleToDiscreteID(bearing)});
     }
 
-    if (turns.empty())
-        return {};
-
     std::sort(turns.begin(), turns.end(),
               [](const TurnPossibility left, const TurnPossibility right) {
                   return left.bearing < right.bearing;
               });
 
+    // check for conflicts
+    const bool has_conflicts = hasConflicts(turns);
+    if (has_conflicts)
+    { // try to handle conflicts, if possible
+        turns = handleConflicts(std::move(turns));
+    }
+
     util::guidance::EntryClass entry_class;
     util::guidance::BearingClass bearing_class;
 
-    std::size_t turn_index = 0;
+    // finally transfer data to the entry/bearing classes
-    for (std::size_t i = 0; i < turns.size(); ++i)
+    std::size_t number = 0;
+    for (const auto turn : turns)
     {
-        if (turns[i].entry_allowed)
+        if (turn.entry_allowed)
-            entry_class.activate(turn_index);
+            entry_class.activate(number);
-        if (bearing_class.addContinuous(turns[i].bearing))
+        bearing_class.addDiscreteID(turn.discrete_id);
-            ++turn_index;
+        ++number;
     }
 
-    if (turn_index != turns.size())
+    static std::size_t mapping_failure_count = 0;
+    if (turns.empty())
     {
+        ++mapping_failure_count;
         util::SimpleLogger().Write(logDEBUG)
-            << "Failed to provide full turn list for intersection: " << turn_index << " roads of "
+            << "Failed to provide full turn list for intersection ( " << mapping_failure_count
-            << turns.size() << " mapped.";
+            << " ) for " << intersection.size() << " roads";
-        for (auto turn : turns)
+
-            std::cout << " " << (int)turn.bearing << " ("
-                      << (int)util::guidance::BearingClass::discreteBearingID(turn.bearing) << ")";
         std::cout << std::endl;
         for (const auto &road : intersection)
         {
@@ -84,9 +313,14 @@ classifyIntersection(NodeID nid,
 
             const double bearing =
                 util::coordinate_calculation::bearing(node_coordinate, edge_coordinate);
-            std::cout << " " << bearing;
+            std::cout << " " << bearing << "("
+                      << (int)util::guidance::BearingClass::angleToDiscreteID(bearing) << ")";
         }
         std::cout << std::endl;
+        std::cout << "Location of intersection: " << std::setprecision(12) << " "
+                  << util::toFloating(query_nodes[nid].lat) << " "
+                  << util::toFloating(query_nodes[nid].lon) << std::endl;
+        return {};
     }
 
     return std::make_pair(entry_class, bearing_class);

src/util/guidance/bearing_class.cpp

@@ -14,7 +14,7 @@ static_assert(
     360 / BearingClass::discrete_angle_step_size <= 8 * sizeof(BearingClass::FlagBaseType),
     "The number of expressable bearings does not fit into the datatype used for storage.");
 
-std::uint8_t BearingClass::discreteBearingID(double angle)
+std::uint8_t BearingClass::angleToDiscreteID(double angle)
 {
     BOOST_ASSERT(angle >= 0. && angle <= 360.);
     // shift angle by half the step size to have the class be located around the center
@@ -25,6 +25,32 @@ std::uint8_t BearingClass::discreteBearingID(double angle)
     return std::uint8_t(angle / BearingClass::discrete_angle_step_size);
 }
 
+double BearingClass::discreteIDToAngle(std::uint8_t id)
+{
+    BOOST_ASSERT(0 <= id && id <= 360. / discrete_angle_step_size);
+    return discrete_angle_step_size * id;
+}
+
+void BearingClass::resetContinuous(const double bearing) {
+    const auto id = angleToDiscreteID(bearing);
+    resetDiscreteID(id);
+}
+
+void BearingClass::resetDiscreteID(const std::uint8_t id) {
+    available_bearings_mask &= ~(1<<id);
+}
+
+bool BearingClass::hasContinuous(const double bearing) const
+{
+    const auto id = angleToDiscreteID(bearing);
+    return hasDiscrete(id);
+}
+
+bool BearingClass::hasDiscrete(const std::uint8_t id) const
+{
+    return 0 != (available_bearings_mask & (1<<id));
+}
+
 BearingClass::BearingClass() : available_bearings_mask(0) {}
 
 bool BearingClass::operator==(const BearingClass &other) const
@@ -39,7 +65,7 @@ bool BearingClass::operator<(const BearingClass &other) const
 
 bool BearingClass::addContinuous(const double angle)
 {
-    return addDiscreteID(discreteBearingID(angle));
+    return addDiscreteID(angleToDiscreteID(angle));
 }
 
 bool BearingClass::addDiscreteID(const std::uint8_t discrete_id)