diff --git a/data_structures/route_parameters.cpp b/data_structures/route_parameters.cpp index df23ce25a..ad0134067 100644 --- a/data_structures/route_parameters.cpp +++ b/data_structures/route_parameters.cpp @@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include @@ -117,6 +118,18 @@ void RouteParameters::addTimestamp(const unsigned timestamp) } } +void RouteParameters::addBearing(const int bearing, boost::spirit::qi::unused_type unused, bool& pass) +{ + bearings.resize(coordinates.size()); + pass = false; + if (bearing < 0 || bearing > 359) return; + if (!bearings.empty()) + { + bearings.back() = bearing; + pass = true; + } +} + void RouteParameters::setLanguage(const std::string &language_string) { language = language_string; diff --git a/data_structures/static_rtree.hpp b/data_structures/static_rtree.hpp index d1a63fb26..04ff2b09a 100644 --- a/data_structures/static_rtree.hpp +++ b/data_structures/static_rtree.hpp @@ -642,10 +642,55 @@ class StaticRTree return result_coordinate.is_valid(); } + /** + * 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 + * */ + static bool IsBearingWithinBounds(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; + } + } + + + bool IncrementalFindPhantomNodeForCoordinate( const FixedPointCoordinate &input_coordinate, std::vector &result_phantom_node_vector, const unsigned max_number_of_phantom_nodes, + const int filter_bearing = 0, + const int filter_bearing_range = 180, const float max_distance = 1100, const unsigned max_checked_elements = 4 * LEAF_NODE_SIZE) { @@ -737,10 +782,35 @@ class StaticRTree m_coordinate_list->at(current_segment.v), input_coordinate, projected_coordinate, foot_point_coordinate_on_segment, current_ratio); + const float forward_edge_bearing = coordinate_calculation::bearing( + m_coordinate_list->at(current_segment.u), + m_coordinate_list->at(current_segment.v)); + + const float backward_edge_bearing = (forward_edge_bearing + 180) > 360 + ? (forward_edge_bearing - 180) + : (forward_edge_bearing + 180); + + const bool forward_bearing_valid = IsBearingWithinBounds(forward_edge_bearing, filter_bearing, filter_bearing_range); + const bool backward_bearing_valid = IsBearingWithinBounds(backward_edge_bearing, filter_bearing, filter_bearing_range); + + if (!forward_bearing_valid && !backward_bearing_valid) + { + continue; + } + // store phantom node in result vector result_phantom_node_vector.emplace_back(current_segment, foot_point_coordinate_on_segment); + if (!forward_bearing_valid) + { + result_phantom_node_vector.back().forward_node_id = SPECIAL_NODEID; + } + else if (!backward_bearing_valid) + { + result_phantom_node_vector.back().reverse_node_id = SPECIAL_NODEID; + } + // Hack to fix rounding errors and wandering via nodes. FixUpRoundingIssue(input_coordinate, result_phantom_node_vector.back()); @@ -788,6 +858,8 @@ class StaticRTree const FixedPointCoordinate &input_coordinate, std::vector> &result_phantom_node_vector, const double max_distance, + const int filter_bearing = 0, + const int filter_bearing_range = 180, const unsigned max_checked_elements = 4 * LEAF_NODE_SIZE) { unsigned inspected_elements = 0; @@ -881,6 +953,23 @@ class StaticRTree continue; } + const float forward_edge_bearing = coordinate_calculation::bearing( + m_coordinate_list->at(current_segment.u), + m_coordinate_list->at(current_segment.v)); + + const float backward_edge_bearing = (forward_edge_bearing + 180) > 360 + ? (forward_edge_bearing - 180) + : (forward_edge_bearing + 180); + + const bool forward_bearing_valid = IsBearingWithinBounds(forward_edge_bearing, filter_bearing, filter_bearing_range); + const bool backward_bearing_valid = IsBearingWithinBounds(backward_edge_bearing, filter_bearing, filter_bearing_range); + + if (!forward_bearing_valid && !backward_bearing_valid) + { + // This edge doesn't fall within our bearing filter + continue; + } + // store phantom node in result vector result_phantom_node_vector.emplace_back( PhantomNode( @@ -893,6 +982,15 @@ class StaticRTree current_segment.forward_travel_mode, current_segment.backward_travel_mode), current_perpendicular_distance); + if (!forward_bearing_valid) + { + result_phantom_node_vector.back().first.forward_node_id = SPECIAL_NODEID; + } + if (!backward_bearing_valid) + { + result_phantom_node_vector.back().first.reverse_node_id = SPECIAL_NODEID; + } + // Hack to fix rounding errors and wandering via nodes. FixUpRoundingIssue(input_coordinate, result_phantom_node_vector.back().first); diff --git a/include/osrm/route_parameters.hpp b/include/osrm/route_parameters.hpp index 6395ad685..f49d4408d 100644 --- a/include/osrm/route_parameters.hpp +++ b/include/osrm/route_parameters.hpp @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include @@ -71,6 +72,8 @@ struct RouteParameters void addTimestamp(const unsigned timestamp); + void addBearing(const int timestamp, boost::spirit::qi::unused_type unused, bool& pass); + void setLanguage(const std::string &language); void setGeometryFlag(const bool flag); @@ -99,6 +102,7 @@ struct RouteParameters std::string language; std::vector hints; std::vector timestamps; + std::vector bearings; std::vector uturns; std::vector coordinates; }; diff --git a/plugins/match.hpp b/plugins/match.hpp index b749f29e7..55a8c1404 100644 --- a/plugins/match.hpp +++ b/plugins/match.hpp @@ -93,7 +93,8 @@ template class MapMatchingPlugin : public BasePlugin return label_with_confidence; } - bool getCandiates(const std::vector &input_coords, + bool getCandidates(const std::vector &input_coords, + const std::vector &input_bearings, const double gps_precision, std::vector &sub_trace_lengths, osrm::matching::CandidateLists &candidates_lists) @@ -128,8 +129,12 @@ template class MapMatchingPlugin : public BasePlugin } std::vector> candidates; + // Use bearing values if supplied, otherwise fallback to 0,180 defaults + auto bearing = input_bearings.size() > 0 ? input_bearings[current_coordinate] : 0; + auto range = input_bearings.size() > 0 ? 10 : 180; facade->IncrementalFindPhantomNodeForCoordinateWithMaxDistance( - input_coords[current_coordinate], candidates, query_radius); + input_coords[current_coordinate], candidates, query_radius, + bearing, range); // sort by foward id, then by reverse id and then by distance std::sort(candidates.begin(), candidates.end(), @@ -270,12 +275,19 @@ template class MapMatchingPlugin : public BasePlugin osrm::matching::CandidateLists candidates_lists; const auto &input_coords = route_parameters.coordinates; const auto &input_timestamps = route_parameters.timestamps; + const auto &input_bearings = route_parameters.bearings; if (input_timestamps.size() > 0 && input_coords.size() != input_timestamps.size()) { json_result.values["status"] = "Number of timestamps does not match number of coordinates ."; return 400; } + if (input_bearings.size() > 0 && input_coords.size() != input_bearings.size()) + { + json_result.values["status"] = "Number of bearings does not match number of coordinates ."; + return 400; + } + // enforce maximum number of locations for performance reasons if (max_locations_map_matching > 0 && static_cast(input_coords.size()) < max_locations_map_matching) @@ -292,7 +304,7 @@ template class MapMatchingPlugin : public BasePlugin } const bool found_candidates = - getCandiates(input_coords, route_parameters.gps_precision, sub_trace_lengths, candidates_lists); + getCandidates(input_coords, input_bearings, route_parameters.gps_precision, sub_trace_lengths, candidates_lists); if (!found_candidates) { json_result.values["status"] = "No suitable matching candidates found."; diff --git a/plugins/viaroute.hpp b/plugins/viaroute.hpp index f705fb599..c054b0dd2 100644 --- a/plugins/viaroute.hpp +++ b/plugins/viaroute.hpp @@ -80,6 +80,13 @@ template class ViaRoutePlugin final : public BasePlugin return 400; } + const auto &input_bearings = route_parameters.bearings; + if (input_bearings.size() > 0 && route_parameters.coordinates.size() != input_bearings.size()) + { + json_result.values["status"] = "Number of bearings does not match number of coordinates ."; + return 400; + } + std::vector phantom_node_pair_list(route_parameters.coordinates.size()); const bool checksum_OK = (route_parameters.check_sum == facade->GetCheckSum()); @@ -96,8 +103,10 @@ template class ViaRoutePlugin final : public BasePlugin } } std::vector phantom_node_vector; + int bearing = input_bearings.size() > 0 ? input_bearings[i] : 0; + int range = input_bearings.size() > 0 ? 8 : 180; if (facade->IncrementalFindPhantomNodeForCoordinate(route_parameters.coordinates[i], - phantom_node_vector, 1)) + phantom_node_vector, 1, bearing, range)) { BOOST_ASSERT(!phantom_node_vector.empty()); phantom_node_pair_list[i].first = phantom_node_vector.front(); diff --git a/server/api_grammar.hpp b/server/api_grammar.hpp index 325f4c356..f9a22a522 100644 --- a/server/api_grammar.hpp +++ b/server/api_grammar.hpp @@ -40,7 +40,7 @@ template struct APIGrammar : qi::grammar> string[boost::bind(&HandlerT::setService, handler, ::_1)] >> *(query) >> -(uturns); - query = ('?') >> (+(zoom | output | jsonp | checksum | location | hint | timestamp | u | cmp | + query = ('?') >> (+(zoom | output | jsonp | checksum | location | hint | timestamp | bearing | u | cmp | language | instruction | geometry | alt_route | old_API | num_results | matching_beta | gps_precision | classify | locs)); @@ -65,6 +65,8 @@ template struct APIGrammar : qi::grammar> qi::lit("t") >> '=' >> qi::uint_[boost::bind(&HandlerT::addTimestamp, handler, ::_1)]; + bearing = (-qi::lit('&')) >> qi::lit("b") >> '=' >> + qi::int_[boost::bind(&HandlerT::addBearing, handler, ::_1, ::_2, ::_3)]; u = (-qi::lit('&')) >> qi::lit("u") >> '=' >> qi::bool_[boost::bind(&HandlerT::setUTurn, handler, ::_1)]; uturns = (-qi::lit('&')) >> qi::lit("uturns") >> '=' >> @@ -95,7 +97,7 @@ template struct APIGrammar : qi::grammar api_call, query; qi::rule service, zoom, output, string, jsonp, checksum, location, - hint, timestamp, stringwithDot, stringwithPercent, language, instruction, geometry, cmp, alt_route, u, + hint, timestamp, bearing, stringwithDot, stringwithPercent, language, instruction, geometry, cmp, alt_route, u, uturns, old_API, num_results, matching_beta, gps_precision, classify, locs, stringforPolyline; HandlerT *handler; diff --git a/server/data_structures/datafacade_base.hpp b/server/data_structures/datafacade_base.hpp index baea2bf0e..691c6f326 100644 --- a/server/data_structures/datafacade_base.hpp +++ b/server/data_structures/datafacade_base.hpp @@ -99,7 +99,8 @@ template class BaseDataFacade virtual bool IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate, std::vector &resulting_phantom_node_vector, - const unsigned number_of_results) = 0; + const unsigned number_of_results, + const int bearing = 0, const int bearing_range = 180) = 0; virtual bool IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate, @@ -107,7 +108,7 @@ template class BaseDataFacade virtual bool IncrementalFindPhantomNodeForCoordinateWithMaxDistance( const FixedPointCoordinate &input_coordinate, std::vector> &resulting_phantom_node_vector, - const double max_distance) = 0; + const double max_distance, const int bearing = 0, const int bearing_range = 180) = 0; virtual unsigned GetCheckSum() const = 0; diff --git a/server/data_structures/internal_datafacade.hpp b/server/data_structures/internal_datafacade.hpp index 1a88536f6..7d279e8a2 100644 --- a/server/data_structures/internal_datafacade.hpp +++ b/server/data_structures/internal_datafacade.hpp @@ -388,7 +388,8 @@ template class InternalDataFacade final : public BaseDataFacad bool IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate, std::vector &resulting_phantom_node_vector, - const unsigned number_of_results) override final + const unsigned number_of_results, + const int bearing = 0, const int range = 180) override final { if (!m_static_rtree.get()) { @@ -396,13 +397,15 @@ template class InternalDataFacade final : public BaseDataFacad } return m_static_rtree->IncrementalFindPhantomNodeForCoordinate( - input_coordinate, resulting_phantom_node_vector, number_of_results); + input_coordinate, resulting_phantom_node_vector, number_of_results, bearing, range); } bool IncrementalFindPhantomNodeForCoordinateWithMaxDistance( const FixedPointCoordinate &input_coordinate, std::vector> &resulting_phantom_node_vector, - const double max_distance) override final + const double max_distance, + const int bearing = 0, + const int bearing_range = 180) override final { if (!m_static_rtree.get()) { @@ -410,7 +413,7 @@ template class InternalDataFacade final : public BaseDataFacad } return m_static_rtree->IncrementalFindPhantomNodeForCoordinateWithDistance( - input_coordinate, resulting_phantom_node_vector, max_distance); + input_coordinate, resulting_phantom_node_vector, max_distance, bearing, bearing_range); } unsigned GetCheckSum() const override final { return m_check_sum; } diff --git a/server/data_structures/shared_datafacade.hpp b/server/data_structures/shared_datafacade.hpp index ab95235c8..1c929c33b 100644 --- a/server/data_structures/shared_datafacade.hpp +++ b/server/data_structures/shared_datafacade.hpp @@ -409,7 +409,8 @@ template class SharedDataFacade final : public BaseDataFacade< bool IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate, std::vector &resulting_phantom_node_vector, - const unsigned number_of_results) override final + const unsigned number_of_results, + const int bearing = 0, const int range = 180) override final { if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first) { @@ -417,13 +418,15 @@ template class SharedDataFacade final : public BaseDataFacade< } return m_static_rtree->second->IncrementalFindPhantomNodeForCoordinate( - input_coordinate, resulting_phantom_node_vector, number_of_results); + input_coordinate, resulting_phantom_node_vector, number_of_results, bearing, range); } bool IncrementalFindPhantomNodeForCoordinateWithMaxDistance( const FixedPointCoordinate &input_coordinate, std::vector> &resulting_phantom_node_vector, - const double max_distance) override final + const double max_distance, + const int bearing = 0, + const int bearing_range = 180) override final { if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first) { @@ -431,7 +434,7 @@ template class SharedDataFacade final : public BaseDataFacade< } return m_static_rtree->second->IncrementalFindPhantomNodeForCoordinateWithDistance( - input_coordinate, resulting_phantom_node_vector, max_distance); + input_coordinate, resulting_phantom_node_vector, max_distance, bearing, bearing_range); } unsigned GetCheckSum() const override final { return m_check_sum; } diff --git a/unit_tests/data_structures/static_rtree.cpp b/unit_tests/data_structures/static_rtree.cpp index 5a3815e62..10310c346 100644 --- a/unit_tests/data_structures/static_rtree.cpp +++ b/unit_tests/data_structures/static_rtree.cpp @@ -258,6 +258,12 @@ struct GraphFixture TestData d; d.u = pair.first; d.v = pair.second; + // We set the forward nodes to the target node-based-node IDs, just + // so we have something to test against. Because this isn't a real + // graph, the actual values aren't important, we just need something + // to examine during tests. + d.forward_edge_based_node_id = pair.second; + d.reverse_edge_based_node_id = pair.first; edges.emplace_back(d); } } @@ -495,4 +501,100 @@ BOOST_AUTO_TEST_CASE(rectangle_test) TestRectangle(10, 10, 0, 0); } +/* Verify that the bearing-bounds checking function behaves as expected */ +BOOST_AUTO_TEST_CASE(bearing_range_test) +{ + // Simple, non-edge-case checks + BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(45, 45, 10)); + BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(35, 45, 10)); + BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(55, 45, 10)); + + BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(34, 45, 10)); + BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(56, 45, 10)); + + BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(34, 45, 10)); + BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(56, 45, 10)); + + // When angle+limit goes > 360 + BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(359, 355, 10)); + + // When angle-limit goes < 0 + BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(359, 5, 10)); + BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(354, 5, 10)); + BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(16, 5, 10)); + + + // Checking other cases of wraparound + BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(359, -5, 10)); + BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(344, -5, 10)); + BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(6, -5, 10)); + + BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(-1, 5, 10)); + BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(-6, 5, 10)); + + BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(-721, 5, 10)); + BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(719, 5, 10)); + +} + +BOOST_AUTO_TEST_CASE(bearing_tests) +{ + + typedef std::pair Coord; + typedef std::pair Edge; + GraphFixture fixture( + { + Coord(0.0, 0.0), + Coord(10.0, 10.0), + }, + {Edge(0, 1), Edge(1,0)}); + + typedef StaticRTree, false, 2, 3> MiniStaticRTree; + + std::string leaves_path; + std::string nodes_path; + build_rtree("test_bearing", &fixture, leaves_path, nodes_path); + MiniStaticRTree rtree(nodes_path, leaves_path, fixture.coords); + + FixedPointCoordinate input(5.0 * COORDINATE_PRECISION, 5.1 * COORDINATE_PRECISION); + std::vector results; + + rtree.IncrementalFindPhantomNodeForCoordinate( input, results, 5); + BOOST_CHECK_EQUAL(results.size(), 2); + BOOST_CHECK_EQUAL(results.back().forward_node_id, 0); + BOOST_CHECK_EQUAL(results.back().reverse_node_id, 1); + + results.clear(); + rtree.IncrementalFindPhantomNodeForCoordinate( input, results, 5, 270, 10); + BOOST_CHECK_EQUAL(results.size(), 0); + + results.clear(); + rtree.IncrementalFindPhantomNodeForCoordinate( input, results, 5, 45, 10); + BOOST_CHECK_EQUAL(results.size(), 2); + BOOST_CHECK_EQUAL(results[0].forward_node_id, 1); + BOOST_CHECK_EQUAL(results[0].reverse_node_id, SPECIAL_NODEID); + BOOST_CHECK_EQUAL(results[1].forward_node_id, SPECIAL_NODEID); + BOOST_CHECK_EQUAL(results[1].reverse_node_id, 1); + + + std::vector> distance_results; + rtree.IncrementalFindPhantomNodeForCoordinateWithDistance( input, distance_results, 11000 ); + BOOST_CHECK_EQUAL(distance_results.size(), 2); + + distance_results.clear(); + rtree.IncrementalFindPhantomNodeForCoordinateWithDistance( input, distance_results, 11000, 270, 10 ); + BOOST_CHECK_EQUAL(distance_results.size(), 0); + + distance_results.clear(); + rtree.IncrementalFindPhantomNodeForCoordinateWithDistance( input, distance_results, 11000, 45, 10 ); + BOOST_CHECK_EQUAL(distance_results.size(), 2); + BOOST_CHECK_EQUAL(distance_results[0].first.forward_node_id, 1); + BOOST_CHECK_EQUAL(distance_results[0].first.reverse_node_id, SPECIAL_NODEID); + BOOST_CHECK_EQUAL(distance_results[1].first.forward_node_id, SPECIAL_NODEID); + BOOST_CHECK_EQUAL(distance_results[1].first.reverse_node_id, 1); + +} + + + BOOST_AUTO_TEST_SUITE_END()