Refactor StaticRTree to remove application dependent code

StaticRTree now acts like a container, just returning the input data
(NodeBasedEdge) and not PhantomNodes.

util/bearing.hpp

@@ -72,6 +72,45 @@ inline std::string get(const double heading)
     }
     return "N";
 }
+
+// Checks whether A is between B-range and B+range, all modulo 360
+// e.g. A = 5, B = 5, range = 10 == true
+//      A = -6, B = 5, range = 10 == false
+//      A = -2, B = 355, range = 10 == true
+//      A = 6, B = 355, range = 10 == false
+//      A = 355, B = -2, range = 10 == true
+//
+// @param A the bearing to check, in degrees, 0-359, 0=north
+// @param B the bearing to check against, in degrees, 0-359, 0=north
+// @param range the number of degrees either side of B that A will still match
+// @return true if B-range <= A <= B+range, modulo 360
+inline bool CheckInBounds(const int A, const int B, const int range)
+{
+
+    if (range >= 180)
+        return true;
+    if (range <= 0)
+        return false;
+
+    // Map both bearings into positive modulo 360 space
+    const int normalized_B = (B < 0) ? (B % 360) + 360 : (B % 360);
+    const int normalized_A = (A < 0) ? (A % 360) + 360 : (A % 360);
+
+    if (normalized_B - range < 0)
+    {
+        return (normalized_B - range + 360 <= normalized_A && normalized_A < 360) ||
+               (0 <= normalized_A && normalized_A <= normalized_B + range);
+    }
+    else if (normalized_B + range > 360)
+    {
+        return (normalized_B - range <= normalized_A && normalized_A < 360) ||
+               (0 <= normalized_A && normalized_A <= normalized_B + range - 360);
+    }
+    else
+    {
+        return normalized_B - range <= normalized_A && normalized_A <= normalized_B + range;
+    }
+}
 }
 
 #endif // BEARING_HPP

util/matching_debug_info.hpp

@@ -62,8 +62,8 @@ struct MatchingDebugInfo
                 osrm::json::Object state;
                 state.values["transitions"] = osrm::json::Array();
                 state.values["coordinate"] =
-                    osrm::json::make_array(elem_s.first.location.lat / COORDINATE_PRECISION,
-                                           elem_s.first.location.lon / COORDINATE_PRECISION);
+                    osrm::json::make_array(elem_s.second.location.lat / COORDINATE_PRECISION,
+                                           elem_s.second.location.lon / COORDINATE_PRECISION);
                 state.values["viterbi"] =
                     osrm::json::clamp_float(osrm::matching::IMPOSSIBLE_LOG_PROB);
                 state.values["pruned"] = 0u;