refactor of turn analysis into turn handlers

src/extractor/guidance/intersection_generator.cpp

@@ -0,0 +1,255 @@
+#include "extractor/guidance/constants.hpp"
+#include "extractor/guidance/intersection_generator.hpp"
+#include "extractor/guidance/toolkit.hpp"
+
+#include <algorithm>
+#include <iterator>
+#include <limits>
+#include <utility>
+
+namespace osrm
+{
+namespace extractor
+{
+namespace guidance
+{
+
+IntersectionGenerator::IntersectionGenerator(const util::NodeBasedDynamicGraph &node_based_graph,
+                      const RestrictionMap &restriction_map,
+                      const std::unordered_set<NodeID> &barrier_nodes,
+                      const std::vector<QueryNode> &node_info_list,
+                      const CompressedEdgeContainer &compressed_edge_container)
+    : node_based_graph(node_based_graph), restriction_map(restriction_map),
+      barrier_nodes(barrier_nodes), node_info_list(node_info_list),
+      compressed_edge_container(compressed_edge_container)
+{
+}
+
+Intersection IntersectionGenerator::operator()(const NodeID from_node, const EdgeID via_eid) const
+{
+    return getConnectedRoads(from_node, via_eid);
+}
+
+//                                               a
+//                                               |
+//                                               |
+//                                               v
+// For an intersection from_node --via_edi--> turn_node ----> c
+//                                               ^
+//                                               |
+//                                               |
+//                                               b
+// This functions returns _all_ turns as if the graph was undirected.
+// That means we not only get (from_node, turn_node, c) in the above example
+// but also (from_node, turn_node, a), (from_node, turn_node, b). These turns are
+// marked as invalid and only needed for intersection classification.
+Intersection IntersectionGenerator::getConnectedRoads(const NodeID from_node,
+                                                      const EdgeID via_eid) const
+{
+    Intersection intersection;
+    const NodeID turn_node = node_based_graph.GetTarget(via_eid);
+    const NodeID only_restriction_to_node =
+        restriction_map.CheckForEmanatingIsOnlyTurn(from_node, turn_node);
+    const bool is_barrier_node = barrier_nodes.find(turn_node) != barrier_nodes.end();
+
+    bool has_uturn_edge = false;
+    for (const EdgeID onto_edge : node_based_graph.GetAdjacentEdgeRange(turn_node))
+    {
+        BOOST_ASSERT(onto_edge != SPECIAL_EDGEID);
+        const NodeID to_node = node_based_graph.GetTarget(onto_edge);
+
+        bool turn_is_valid =
+            // reverse edges are never valid turns because the resulting turn would look like this:
+            // from_node --via_edge--> turn_node <--onto_edge-- to_node
+            // however we need this for capture intersection shape for incoming one-ways
+            !node_based_graph.GetEdgeData(onto_edge).reversed &&
+            // we are not turning over a barrier
+            (!is_barrier_node || from_node == to_node) &&
+            // We are at an only_-restriction but not at the right turn.
+            (only_restriction_to_node == SPECIAL_NODEID || to_node == only_restriction_to_node) &&
+            // the turn is not restricted
+            !restriction_map.CheckIfTurnIsRestricted(from_node, turn_node, to_node);
+
+        auto angle = 0.;
+        if (from_node == to_node)
+        {
+            if (turn_is_valid && !is_barrier_node)
+            {
+                // we only add u-turns for dead-end streets.
+                if (node_based_graph.GetOutDegree(turn_node) > 1)
+                {
+                    auto number_of_emmiting_bidirectional_edges = 0;
+                    for (auto edge : node_based_graph.GetAdjacentEdgeRange(turn_node))
+                    {
+                        auto target = node_based_graph.GetTarget(edge);
+                        auto reverse_edge = node_based_graph.FindEdge(target, turn_node);
+                        BOOST_ASSERT(reverse_edge != SPECIAL_EDGEID);
+                        if (!node_based_graph.GetEdgeData(reverse_edge).reversed)
+                        {
+                            ++number_of_emmiting_bidirectional_edges;
+                        }
+                    }
+                    // is a dead-end
+                    turn_is_valid = number_of_emmiting_bidirectional_edges <= 1;
+                }
+            }
+            has_uturn_edge = true;
+            BOOST_ASSERT(angle >= 0. && angle < std::numeric_limits<double>::epsilon());
+        }
+        else
+        {
+            // unpack first node of second segment if packed
+            const auto first_coordinate = getRepresentativeCoordinate(
+                from_node, turn_node, via_eid, INVERT, compressed_edge_container, node_info_list);
+            const auto third_coordinate = getRepresentativeCoordinate(
+                turn_node, to_node, onto_edge, !INVERT, compressed_edge_container, node_info_list);
+            angle = util::coordinate_calculation::computeAngle(
+                first_coordinate, node_info_list[turn_node], third_coordinate);
+            if (angle < std::numeric_limits<double>::epsilon())
+                has_uturn_edge = true;
+        }
+
+        intersection.push_back(ConnectedRoad(
+            TurnOperation{onto_edge, angle, {TurnType::Invalid, DirectionModifier::UTurn}},
+            turn_is_valid));
+    }
+
+    // We hit the case of a street leading into nothing-ness. Since the code here assumes that this
+    // will
+    // never happen we add an artificial invalid uturn in this case.
+    if (!has_uturn_edge)
+    {
+        intersection.push_back(
+            {TurnOperation{via_eid, 0., {TurnType::Invalid, DirectionModifier::UTurn}}, false});
+    }
+
+    const auto ByAngle = [](const ConnectedRoad &first, const ConnectedRoad second) {
+        return first.turn.angle < second.turn.angle;
+    };
+    std::sort(std::begin(intersection), std::end(intersection), ByAngle);
+
+    BOOST_ASSERT(intersection[0].turn.angle >= 0. &&
+                 intersection[0].turn.angle < std::numeric_limits<double>::epsilon());
+
+    return mergeSegregatedRoads(std::move(intersection));
+}
+
+/*
+ * Segregated Roads often merge onto a single intersection.
+ * While technically representing different roads, they are
+ * often looked at as a single road.
+ * Due to the merging, turn Angles seem off, wenn we compute them from the
+ * initial positions.
+ *
+ *         b<b<b<b(1)<b<b<b
+ * aaaaa-b
+ *         b>b>b>b(2)>b>b>b
+ *
+ * Would be seen as a slight turn going fro a to (2). A Sharp turn going from
+ * (1) to (2).
+ *
+ * In cases like these, we megre this segregated roads into a single road to
+ * end up with a case like:
+ *
+ * aaaaa-bbbbbb
+ *
+ * for the turn representation.
+ * Anything containing the first u-turn in a merge affects all other angles
+ * and is handled separately from all others.
+ */
+Intersection IntersectionGenerator::mergeSegregatedRoads(Intersection intersection) const
+{
+    const auto getRight = [&](std::size_t index) {
+        return (index + intersection.size() - 1) % intersection.size();
+    };
+
+    const auto mergable = [&](std::size_t first, std::size_t second) -> bool {
+        const auto &first_data = node_based_graph.GetEdgeData(intersection[first].turn.eid);
+        const auto &second_data = node_based_graph.GetEdgeData(intersection[second].turn.eid);
+
+        return first_data.name_id != INVALID_NAME_ID && first_data.name_id == second_data.name_id &&
+               !first_data.roundabout && !second_data.roundabout &&
+               first_data.travel_mode == second_data.travel_mode &&
+               first_data.road_classification == second_data.road_classification &&
+               // compatible threshold
+               angularDeviation(intersection[first].turn.angle, intersection[second].turn.angle) <
+                   60 &&
+               first_data.reversed != second_data.reversed;
+    };
+
+    const auto merge = [](const ConnectedRoad &first,
+                          const ConnectedRoad &second) -> ConnectedRoad {
+        if (!first.entry_allowed)
+        {
+            ConnectedRoad result = second;
+            result.turn.angle = (first.turn.angle + second.turn.angle) / 2;
+            if (first.turn.angle - second.turn.angle > 180)
+                result.turn.angle += 180;
+            if (result.turn.angle > 360)
+                result.turn.angle -= 360;
+
+            return result;
+        }
+        else
+        {
+            BOOST_ASSERT(!second.entry_allowed);
+            ConnectedRoad result = first;
+            result.turn.angle = (first.turn.angle + second.turn.angle) / 2;
+
+            if (first.turn.angle - second.turn.angle > 180)
+                result.turn.angle += 180;
+            if (result.turn.angle > 360)
+                result.turn.angle -= 360;
+
+            return result;
+        }
+    };
+    if (intersection.size() == 1)
+        return intersection;
+
+    // check for merges including the basic u-turn
+    // these result in an adjustment of all other angles
+    if (mergable(0, intersection.size() - 1))
+    {
+        const double correction_factor =
+            (360 - intersection[intersection.size() - 1].turn.angle) / 2;
+        for (std::size_t i = 1; i + 1 < intersection.size(); ++i)
+            intersection[i].turn.angle += correction_factor;
+        intersection[0] = merge(intersection.front(), intersection.back());
+        intersection[0].turn.angle = 0;
+        intersection.pop_back();
+    }
+
+    else if (mergable(0, 1))
+    {
+        const double correction_factor = (intersection[1].turn.angle) / 2;
+        for (std::size_t i = 2; i < intersection.size(); ++i)
+            intersection[i].turn.angle += correction_factor;
+        intersection[0] = merge(intersection[0], intersection[1]);
+        intersection[0].turn.angle = 0;
+        intersection.erase(intersection.begin() + 1);
+    }
+
+    // a merge including the first u-turn requres an adjustment of the turn angles
+    // therefore these are handled prior to this step
+    for (std::size_t index = 2; index < intersection.size(); ++index)
+    {
+        if (mergable(index, getRight(index)))
+        {
+            intersection[getRight(index)] =
+                merge(intersection[getRight(index)], intersection[index]);
+            intersection.erase(intersection.begin() + index);
+            --index;
+        }
+    }
+
+    const auto ByAngle = [](const ConnectedRoad &first, const ConnectedRoad second) {
+        return first.turn.angle < second.turn.angle;
+    };
+    std::sort(std::begin(intersection), std::end(intersection), ByAngle);
+    return intersection;
+}
+
+} // namespace guidance
+} // namespace extractor
+} // namespace osrm