From 30a9bc317917ddbf4fe58ec51a0bb35a24a711df Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Mon, 28 Mar 2016 20:38:19 +0200 Subject: [PATCH] Fix mathematical assumptions in StaticRTree StaticRTree now uses projected coordinates internally. That means we can use a euclidean distance measure (squared distance) for sorting the query queue. --- include/engine/geospatial_query.hpp | 231 ++++++++++++--------- include/util/coordinate.hpp | 22 ++ include/util/coordinate_calculation.hpp | 46 ++-- include/util/rectangle.hpp | 52 ++--- include/util/static_rtree.hpp | 120 +++++++---- src/engine/plugins/tile.cpp | 6 +- src/util/coordinate.cpp | 44 ++++ src/util/coordinate_calculation.cpp | 190 +++++++---------- unit_tests/util/coordinate.cpp | 92 -------- unit_tests/util/coordinate_calculation.cpp | 191 +++++++++++++++++ unit_tests/util/rectangle.cpp | 110 ++++++---- unit_tests/util/static_rtree.cpp | 183 +++++++--------- 12 files changed, 717 insertions(+), 570 deletions(-) delete mode 100644 unit_tests/util/coordinate.cpp create mode 100644 unit_tests/util/coordinate_calculation.cpp diff --git a/include/engine/geospatial_query.hpp b/include/engine/geospatial_query.hpp index b22032ed1..e880139f3 100644 --- a/include/engine/geospatial_query.hpp +++ b/include/engine/geospatial_query.hpp @@ -26,6 +26,7 @@ template class GeospatialQuery { using EdgeData = typename RTreeT::EdgeData; using CoordinateList = typename RTreeT::CoordinateList; + using CandidateSegment = typename RTreeT::CandidateSegment; public: GeospatialQuery(RTreeT &rtree_, @@ -45,15 +46,17 @@ template class GeospatialQuery std::vector NearestPhantomNodesInRange(const util::Coordinate input_coordinate, const double max_distance) { - auto results = rtree.Nearest(input_coordinate, - [](const EdgeData &) - { - return std::make_pair(true, true); - }, - [max_distance](const std::size_t, const double min_dist) - { - return min_dist > max_distance; - }); + auto results = + rtree.Nearest(input_coordinate, + [](const CandidateSegment &) + { + return std::make_pair(true, true); + }, + [this, max_distance, input_coordinate](const std::size_t, + const CandidateSegment &segment) + { + return checkSegmentDistance(input_coordinate, segment, max_distance); + }); return MakePhantomNodes(input_coordinate, results); } @@ -66,16 +69,17 @@ template class GeospatialQuery const int bearing, const int bearing_range) { - auto results = - rtree.Nearest(input_coordinate, - [this, bearing, bearing_range, max_distance](const EdgeData &data) - { - return checkSegmentBearing(data, bearing, bearing_range); - }, - [max_distance](const std::size_t, const double min_dist) - { - return min_dist > max_distance; - }); + auto results = rtree.Nearest( + input_coordinate, + [this, bearing, bearing_range, max_distance](const CandidateSegment &segment) + { + return checkSegmentBearing(segment, bearing, bearing_range); + }, + [this, max_distance, input_coordinate](const std::size_t, + const CandidateSegment &segment) + { + return checkSegmentDistance(input_coordinate, segment, max_distance); + }); return MakePhantomNodes(input_coordinate, results); } @@ -88,15 +92,16 @@ template class GeospatialQuery const int bearing, const int bearing_range) { - auto results = rtree.Nearest(input_coordinate, - [this, bearing, bearing_range](const EdgeData &data) - { - return checkSegmentBearing(data, bearing, bearing_range); - }, - [max_results](const std::size_t num_results, const double) - { - return num_results >= max_results; - }); + auto results = + rtree.Nearest(input_coordinate, + [this, bearing, bearing_range](const CandidateSegment &segment) + { + return checkSegmentBearing(segment, bearing, bearing_range); + }, + [max_results](const std::size_t num_results, const CandidateSegment &) + { + return num_results >= max_results; + }); return MakePhantomNodes(input_coordinate, results); } @@ -111,16 +116,18 @@ template class GeospatialQuery const int bearing, const int bearing_range) { - auto results = rtree.Nearest( - input_coordinate, - [this, bearing, bearing_range](const EdgeData &data) - { - return checkSegmentBearing(data, bearing, bearing_range); - }, - [max_results, max_distance](const std::size_t num_results, const double min_dist) - { - return num_results >= max_results || min_dist > max_distance; - }); + auto results = + rtree.Nearest(input_coordinate, + [this, bearing, bearing_range](const CandidateSegment &segment) + { + return checkSegmentBearing(segment, bearing, bearing_range); + }, + [this, max_distance, max_results, input_coordinate]( + const std::size_t num_results, const CandidateSegment &segment) + { + return num_results >= max_results || + checkSegmentDistance(input_coordinate, segment, max_distance); + }); return MakePhantomNodes(input_coordinate, results); } @@ -130,15 +137,16 @@ template class GeospatialQuery std::vector NearestPhantomNodes(const util::Coordinate input_coordinate, const unsigned max_results) { - auto results = rtree.Nearest(input_coordinate, - [](const EdgeData &) - { - return std::make_pair(true, true); - }, - [max_results](const std::size_t num_results, const double) - { - return num_results >= max_results; - }); + auto results = + rtree.Nearest(input_coordinate, + [](const CandidateSegment &) + { + return std::make_pair(true, true); + }, + [max_results](const std::size_t num_results, const CandidateSegment &) + { + return num_results >= max_results; + }); return MakePhantomNodes(input_coordinate, results); } @@ -150,16 +158,18 @@ template class GeospatialQuery const unsigned max_results, const double max_distance) { - auto results = rtree.Nearest( - input_coordinate, - [](const EdgeData &) - { - return std::make_pair(true, true); - }, - [max_results, max_distance](const std::size_t num_results, const double min_dist) - { - return num_results >= max_results || min_dist > max_distance; - }); + auto results = + rtree.Nearest(input_coordinate, + [](const CandidateSegment &) + { + return std::make_pair(true, true); + }, + [this, max_distance, max_results, input_coordinate]( + const std::size_t num_results, const CandidateSegment &segment) + { + return num_results >= max_results || + checkSegmentDistance(input_coordinate, segment, max_distance); + }); return MakePhantomNodes(input_coordinate, results); } @@ -174,20 +184,22 @@ template class GeospatialQuery bool has_big_component = false; auto results = rtree.Nearest( input_coordinate, - [&has_big_component, &has_small_component](const EdgeData &data) + [&has_big_component, &has_small_component](const CandidateSegment &segment) { - auto use_segment = - (!has_small_component || (!has_big_component && !data.component.is_tiny)); + auto use_segment = (!has_small_component || + (!has_big_component && !segment.data.component.is_tiny)); auto use_directions = std::make_pair(use_segment, use_segment); - has_big_component = has_big_component || !data.component.is_tiny; - has_small_component = has_small_component || data.component.is_tiny; + has_big_component = has_big_component || !segment.data.component.is_tiny; + has_small_component = has_small_component || segment.data.component.is_tiny; return use_directions; }, - [&has_big_component, max_distance](const std::size_t num_results, const double min_dist) + [this, &has_big_component, max_distance, + input_coordinate](const std::size_t num_results, const CandidateSegment &segment) { - return (num_results > 0 && has_big_component) || min_dist > max_distance; + return (num_results > 0 && has_big_component) || + checkSegmentDistance(input_coordinate, segment, max_distance); }); if (results.size() == 0) @@ -207,23 +219,23 @@ template class GeospatialQuery { bool has_small_component = false; bool has_big_component = false; - auto results = - rtree.Nearest(input_coordinate, - [&has_big_component, &has_small_component](const EdgeData &data) - { - auto use_segment = (!has_small_component || - (!has_big_component && !data.component.is_tiny)); - auto use_directions = std::make_pair(use_segment, use_segment); + auto results = rtree.Nearest( + input_coordinate, + [&has_big_component, &has_small_component](const CandidateSegment &segment) + { + auto use_segment = (!has_small_component || + (!has_big_component && !segment.data.component.is_tiny)); + auto use_directions = std::make_pair(use_segment, use_segment); - has_big_component = has_big_component || !data.component.is_tiny; - has_small_component = has_small_component || data.component.is_tiny; + has_big_component = has_big_component || !segment.data.component.is_tiny; + has_small_component = has_small_component || segment.data.component.is_tiny; - return use_directions; - }, - [&has_big_component](const std::size_t num_results, const double) - { - return num_results > 0 && has_big_component; - }); + return use_directions; + }, + [&has_big_component](const std::size_t num_results, const CandidateSegment &) + { + return num_results > 0 && has_big_component; + }); if (results.size() == 0) { @@ -245,25 +257,25 @@ template class GeospatialQuery auto results = rtree.Nearest( input_coordinate, [this, bearing, bearing_range, &has_big_component, - &has_small_component](const EdgeData &data) + &has_small_component](const CandidateSegment &segment) { - auto use_segment = - (!has_small_component || (!has_big_component && !data.component.is_tiny)); + auto use_segment = (!has_small_component || + (!has_big_component && !segment.data.component.is_tiny)); auto use_directions = std::make_pair(use_segment, use_segment); if (use_segment) { - use_directions = checkSegmentBearing(data, bearing, bearing_range); + use_directions = checkSegmentBearing(segment, bearing, bearing_range); if (use_directions.first || use_directions.second) { - has_big_component = has_big_component || !data.component.is_tiny; - has_small_component = has_small_component || data.component.is_tiny; + has_big_component = has_big_component || !segment.data.component.is_tiny; + has_small_component = has_small_component || segment.data.component.is_tiny; } } return use_directions; }, - [&has_big_component](const std::size_t num_results, const double) + [&has_big_component](const std::size_t num_results, const CandidateSegment &) { return num_results > 0 && has_big_component; }); @@ -291,27 +303,29 @@ template class GeospatialQuery auto results = rtree.Nearest( input_coordinate, [this, bearing, bearing_range, &has_big_component, - &has_small_component](const EdgeData &data) + &has_small_component](const CandidateSegment &segment) { - auto use_segment = - (!has_small_component || (!has_big_component && !data.component.is_tiny)); + auto use_segment = (!has_small_component || + (!has_big_component && !segment.data.component.is_tiny)); auto use_directions = std::make_pair(use_segment, use_segment); if (use_segment) { - use_directions = checkSegmentBearing(data, bearing, bearing_range); + use_directions = checkSegmentBearing(segment, bearing, bearing_range); if (use_directions.first || use_directions.second) { - has_big_component = has_big_component || !data.component.is_tiny; - has_small_component = has_small_component || data.component.is_tiny; + has_big_component = has_big_component || !segment.data.component.is_tiny; + has_small_component = has_small_component || segment.data.component.is_tiny; } } return use_directions; }, - [&has_big_component, max_distance](const std::size_t num_results, const double min_dist) + [this, &has_big_component, max_distance, + input_coordinate](const std::size_t num_results, const CandidateSegment &segment) { - return (num_results > 0 && has_big_component) || min_dist > max_distance; + return (num_results > 0 && has_big_component) || + checkSegmentDistance(input_coordinate, segment, max_distance); }); if (results.size() == 0) @@ -401,15 +415,32 @@ template class GeospatialQuery return transformed; } - std::pair checkSegmentBearing(const EdgeData &segment, + bool checkSegmentDistance(const Coordinate input_coordinate, + const CandidateSegment &segment, + const double max_distance) + { + BOOST_ASSERT(segment.data.forward_segment_id.id != SPECIAL_SEGMENTID || + !segment.data.forward_segment_id.enabled); + BOOST_ASSERT(segment.data.reverse_segment_id.id != SPECIAL_SEGMENTID || + !segment.data.reverse_segment_id.enabled); + + Coordinate wsg84_coordinate = util::coordinate_calculation::mercator::toWGS84( + segment.fixed_projected_coordinate); + + return util::coordinate_calculation::haversineDistance(input_coordinate, wsg84_coordinate) > max_distance; + } + + std::pair checkSegmentBearing(const CandidateSegment &segment, const int filter_bearing, const int filter_bearing_range) { - BOOST_ASSERT(segment.forward_segment_id.id != SPECIAL_SEGMENTID || !segment.forward_segment_id.enabled); - BOOST_ASSERT(segment.reverse_segment_id.id != SPECIAL_SEGMENTID || !segment.reverse_segment_id.enabled); + BOOST_ASSERT(segment.data.forward_segment_id.id != SPECIAL_SEGMENTID || + !segment.data.forward_segment_id.enabled); + BOOST_ASSERT(segment.data.reverse_segment_id.id != SPECIAL_SEGMENTID || + !segment.data.reverse_segment_id.enabled); const double forward_edge_bearing = util::coordinate_calculation::bearing( - coordinates->at(segment.u), coordinates->at(segment.v)); + coordinates->at(segment.data.u), coordinates->at(segment.data.v)); const double backward_edge_bearing = (forward_edge_bearing + 180) > 360 ? (forward_edge_bearing - 180) @@ -418,11 +449,11 @@ template class GeospatialQuery const bool forward_bearing_valid = util::bearing::CheckInBounds(std::round(forward_edge_bearing), filter_bearing, filter_bearing_range) && - segment.forward_segment_id.enabled; + segment.data.forward_segment_id.enabled; const bool backward_bearing_valid = util::bearing::CheckInBounds(std::round(backward_edge_bearing), filter_bearing, filter_bearing_range) && - segment.reverse_segment_id.enabled; + segment.data.reverse_segment_id.enabled; return std::make_pair(forward_bearing_valid, backward_bearing_valid); } diff --git a/include/util/coordinate.hpp b/include/util/coordinate.hpp index e0cf88c47..d8e41a52e 100644 --- a/include/util/coordinate.hpp +++ b/include/util/coordinate.hpp @@ -78,6 +78,8 @@ inline FloatLongitude toFloating(const FixedLongitude fixed) return FloatLongitude(floating); } +struct FloatCoordinate; + // Coordinate encoded as longitude, latitude struct Coordinate { @@ -85,6 +87,7 @@ struct Coordinate FixedLatitude lat; Coordinate(); + Coordinate(const FloatCoordinate &other); Coordinate(const FixedLongitude lon_, const FixedLatitude lat_); Coordinate(const FloatLongitude lon_, const FloatLatitude lat_); @@ -104,8 +107,27 @@ struct Coordinate friend std::ostream &operator<<(std::ostream &out, const Coordinate coordinate); }; +// Coordinate encoded as longitude, latitude +struct FloatCoordinate +{ + FloatLongitude lon; + FloatLatitude lat; + + FloatCoordinate(); + FloatCoordinate(const FixedLongitude lon_, const FixedLatitude lat_); + FloatCoordinate(const FloatLongitude lon_, const FloatLatitude lat_); + FloatCoordinate(const Coordinate other); + + bool IsValid() const; + friend bool operator==(const FloatCoordinate lhs, const FloatCoordinate rhs); + friend bool operator!=(const FloatCoordinate lhs, const FloatCoordinate rhs); + friend std::ostream &operator<<(std::ostream &out, const FloatCoordinate coordinate); +}; + bool operator==(const Coordinate lhs, const Coordinate rhs); +bool operator==(const FloatCoordinate lhs, const FloatCoordinate rhs); std::ostream &operator<<(std::ostream &out, const Coordinate coordinate); +std::ostream &operator<<(std::ostream &out, const FloatCoordinate coordinate); } } diff --git a/include/util/coordinate_calculation.hpp b/include/util/coordinate_calculation.hpp index 8dba68406..806b538ac 100644 --- a/include/util/coordinate_calculation.hpp +++ b/include/util/coordinate_calculation.hpp @@ -24,22 +24,26 @@ const constexpr double EARTH_RADIUS_WGS84 = 6378137.0; namespace detail { - // earth circumference devided by 2 - const constexpr double MAXEXTENT = EARTH_RADIUS_WGS84 * boost::math::constants::pi(); - // ^ math functions are not constexpr since they have side-effects (setting errno) :( - const double MAX_LATITUDE = RAD_TO_DEGREE * (2.0 * std::atan(std::exp(180.0 * DEGREE_TO_RAD)) - boost::math::constants::half_pi()); - const constexpr double MAX_LONGITUDE = 180.0; +// earth circumference devided by 2 +const constexpr double MAXEXTENT = EARTH_RADIUS_WGS84 * boost::math::constants::pi(); +// ^ math functions are not constexpr since they have side-effects (setting errno) :( +const double MAX_LATITUDE = RAD_TO_DEGREE * (2.0 * std::atan(std::exp(180.0 * DEGREE_TO_RAD)) - + boost::math::constants::half_pi()); +const constexpr double MAX_LONGITUDE = 180.0; } - -//! Projects both coordinates and takes the euclidean distance of the projected points -// Does not return meters! -double euclideanDistance(const Coordinate first_coordinate, const Coordinate second_coordinate); +//! Takes the squared euclidean distance of the input coordinates. Does not return meters! +double squaredEuclideanDistance(const FloatCoordinate &lhs, const FloatCoordinate &rhs); double haversineDistance(const Coordinate first_coordinate, const Coordinate second_coordinate); double greatCircleDistance(const Coordinate first_coordinate, const Coordinate second_coordinate); +std::pair +projectPointOnSegment(const FloatCoordinate &projected_xy_source, + const FloatCoordinate &projected_xy_target, + const FloatCoordinate &projected_xy_coordinate); + double perpendicularDistance(const Coordinate segment_source, const Coordinate segment_target, const Coordinate query_location); @@ -50,20 +54,6 @@ double perpendicularDistance(const Coordinate segment_source, Coordinate &nearest_location, double &ratio); -double perpendicularDistanceFromProjectedCoordinate( - const Coordinate segment_source, - const Coordinate segment_target, - const Coordinate query_location, - const std::pair projected_xy_coordinate); - -double perpendicularDistanceFromProjectedCoordinate( - const Coordinate segment_source, - const Coordinate segment_target, - const Coordinate query_location, - const std::pair projected_xy_coordinate, - Coordinate &nearest_location, - double &ratio); - Coordinate centroid(const Coordinate lhs, const Coordinate rhs); double bearing(const Coordinate first_coordinate, const Coordinate second_coordinate); @@ -86,8 +76,14 @@ double degreeToPixel(FloatLatitude lat, unsigned zoom); double degreeToPixel(FloatLongitude lon, unsigned zoom); FloatLatitude yToLat(const double value); double latToY(const FloatLatitude latitude); -void xyzToMercator(const int x, const int y, const int z, double &minx, double &miny, double &maxx, double &maxy); -void xyzToWSG84(const int x, const int y, const int z, double &minx, double &miny, double &maxx, double &maxy); + +FloatCoordinate fromWGS84(const FloatCoordinate &wgs84_coordinate); +FloatCoordinate toWGS84(const FloatCoordinate &mercator_coordinate); + +void xyzToMercator( + const int x, const int y, const int z, double &minx, double &miny, double &maxx, double &maxy); +void xyzToWGS84( + const int x, const int y, const int z, double &minx, double &miny, double &maxx, double &maxy); } // ns mercator } // ns coordinate_calculation } // ns util diff --git a/include/util/rectangle.hpp b/include/util/rectangle.hpp index 333405ba7..fd0f87188 100644 --- a/include/util/rectangle.hpp +++ b/include/util/rectangle.hpp @@ -80,7 +80,10 @@ struct RectangleInt2D min_lat > other.max_lat); } - double GetMinDist(const Coordinate location) const + // This code assumes that we are operating in euclidean space! + // That means if you just put unprojected lat/lon in here you will + // get invalid results. + double GetMinSquaredDist(const Coordinate location) const { const bool is_contained = Contains(location); if (is_contained) @@ -117,36 +120,36 @@ struct RectangleInt2D switch (d) { case NORTH: - min_dist = coordinate_calculation::greatCircleDistance( + min_dist = coordinate_calculation::squaredEuclideanDistance( location, Coordinate(location.lon, max_lat)); break; case SOUTH: - min_dist = coordinate_calculation::greatCircleDistance( + min_dist = coordinate_calculation::squaredEuclideanDistance( location, Coordinate(location.lon, min_lat)); break; case WEST: - min_dist = coordinate_calculation::greatCircleDistance( + min_dist = coordinate_calculation::squaredEuclideanDistance( location, Coordinate(min_lon, location.lat)); break; case EAST: - min_dist = coordinate_calculation::greatCircleDistance( + min_dist = coordinate_calculation::squaredEuclideanDistance( location, Coordinate(max_lon, location.lat)); break; case NORTH_EAST: min_dist = - coordinate_calculation::greatCircleDistance(location, Coordinate(max_lon, max_lat)); + coordinate_calculation::squaredEuclideanDistance(location, Coordinate(max_lon, max_lat)); break; case NORTH_WEST: min_dist = - coordinate_calculation::greatCircleDistance(location, Coordinate(min_lon, max_lat)); + coordinate_calculation::squaredEuclideanDistance(location, Coordinate(min_lon, max_lat)); break; case SOUTH_EAST: min_dist = - coordinate_calculation::greatCircleDistance(location, Coordinate(max_lon, min_lat)); + coordinate_calculation::squaredEuclideanDistance(location, Coordinate(max_lon, min_lat)); break; case SOUTH_WEST: min_dist = - coordinate_calculation::greatCircleDistance(location, Coordinate(min_lon, min_lat)); + coordinate_calculation::squaredEuclideanDistance(location, Coordinate(min_lon, min_lat)); break; default: break; @@ -157,37 +160,6 @@ struct RectangleInt2D return min_dist; } - double GetMinMaxDist(const Coordinate location) const - { - double min_max_dist = std::numeric_limits::max(); - // Get minmax distance to each of the four sides - const Coordinate upper_left(min_lon, max_lat); - const Coordinate upper_right(max_lon, max_lat); - const Coordinate lower_right(max_lon, min_lat); - const Coordinate lower_left(min_lon, min_lat); - - min_max_dist = - std::min(min_max_dist, - std::max(coordinate_calculation::greatCircleDistance(location, upper_left), - coordinate_calculation::greatCircleDistance(location, upper_right))); - - min_max_dist = - std::min(min_max_dist, - std::max(coordinate_calculation::greatCircleDistance(location, upper_right), - coordinate_calculation::greatCircleDistance(location, lower_right))); - - min_max_dist = - std::min(min_max_dist, - std::max(coordinate_calculation::greatCircleDistance(location, lower_right), - coordinate_calculation::greatCircleDistance(location, lower_left))); - - min_max_dist = - std::min(min_max_dist, - std::max(coordinate_calculation::greatCircleDistance(location, lower_left), - coordinate_calculation::greatCircleDistance(location, upper_left))); - return min_max_dist; - } - bool Contains(const Coordinate location) const { const bool lons_contained = (location.lon >= min_lon) && (location.lon <= max_lon); diff --git a/include/util/static_rtree.hpp b/include/util/static_rtree.hpp index 43f41e29d..6fe64d041 100644 --- a/include/util/static_rtree.hpp +++ b/include/util/static_rtree.hpp @@ -36,6 +36,8 @@ namespace util { // Static RTree for serving nearest neighbour queries +// All coordinates are pojected first to Web Mercator before the bounding boxes +// are computed, this means the internal distance metric doesn not represent meters! template , bool UseSharedMemory = false, @@ -48,7 +50,11 @@ class StaticRTree using EdgeData = EdgeDataT; using CoordinateList = CoordinateListT; - static constexpr std::size_t MAX_CHECKED_ELEMENTS = 4 * LEAF_NODE_SIZE; + struct CandidateSegment + { + Coordinate fixed_projected_coordinate; + EdgeDataT data; + }; struct TreeNode { @@ -86,16 +92,16 @@ class StaticRTree } }; - using QueryNodeType = mapbox::util::variant; + using QueryNodeType = mapbox::util::variant; struct QueryCandidate { inline bool operator<(const QueryCandidate &other) const { // Attn: this is reversed order. std::pq is a max pq! - return other.min_dist < min_dist; + return other.squared_min_dist < squared_min_dist; } - float min_dist; + double squared_min_dist; QueryNodeType node; }; @@ -315,9 +321,16 @@ class StaticRTree leaves_stream.read((char *)&m_element_count, sizeof(uint64_t)); } - /* Returns all features inside the bounding box */ + /* Returns all features inside the bounding box. + Rectangle needs to be projected!*/ std::vector SearchInBox(const Rectangle &search_rectangle) { + const Rectangle projected_rectangle{ + search_rectangle.min_lon, search_rectangle.max_lon, + toFixed(FloatLatitude{coordinate_calculation::mercator::latToY( + toFloating(FixedLatitude(search_rectangle.min_lat)))}), + toFixed(FloatLatitude{coordinate_calculation::mercator::latToY( + toFloating(FixedLatitude(search_rectangle.max_lat)))})}; std::vector results; std::queue traversal_queue; @@ -377,11 +390,11 @@ class StaticRTree std::vector Nearest(const Coordinate input_coordinate, const std::size_t max_results) { return Nearest(input_coordinate, - [](const EdgeDataT &) + [](const CandidateSegment &) { return std::make_pair(true, true); }, - [max_results](const std::size_t num_results, const float) + [max_results](const std::size_t num_results, const CandidateSegment &) { return num_results >= max_results; }); @@ -393,9 +406,8 @@ class StaticRTree Nearest(const Coordinate input_coordinate, const FilterT filter, const TerminationT terminate) { std::vector results; - std::pair projected_coordinate = { - static_cast(toFloating(input_coordinate.lon)), - coordinate_calculation::mercator::latToY(toFloating(input_coordinate.lat))}; + auto projected_coordinate = coordinate_calculation::mercator::fromWGS84(input_coordinate); + Coordinate fixed_projected_coordinate{projected_coordinate}; // initialize queue with root element std::priority_queue traversal_queue; @@ -403,13 +415,7 @@ class StaticRTree while (!traversal_queue.empty()) { - const QueryCandidate current_query_node = traversal_queue.top(); - if (terminate(results.size(), current_query_node.min_dist)) - { - traversal_queue = std::priority_queue{}; - break; - } - + QueryCandidate current_query_node = traversal_queue.top(); traversal_queue.pop(); if (current_query_node.node.template is()) @@ -418,30 +424,34 @@ class StaticRTree current_query_node.node.template get(); if (current_tree_node.child_is_on_disk) { - ExploreLeafNode(current_tree_node.children[0], input_coordinate, - projected_coordinate, traversal_queue); + ExploreLeafNode(current_tree_node.children[0], projected_coordinate, + traversal_queue); } else { - ExploreTreeNode(current_tree_node, input_coordinate, traversal_queue); + ExploreTreeNode(current_tree_node, fixed_projected_coordinate, traversal_queue); } } else { // inspecting an actual road segment - const auto ¤t_segment = current_query_node.node.template get(); + auto ¤t_candidate = current_query_node.node.template get(); + if (terminate(results.size(), current_candidate)) + { + traversal_queue = std::priority_queue{}; + break; + } - auto use_segment = filter(current_segment); + auto use_segment = filter(current_candidate); if (!use_segment.first && !use_segment.second) { continue; } + current_candidate.data.forward_segment_id.enabled &= use_segment.first; + current_candidate.data.reverse_segment_id.enabled &= use_segment.second; // store phantom node in result vector - results.push_back(std::move(current_segment)); - - results.back().forward_segment_id.enabled &= use_segment.first; - results.back().reverse_segment_id.enabled &= use_segment.second; + results.push_back(std::move(current_candidate.data)); } } @@ -451,8 +461,7 @@ class StaticRTree private: template void ExploreLeafNode(const std::uint32_t leaf_id, - const Coordinate input_coordinate, - const std::pair &projected_coordinate, + const FloatCoordinate &projected_input_coordinate, QueueT &traversal_queue) { LeafNode current_leaf_node; @@ -462,21 +471,30 @@ class StaticRTree for (const auto i : irange(0u, current_leaf_node.object_count)) { auto ¤t_edge = current_leaf_node.objects[i]; - const float current_perpendicular_distance = - coordinate_calculation::perpendicularDistanceFromProjectedCoordinate( - m_coordinate_list->at(current_edge.u), m_coordinate_list->at(current_edge.v), - input_coordinate, projected_coordinate); + auto projected_u = + coordinate_calculation::mercator::fromWGS84((*m_coordinate_list)[current_edge.u]); + auto projected_v = + coordinate_calculation::mercator::fromWGS84((*m_coordinate_list)[current_edge.v]); + + FloatCoordinate projected_nearest; + std::tie(std::ignore, projected_nearest) = + coordinate_calculation::projectPointOnSegment(projected_u, projected_v, + projected_input_coordinate); + + auto squared_distance = coordinate_calculation::squaredEuclideanDistance( + projected_input_coordinate, projected_nearest); // distance must be non-negative - BOOST_ASSERT(0.f <= current_perpendicular_distance); + BOOST_ASSERT(0. <= squared_distance); traversal_queue.push( - QueryCandidate{current_perpendicular_distance, std::move(current_edge)}); + QueryCandidate{squared_distance, CandidateSegment{Coordinate{projected_nearest}, + std::move(current_edge)}}); } } template void ExploreTreeNode(const TreeNode &parent, - const Coordinate input_coordinate, + const Coordinate fixed_projected_input_coordinate, QueueT &traversal_queue) { for (std::uint32_t i = 0; i < parent.child_count; ++i) @@ -484,8 +502,10 @@ class StaticRTree const std::int32_t child_id = parent.children[i]; const auto &child_tree_node = m_search_tree[child_id]; const auto &child_rectangle = child_tree_node.minimum_bounding_rectangle; - const float lower_bound_to_element = child_rectangle.GetMinDist(input_coordinate); - traversal_queue.push(QueryCandidate{lower_bound_to_element, m_search_tree[child_id]}); + const auto squared_lower_bound_to_element = + child_rectangle.GetMinSquaredDist(fixed_projected_input_coordinate); + traversal_queue.push( + QueryCandidate{squared_lower_bound_to_element, m_search_tree[child_id]}); } } @@ -517,19 +537,29 @@ class StaticRTree BOOST_ASSERT(objects[i].u < coordinate_list.size()); BOOST_ASSERT(objects[i].v < coordinate_list.size()); + Coordinate projected_u{coordinate_calculation::mercator::fromWGS84( + Coordinate{coordinate_list[objects[i].u]})}; + Coordinate projected_v{coordinate_calculation::mercator::fromWGS84( + Coordinate{coordinate_list[objects[i].v]})}; + + BOOST_ASSERT(toFloating(projected_u.lon) <= FloatLongitude(180.)); + BOOST_ASSERT(toFloating(projected_u.lon) >= FloatLongitude(-180.)); + BOOST_ASSERT(toFloating(projected_u.lat) <= FloatLatitude(180.)); + BOOST_ASSERT(toFloating(projected_u.lat) >= FloatLatitude(-180.)); + BOOST_ASSERT(toFloating(projected_v.lon) <= FloatLongitude(180.)); + BOOST_ASSERT(toFloating(projected_v.lon) >= FloatLongitude(-180.)); + BOOST_ASSERT(toFloating(projected_v.lat) <= FloatLatitude(180.)); + BOOST_ASSERT(toFloating(projected_v.lat) >= FloatLatitude(-180.)); + rectangle.min_lon = - std::min(rectangle.min_lon, std::min(coordinate_list[objects[i].u].lon, - coordinate_list[objects[i].v].lon)); + std::min(rectangle.min_lon, std::min(projected_u.lon, projected_v.lon)); rectangle.max_lon = - std::max(rectangle.max_lon, std::max(coordinate_list[objects[i].u].lon, - coordinate_list[objects[i].v].lon)); + std::max(rectangle.max_lon, std::max(projected_u.lon, projected_v.lon)); rectangle.min_lat = - std::min(rectangle.min_lat, std::min(coordinate_list[objects[i].u].lat, - coordinate_list[objects[i].v].lat)); + std::min(rectangle.min_lat, std::min(projected_u.lat, projected_v.lat)); rectangle.max_lat = - std::max(rectangle.max_lat, std::max(coordinate_list[objects[i].u].lat, - coordinate_list[objects[i].v].lat)); + std::max(rectangle.max_lat, std::max(projected_u.lat, projected_v.lat)); } BOOST_ASSERT(rectangle.min_lon != FixedLongitude(std::numeric_limits::min())); BOOST_ASSERT(rectangle.min_lat != FixedLatitude(std::numeric_limits::min())); diff --git a/src/engine/plugins/tile.cpp b/src/engine/plugins/tile.cpp index eb168852c..7d18d9029 100644 --- a/src/engine/plugins/tile.cpp +++ b/src/engine/plugins/tile.cpp @@ -30,7 +30,7 @@ namespace detail const constexpr double VECTOR_TILE_EXTENT = 4096.0; const constexpr double VECTOR_TILE_BUFFER = 128.0; -// Simple container class for WSG84 coordinates +// Simple container class for WGS84 coordinates template struct Point final { Point(T _x, T _y) : x(_x), y(_y) {} @@ -173,8 +173,8 @@ Status TilePlugin::HandleRequest(const api::TileParameters ¶meters, std::str using namespace util::coordinate_calculation; double min_lon, min_lat, max_lon, max_lat; - // Convert the z,x,y mercator tile coordinates into WSG84 lon/lat values - mercator::xyzToWSG84(parameters.x, parameters.y, parameters.z, min_lon, min_lat, max_lon, + // Convert the z,x,y mercator tile coordinates into WGS84 lon/lat values + mercator::xyzToWGS84(parameters.x, parameters.y, parameters.z, min_lon, min_lat, max_lon, max_lat); util::Coordinate southwest{util::FloatLongitude(min_lon), util::FloatLatitude(min_lat)}; diff --git a/src/util/coordinate.cpp b/src/util/coordinate.cpp index 0b7fed49d..9ccbf17f3 100644 --- a/src/util/coordinate.cpp +++ b/src/util/coordinate.cpp @@ -22,6 +22,11 @@ Coordinate::Coordinate() { } +Coordinate::Coordinate(const FloatCoordinate &other) + : Coordinate(toFixed(other.lon), toFixed(other.lat)) +{ +} + Coordinate::Coordinate(const FloatLongitude lon_, const FloatLatitude lat_) : Coordinate(toFixed(lon_), toFixed(lat_)) { @@ -39,12 +44,45 @@ bool Coordinate::IsValid() const lon < FixedLongitude(-180 * COORDINATE_PRECISION)); } +FloatCoordinate::FloatCoordinate() + : lon(std::numeric_limits::min()), lat(std::numeric_limits::min()) +{ +} + +FloatCoordinate::FloatCoordinate(const Coordinate other) + : FloatCoordinate(toFloating(other.lon), toFloating(other.lat)) +{ +} + +FloatCoordinate::FloatCoordinate(const FixedLongitude lon_, const FixedLatitude lat_) + : FloatCoordinate(toFloating(lon_), toFloating(lat_)) +{ +} + +FloatCoordinate::FloatCoordinate(const FloatLongitude lon_, const FloatLatitude lat_) : lon(lon_), lat(lat_) +{ +} + +bool FloatCoordinate::IsValid() const +{ + return !(lat > FloatLatitude(90) || + lat < FloatLatitude(-90) || + lon > FloatLongitude(180) || + lon < FloatLongitude(-180)); +} + + bool operator==(const Coordinate lhs, const Coordinate rhs) { return lhs.lat == rhs.lat && lhs.lon == rhs.lon; } +bool operator==(const FloatCoordinate lhs, const FloatCoordinate rhs) +{ + return lhs.lat == rhs.lat && lhs.lon == rhs.lon; +} bool operator!=(const Coordinate lhs, const Coordinate rhs) { return !(lhs == rhs); } +bool operator!=(const FloatCoordinate lhs, const FloatCoordinate rhs) { return !(lhs == rhs); } std::ostream &operator<<(std::ostream &out, const Coordinate coordinate) { @@ -52,5 +90,11 @@ std::ostream &operator<<(std::ostream &out, const Coordinate coordinate) << ", lat:" << toFloating(coordinate.lat) << ")"; return out; } +std::ostream &operator<<(std::ostream &out, const FloatCoordinate coordinate) +{ + out << std::setprecision(12) << "(lon:" << coordinate.lon + << ", lat:" << coordinate.lat << ")"; + return out; +} } } diff --git a/src/util/coordinate_calculation.cpp b/src/util/coordinate_calculation.cpp index 67662ca9c..8ed69c954 100644 --- a/src/util/coordinate_calculation.cpp +++ b/src/util/coordinate_calculation.cpp @@ -17,16 +17,13 @@ namespace util namespace coordinate_calculation { -double euclideanDistance(const Coordinate coordinate_1, const Coordinate coordinate_2) +// Does not project the coordinates! +double squaredEuclideanDistance(const FloatCoordinate &lhs, const FloatCoordinate &rhs) { - const double x1 = static_cast(toFloating(coordinate_1.lon)); - const double y1 = mercator::latToY(toFloating(coordinate_1.lat)); - const double x2 = static_cast(toFloating(coordinate_2.lon)); - const double y2 = mercator::latToY(toFloating(coordinate_2.lat)); - const double dx = x1 - x2; - const double dy = y1 - y2; + const double dx = static_cast(lhs.lon - rhs.lon); + const double dy = static_cast(lhs.lat - rhs.lat); - return std::sqrt(dx * dx + dy * dy); + return dx * dx + dy * dy; } double haversineDistance(const Coordinate coordinate_1, const Coordinate coordinate_2) @@ -79,15 +76,39 @@ double greatCircleDistance(const Coordinate coordinate_1, const Coordinate coord return std::hypot(x_value, y_value) * EARTH_RADIUS; } -double perpendicularDistance(const Coordinate source_coordinate, - const Coordinate target_coordinate, - const Coordinate query_location) +std::pair projectPointOnSegment(const FloatCoordinate &source, + const FloatCoordinate &target, + const FloatCoordinate &coordinate) { - double ratio; - Coordinate nearest_location; + const FloatCoordinate slope_vector{target.lon - source.lon, target.lat - source.lat}; + const FloatCoordinate rel_coordinate{coordinate.lon - source.lon, coordinate.lat - source.lat}; + // dot product of two un-normed vectors + const auto unnormed_ratio = static_cast(slope_vector.lon * rel_coordinate.lon) + + static_cast(slope_vector.lat * rel_coordinate.lat); + // squared length of the slope vector + const auto squared_length = static_cast(slope_vector.lon * slope_vector.lon) + + static_cast(slope_vector.lat * slope_vector.lat); - return perpendicularDistance(source_coordinate, target_coordinate, query_location, - nearest_location, ratio); + if (squared_length < std::numeric_limits::epsilon()) + { + return {0, source}; + } + + const double normed_ratio = unnormed_ratio / squared_length; + double clamped_ratio = normed_ratio; + if (clamped_ratio > 1.) + { + clamped_ratio = 1.; + } + else if (clamped_ratio < 0.) + { + clamped_ratio = 0.; + } + return {clamped_ratio, + { + source.lon + slope_vector.lon * FloatLongitude(clamped_ratio), + source.lat + slope_vector.lat * FloatLatitude(clamped_ratio), + }}; } double perpendicularDistance(const Coordinate segment_source, @@ -98,108 +119,29 @@ double perpendicularDistance(const Coordinate segment_source, { using namespace coordinate_calculation; - return perpendicularDistanceFromProjectedCoordinate( - segment_source, segment_target, query_location, - {static_cast(toFloating(query_location.lon)), - mercator::latToY(toFloating(query_location.lat))}, - nearest_location, ratio); -} - -double perpendicularDistanceFromProjectedCoordinate( - const Coordinate source_coordinate, - const Coordinate target_coordinate, - const Coordinate query_location, - const std::pair projected_xy_coordinate) -{ - double ratio; - Coordinate nearest_location; - - return perpendicularDistanceFromProjectedCoordinate(source_coordinate, target_coordinate, - query_location, projected_xy_coordinate, - nearest_location, ratio); -} - -double perpendicularDistanceFromProjectedCoordinate( - const Coordinate segment_source, - const Coordinate segment_target, - const Coordinate query_location, - const std::pair projected_xy_coordinate, - Coordinate &nearest_location, - double &ratio) -{ - using namespace coordinate_calculation; - BOOST_ASSERT(query_location.IsValid()); - // initialize values - const double x = projected_xy_coordinate.second; - const double y = projected_xy_coordinate.first; - const double a = mercator::latToY(toFloating(segment_source.lat)); - const double b = static_cast(toFloating(segment_source.lon)); - const double c = mercator::latToY(toFloating(segment_target.lat)); - const double d = static_cast(toFloating(segment_target.lon)); - double p, q /*,mX*/, new_y; - if (std::abs(a - c) > std::numeric_limits::epsilon()) - { - const double m = (d - b) / (c - a); // slope - // Projection of (x,y) on line joining (a,b) and (c,d) - p = ((x + (m * y)) + (m * m * a - m * b)) / (1.0 + m * m); - q = b + m * (p - a); - } - else - { - p = c; - q = y; - } - new_y = (d * p - c * q) / (a * d - b * c); - - // discretize the result to coordinate precision. it's a hack! - if (std::abs(new_y) < (1.0 / COORDINATE_PRECISION)) - { - new_y = 0.0; - } - - // compute ratio - ratio = static_cast((p - new_y * a) / - c); // These values are actually n/m+n and m/m+n , we need - // not calculate the explicit values of m an n as we - // are just interested in the ratio - if (std::isnan(ratio)) - { - ratio = (segment_target == query_location ? 1.0 : 0.0); - } - else if (std::abs(ratio) <= std::numeric_limits::epsilon()) - { - ratio = 0.0; - } - else if (std::abs(ratio - 1.0) <= std::numeric_limits::epsilon()) - { - ratio = 1.0; - } - - // compute nearest location - BOOST_ASSERT(!std::isnan(ratio)); - if (ratio <= 0.0) - { - nearest_location = segment_source; - } - else if (ratio >= 1.0) - { - nearest_location = segment_target; - } - else - { - // point lies in between - nearest_location.lon = toFixed(FloatLongitude(q)); - nearest_location.lat = toFixed(FloatLatitude(mercator::yToLat(p))); - } - BOOST_ASSERT(nearest_location.IsValid()); + FloatCoordinate projected_nearest; + std::tie(ratio, projected_nearest) = + projectPointOnSegment(mercator::fromWGS84(segment_source), mercator::fromWGS84(segment_target), mercator::fromWGS84(query_location)); + nearest_location = mercator::toWGS84(projected_nearest); const double approximate_distance = greatCircleDistance(query_location, nearest_location); BOOST_ASSERT(0.0 <= approximate_distance); return approximate_distance; } +double perpendicularDistance(const Coordinate source_coordinate, + const Coordinate target_coordinate, + const Coordinate query_location) +{ + double ratio; + Coordinate nearest_location; + + return perpendicularDistance(source_coordinate, target_coordinate, query_location, + nearest_location, ratio); +} + Coordinate centroid(const Coordinate lhs, const Coordinate rhs) { Coordinate centroid; @@ -283,7 +225,9 @@ namespace mercator { FloatLatitude yToLat(const double y) { - const double normalized_lat = RAD_TO_DEGREE * 2. * std::atan(std::exp(y * DEGREE_TO_RAD)); + const auto clamped_y = std::max(-180., std::min(180., y)); + const double normalized_lat = + RAD_TO_DEGREE * 2. * std::atan(std::exp(clamped_y * DEGREE_TO_RAD)); return FloatLatitude(normalized_lat - 90.); } @@ -292,7 +236,9 @@ double latToY(const FloatLatitude latitude) { const double normalized_lat = 90. + static_cast(latitude); - return RAD_TO_DEGREE * std::log(std::tan(normalized_lat * DEGREE_TO_RAD * 0.5)); + const double y = RAD_TO_DEGREE * std::log(std::tan(normalized_lat * DEGREE_TO_RAD * 0.5)); + const auto clamped_y = std::max(-180., std::min(180., y)); + return clamped_y; } FloatLatitude clamp(const FloatLatitude lat) @@ -313,7 +259,7 @@ inline void pixelToDegree(const double shift, double &x, double &y) x = (x - b) / shift * 360.0; // FIXME needs to be simplified const double g = (y - b) / -(shift / (2 * M_PI)) / DEGREE_TO_RAD; - static_assert(DEGREE_TO_RAD / (2 * M_PI) - 1/360. < 0.0001, ""); + static_assert(DEGREE_TO_RAD / (2 * M_PI) - 1 / 360. < 0.0001, ""); y = static_cast(util::coordinate_calculation::mercator::yToLat(g)); } @@ -333,8 +279,19 @@ double degreeToPixel(FloatLatitude lat, unsigned zoom) return y; } -// Converts a WMS tile coordinate (z,x,y) into a wsg84 bounding box -void xyzToWSG84(const int x, const int y, const int z, double &minx, double &miny, double &maxx, double &maxy) +FloatCoordinate fromWGS84(const FloatCoordinate &wgs84_coordinate) +{ + return {wgs84_coordinate.lon, FloatLatitude{coordinate_calculation::mercator::latToY(wgs84_coordinate.lat)}}; +} + +FloatCoordinate toWGS84(const FloatCoordinate &mercator_coordinate) +{ + return {mercator_coordinate.lon, coordinate_calculation::mercator::yToLat(static_cast(mercator_coordinate.lat))}; +} + +// Converts a WMS tile coordinate (z,x,y) into a wgs bounding box +void xyzToWGS84( + const int x, const int y, const int z, double &minx, double &miny, double &maxx, double &maxy) { using util::coordinate_calculation::mercator::TILE_SIZE; @@ -349,11 +306,12 @@ void xyzToWSG84(const int x, const int y, const int z, double &minx, double &min } // Converts a WMS tile coordinate (z,x,y) into a mercator bounding box -void xyzToMercator(const int x, const int y, const int z, double &minx, double &miny, double &maxx, double &maxy) +void xyzToMercator( + const int x, const int y, const int z, double &minx, double &miny, double &maxx, double &maxy) { using namespace util::coordinate_calculation::mercator; - xyzToWSG84(x, y, z, minx, miny, maxx, maxy); + xyzToWGS84(x, y, z, minx, miny, maxx, maxy); minx = static_cast(clamp(util::FloatLongitude(minx))) * DEGREE_TO_PX; miny = latToY(clamp(util::FloatLatitude(miny))) * DEGREE_TO_PX; diff --git a/unit_tests/util/coordinate.cpp b/unit_tests/util/coordinate.cpp deleted file mode 100644 index ec5cd5a5d..000000000 --- a/unit_tests/util/coordinate.cpp +++ /dev/null @@ -1,92 +0,0 @@ -#include - -#include "util/coordinate_calculation.hpp" - -#include - -#include - -using namespace osrm; -using namespace osrm::util; - -// Regression test for bug captured in #1347 -BOOST_AUTO_TEST_CASE(regression_test_1347) -{ - Coordinate u(FloatLongitude(-100), FloatLatitude(10)); - Coordinate v(FloatLongitude(-100.002), FloatLatitude(10.001)); - Coordinate q(FloatLongitude(-100.001), FloatLatitude(10.002)); - - double d1 = coordinate_calculation::perpendicularDistance(u, v, q); - - double ratio; - Coordinate nearest_location; - double d2 = coordinate_calculation::perpendicularDistance(u, v, q, nearest_location, ratio); - - BOOST_CHECK_LE(std::abs(d1 - d2), 0.01); -} - -BOOST_AUTO_TEST_CASE(lon_to_pixel) -{ - using namespace coordinate_calculation; - BOOST_CHECK_CLOSE(7.416042 * mercator::DEGREE_TO_PX, 825550.019142, 0.1); - BOOST_CHECK_CLOSE(7.415892 * mercator::DEGREE_TO_PX, 825533.321218, 0.1); - BOOST_CHECK_CLOSE(7.416016 * mercator::DEGREE_TO_PX, 825547.124835, 0.1); - BOOST_CHECK_CLOSE(7.41577 * mercator::DEGREE_TO_PX, 825519.74024, 0.1); - BOOST_CHECK_CLOSE(7.415808 * mercator::DEGREE_TO_PX, 825523.970381, 0.1); -} - -BOOST_AUTO_TEST_CASE(lat_to_pixel) -{ - using namespace coordinate_calculation; - BOOST_CHECK_CLOSE(mercator::latToY(util::FloatLatitude(43.733947)) * mercator::DEGREE_TO_PX, - 5424361.75863, 0.1); - BOOST_CHECK_CLOSE(mercator::latToY(util::FloatLatitude(43.733799)) * mercator::DEGREE_TO_PX, - 5424338.95731, 0.1); - BOOST_CHECK_CLOSE(mercator::latToY(util::FloatLatitude(43.733922)) * mercator::DEGREE_TO_PX, - 5424357.90705, 0.1); - BOOST_CHECK_CLOSE(mercator::latToY(util::FloatLatitude(43.733697)) * mercator::DEGREE_TO_PX, - 5424323.24293, 0.1); - BOOST_CHECK_CLOSE(mercator::latToY(util::FloatLatitude(43.733729)) * mercator::DEGREE_TO_PX, - 5424328.17293, 0.1); -} - -BOOST_AUTO_TEST_CASE(xyz_to_wgs84) -{ - using namespace coordinate_calculation; - - double minx_1; - double miny_1; - double maxx_1; - double maxy_1; - mercator::xyzToWSG84(2, 2, 1, minx_1, miny_1, maxx_1, maxy_1); - BOOST_CHECK_CLOSE(minx_1, 180, 0.0001); - BOOST_CHECK_CLOSE(miny_1, -89.786, 0.0001); - BOOST_CHECK_CLOSE(maxx_1, 360, 0.0001); - BOOST_CHECK_CLOSE(maxy_1, -85.0511, 0.0001); - - double minx_2; - double miny_2; - double maxx_2; - double maxy_2; - mercator::xyzToWSG84(100, 0, 13, minx_2, miny_2, maxx_2, maxy_2); - BOOST_CHECK_CLOSE(minx_2, -175.6054, 0.0001); - BOOST_CHECK_CLOSE(miny_2, 85.0473, 0.0001); - BOOST_CHECK_CLOSE(maxx_2, -175.5615, 0.0001); - BOOST_CHECK_CLOSE(maxy_2, 85.0511, 0.0001); -} - -BOOST_AUTO_TEST_CASE(xyz_to_mercator) -{ - using namespace coordinate_calculation; - - double minx; - double miny; - double maxx; - double maxy; - mercator::xyzToMercator(100, 0, 13, minx, miny, maxx, maxy); - - BOOST_CHECK_CLOSE(minx, -19548311.361764118075, 0.0001); - BOOST_CHECK_CLOSE(miny, 20032616.372979003936, 0.0001); - BOOST_CHECK_CLOSE(maxx, -19543419.391953866929, 0.0001); - BOOST_CHECK_CLOSE(maxy, 20037508.342789277434, 0.0001); -} diff --git a/unit_tests/util/coordinate_calculation.cpp b/unit_tests/util/coordinate_calculation.cpp new file mode 100644 index 000000000..d159441be --- /dev/null +++ b/unit_tests/util/coordinate_calculation.cpp @@ -0,0 +1,191 @@ +#include + +#include "util/coordinate_calculation.hpp" + +#include + +#include + +using namespace osrm; +using namespace osrm::util; + +BOOST_AUTO_TEST_SUITE(coordinate_calculation_tests) + +// Regression test for bug captured in #1347 +BOOST_AUTO_TEST_CASE(regression_test_1347) +{ + Coordinate u(FloatLongitude(-100), FloatLatitude(10)); + Coordinate v(FloatLongitude(-100.002), FloatLatitude(10.001)); + Coordinate q(FloatLongitude(-100.001), FloatLatitude(10.002)); + + double d1 = coordinate_calculation::perpendicularDistance(u, v, q); + + double ratio; + Coordinate nearest_location; + double d2 = coordinate_calculation::perpendicularDistance(u, v, q, nearest_location, ratio); + + BOOST_CHECK_LE(std::abs(d1 - d2), 0.01); +} + +BOOST_AUTO_TEST_CASE(lon_to_pixel) +{ + using namespace coordinate_calculation; + BOOST_CHECK_CLOSE(7.416042 * mercator::DEGREE_TO_PX, 825550.019142, 0.1); + BOOST_CHECK_CLOSE(7.415892 * mercator::DEGREE_TO_PX, 825533.321218, 0.1); + BOOST_CHECK_CLOSE(7.416016 * mercator::DEGREE_TO_PX, 825547.124835, 0.1); + BOOST_CHECK_CLOSE(7.41577 * mercator::DEGREE_TO_PX, 825519.74024, 0.1); + BOOST_CHECK_CLOSE(7.415808 * mercator::DEGREE_TO_PX, 825523.970381, 0.1); +} + +BOOST_AUTO_TEST_CASE(lat_to_pixel) +{ + using namespace coordinate_calculation; + BOOST_CHECK_CLOSE(mercator::latToY(util::FloatLatitude(43.733947)) * mercator::DEGREE_TO_PX, + 5424361.75863, 0.1); + BOOST_CHECK_CLOSE(mercator::latToY(util::FloatLatitude(43.733799)) * mercator::DEGREE_TO_PX, + 5424338.95731, 0.1); + BOOST_CHECK_CLOSE(mercator::latToY(util::FloatLatitude(43.733922)) * mercator::DEGREE_TO_PX, + 5424357.90705, 0.1); + BOOST_CHECK_CLOSE(mercator::latToY(util::FloatLatitude(43.733697)) * mercator::DEGREE_TO_PX, + 5424323.24293, 0.1); + BOOST_CHECK_CLOSE(mercator::latToY(util::FloatLatitude(43.733729)) * mercator::DEGREE_TO_PX, + 5424328.17293, 0.1); +} + +BOOST_AUTO_TEST_CASE(xyz_to_wgs84) +{ + using namespace coordinate_calculation; + + double minx_1; + double miny_1; + double maxx_1; + double maxy_1; + mercator::xyzToWGS84(2, 2, 1, minx_1, miny_1, maxx_1, maxy_1); + BOOST_CHECK_CLOSE(minx_1, 180, 0.0001); + BOOST_CHECK_CLOSE(miny_1, -85.0511, 0.0001); + BOOST_CHECK_CLOSE(maxx_1, 360, 0.0001); + BOOST_CHECK_CLOSE(maxy_1, -85.0511, 0.0001); + + double minx_2; + double miny_2; + double maxx_2; + double maxy_2; + mercator::xyzToWGS84(100, 0, 13, minx_2, miny_2, maxx_2, maxy_2); + BOOST_CHECK_CLOSE(minx_2, -175.6054, 0.0001); + BOOST_CHECK_CLOSE(miny_2, 85.0473, 0.0001); + BOOST_CHECK_CLOSE(maxx_2, -175.5615, 0.0001); + BOOST_CHECK_CLOSE(maxy_2, 85.0511, 0.0001); +} + +BOOST_AUTO_TEST_CASE(xyz_to_mercator) +{ + using namespace coordinate_calculation; + + double minx; + double miny; + double maxx; + double maxy; + mercator::xyzToMercator(100, 0, 13, minx, miny, maxx, maxy); + + BOOST_CHECK_CLOSE(minx, -19548311.361764118075, 0.0001); + BOOST_CHECK_CLOSE(miny, 20032616.372979003936, 0.0001); + BOOST_CHECK_CLOSE(maxx, -19543419.391953866929, 0.0001); + BOOST_CHECK_CLOSE(maxy, 20037508.342789277434, 0.0001); +} + +BOOST_AUTO_TEST_CASE(regression_point_on_segment) +{ + // ^ + // | t + // | + // | i + // | + // |---|---|---|---|---|---|---|---> + // | + // | + // | + // | + // | + // | + // | + // | + // | s + FloatCoordinate input{FloatLongitude{55.995715}, FloatLatitude{48.332711}}; + FloatCoordinate start{FloatLongitude{74.140427}, FloatLatitude{-180}}; + FloatCoordinate target{FloatLongitude{53.041084}, FloatLatitude{77.21011}}; + + FloatCoordinate nearest; + double ratio; + std::tie(ratio, nearest) = coordinate_calculation::projectPointOnSegment(start, target, input); + + FloatCoordinate diff{target.lon - start.lon, target.lat - start.lat}; + + BOOST_CHECK_CLOSE(static_cast(start.lon + FloatLongitude(ratio) * diff.lon), static_cast(nearest.lon), 0.1); + BOOST_CHECK_CLOSE(static_cast(start.lat + FloatLatitude(ratio) * diff.lat), static_cast(nearest.lat), 0.1); +} + +BOOST_AUTO_TEST_CASE(point_on_segment) +{ + // t + // | + // |---- i + // | + // s + auto result_1 = coordinate_calculation::projectPointOnSegment( + {FloatLongitude{0}, FloatLatitude{0}}, {FloatLongitude{0}, FloatLatitude{2}}, + {FloatLongitude{2}, FloatLatitude{1}}); + auto reference_ratio_1 = 0.5; + auto reference_point_1 = FloatCoordinate{FloatLongitude{0}, FloatLatitude{1}}; + BOOST_CHECK_EQUAL(result_1.first, reference_ratio_1); + BOOST_CHECK_EQUAL(result_1.second.lon, reference_point_1.lon); + BOOST_CHECK_EQUAL(result_1.second.lat, reference_point_1.lat); + + // i + // : + // t + // | + // | + // | + // s + auto result_2 = coordinate_calculation::projectPointOnSegment( + {FloatLongitude{0.}, FloatLatitude{0.}}, {FloatLongitude{0}, FloatLatitude{2}}, + {FloatLongitude{0}, FloatLatitude{3}}); + auto reference_ratio_2 = 1.; + auto reference_point_2 = FloatCoordinate{FloatLongitude{0}, FloatLatitude{2}}; + BOOST_CHECK_EQUAL(result_2.first, reference_ratio_2); + BOOST_CHECK_EQUAL(result_2.second.lon, reference_point_2.lon); + BOOST_CHECK_EQUAL(result_2.second.lat, reference_point_2.lat); + + // t + // | + // | + // | + // s + // : + // i + auto result_3 = coordinate_calculation::projectPointOnSegment( + {FloatLongitude{0.}, FloatLatitude{0.}}, {FloatLongitude{0}, FloatLatitude{2}}, + {FloatLongitude{0}, FloatLatitude{-1}}); + auto reference_ratio_3 = 0.; + auto reference_point_3 = FloatCoordinate{FloatLongitude{0}, FloatLatitude{0}}; + BOOST_CHECK_EQUAL(result_3.first, reference_ratio_3); + BOOST_CHECK_EQUAL(result_3.second.lon, reference_point_3.lon); + BOOST_CHECK_EQUAL(result_3.second.lat, reference_point_3.lat); + + // t + // / + // /. + // / i + // s + // + auto result_4 = coordinate_calculation::projectPointOnSegment( + {FloatLongitude{0}, FloatLatitude{0}}, {FloatLongitude{1}, FloatLatitude{1}}, + {FloatLongitude{0.5 + 0.1}, FloatLatitude{0.5 - 0.1}}); + auto reference_ratio_4 = 0.5; + auto reference_point_4 = FloatCoordinate{FloatLongitude{0.5}, FloatLatitude{0.5}}; + BOOST_CHECK_EQUAL(result_4.first, reference_ratio_4); + BOOST_CHECK_EQUAL(result_4.second.lon, reference_point_4.lon); + BOOST_CHECK_EQUAL(result_4.second.lat, reference_point_4.lat); +} + +BOOST_AUTO_TEST_SUITE_END() diff --git a/unit_tests/util/rectangle.cpp b/unit_tests/util/rectangle.cpp index b35f5d32c..f85d07351 100644 --- a/unit_tests/util/rectangle.cpp +++ b/unit_tests/util/rectangle.cpp @@ -26,48 +26,82 @@ BOOST_AUTO_TEST_CASE(get_min_dist_test) // | // +- -80 // | - RectangleInt2D nw{FloatLongitude(10), FloatLongitude(100), FloatLatitude(10), + RectangleInt2D ne{FloatLongitude(10), FloatLongitude(100), FloatLatitude(10), FloatLatitude(80)}; - // RectangleInt2D ne {FloatLongitude(-100), FloatLongitude(-10), FloatLatitude(10), - // FloatLatitude(80)}; - // RectangleInt2D sw {FloatLongitude(10), FloatLongitude(100), FloatLatitude(-80), - // FloatLatitude(-10)}; - RectangleInt2D se{FloatLongitude(-100), FloatLongitude(-10), FloatLatitude(-80), + RectangleInt2D nw{FloatLongitude(-100), FloatLongitude(-10), FloatLatitude(10), + FloatLatitude(80)}; + RectangleInt2D se{FloatLongitude(10), FloatLongitude(100), FloatLatitude(-80), + FloatLatitude(-10)}; + RectangleInt2D sw{FloatLongitude(-100), FloatLongitude(-10), FloatLatitude(-80), FloatLatitude(-10)}; - Coordinate nw_sw{FloatLongitude(9.9), FloatLatitude(9.9)}; - Coordinate nw_se{FloatLongitude(100.1), FloatLatitude(9.9)}; - Coordinate nw_ne{FloatLongitude(100.1), FloatLatitude(80.1)}; - Coordinate nw_nw{FloatLongitude(9.9), FloatLatitude(80.1)}; - Coordinate nw_s{FloatLongitude(55), FloatLatitude(9.9)}; - Coordinate nw_e{FloatLongitude(100.1), FloatLatitude(45.0)}; - Coordinate nw_w{FloatLongitude(9.9), FloatLatitude(45.0)}; - Coordinate nw_n{FloatLongitude(55), FloatLatitude(80.1)}; - BOOST_CHECK_CLOSE(nw.GetMinDist(nw_sw), 15611.9, 0.1); - BOOST_CHECK_CLOSE(nw.GetMinDist(nw_se), 15611.9, 0.1); - BOOST_CHECK_CLOSE(nw.GetMinDist(nw_ne), 11287.4, 0.1); - BOOST_CHECK_CLOSE(nw.GetMinDist(nw_nw), 11287.4, 0.1); - BOOST_CHECK_CLOSE(nw.GetMinDist(nw_s), 11122.6, 0.1); - BOOST_CHECK_CLOSE(nw.GetMinDist(nw_e), 7864.89, 0.1); - BOOST_CHECK_CLOSE(nw.GetMinDist(nw_w), 7864.89, 0.1); - BOOST_CHECK_CLOSE(nw.GetMinDist(nw_n), 11122.6, 0.1); + Coordinate nw_sw{FloatLongitude(-100.1), FloatLatitude(9.9)}; + Coordinate nw_se{FloatLongitude(-9.9), FloatLatitude(9.9)}; + Coordinate nw_ne{FloatLongitude(-9.9), FloatLatitude(80.1)}; + Coordinate nw_nw{FloatLongitude(-100.1), FloatLatitude(80.1)}; + Coordinate nw_s{FloatLongitude(-55), FloatLatitude(9.9)}; + Coordinate nw_e{FloatLongitude(-9.9), FloatLatitude(45.0)}; + Coordinate nw_w{FloatLongitude(-100.1), FloatLatitude(45.0)}; + Coordinate nw_n{FloatLongitude(-55), FloatLatitude(80.1)}; + BOOST_CHECK_CLOSE(nw.GetMinSquaredDist(nw_sw), 0.02, 0.1); + BOOST_CHECK_CLOSE(nw.GetMinSquaredDist(nw_se), 0.02, 0.1); + BOOST_CHECK_CLOSE(nw.GetMinSquaredDist(nw_ne), 0.02, 0.1); + BOOST_CHECK_CLOSE(nw.GetMinSquaredDist(nw_nw), 0.02, 0.1); + BOOST_CHECK_CLOSE(nw.GetMinSquaredDist(nw_s), 0.01, 0.1); + BOOST_CHECK_CLOSE(nw.GetMinSquaredDist(nw_e), 0.01, 0.1); + BOOST_CHECK_CLOSE(nw.GetMinSquaredDist(nw_w), 0.01, 0.1); + BOOST_CHECK_CLOSE(nw.GetMinSquaredDist(nw_n), 0.01, 0.1); - Coordinate se_ne{FloatLongitude(-9.9), FloatLatitude(-9.9)}; - Coordinate se_nw{FloatLongitude(-100.1), FloatLatitude(-9.9)}; - Coordinate se_sw{FloatLongitude(-100.1), FloatLatitude(-80.1)}; - Coordinate se_se{FloatLongitude(-9.9), FloatLatitude(-80.1)}; - Coordinate se_n{FloatLongitude(-55), FloatLatitude(-9.9)}; - Coordinate se_w{FloatLongitude(-100.1), FloatLatitude(-45.0)}; - Coordinate se_e{FloatLongitude(-9.9), FloatLatitude(-45.0)}; - Coordinate se_s{FloatLongitude(-55), FloatLatitude(-80.1)}; - BOOST_CHECK_CLOSE(se.GetMinDist(se_sw), 11287.4, 0.1); - BOOST_CHECK_CLOSE(se.GetMinDist(se_se), 11287.4, 0.1); - BOOST_CHECK_CLOSE(se.GetMinDist(se_ne), 15611.9, 0.1); - BOOST_CHECK_CLOSE(se.GetMinDist(se_nw), 15611.9, 0.1); - BOOST_CHECK_CLOSE(se.GetMinDist(se_s), 11122.6, 0.1); - BOOST_CHECK_CLOSE(se.GetMinDist(se_e), 7864.89, 0.1); - BOOST_CHECK_CLOSE(se.GetMinDist(se_w), 7864.89, 0.1); - BOOST_CHECK_CLOSE(se.GetMinDist(se_n), 11122.6, 0.1); + Coordinate ne_sw{FloatLongitude(9.9), FloatLatitude(9.9)}; + Coordinate ne_se{FloatLongitude(100.1), FloatLatitude(9.9)}; + Coordinate ne_ne{FloatLongitude(100.1), FloatLatitude(80.1)}; + Coordinate ne_nw{FloatLongitude(9.9), FloatLatitude(80.1)}; + Coordinate ne_s{FloatLongitude(55), FloatLatitude(9.9)}; + Coordinate ne_e{FloatLongitude(100.1), FloatLatitude(45.0)}; + Coordinate ne_w{FloatLongitude(9.9), FloatLatitude(45.0)}; + Coordinate ne_n{FloatLongitude(55), FloatLatitude(80.1)}; + BOOST_CHECK_CLOSE(ne.GetMinSquaredDist(ne_sw), 0.02, 0.1); + BOOST_CHECK_CLOSE(ne.GetMinSquaredDist(ne_se), 0.02, 0.1); + BOOST_CHECK_CLOSE(ne.GetMinSquaredDist(ne_ne), 0.02, 0.1); + BOOST_CHECK_CLOSE(ne.GetMinSquaredDist(ne_nw), 0.02, 0.1); + BOOST_CHECK_CLOSE(ne.GetMinSquaredDist(ne_s), 0.01, 0.1); + BOOST_CHECK_CLOSE(ne.GetMinSquaredDist(ne_e), 0.01, 0.1); + BOOST_CHECK_CLOSE(ne.GetMinSquaredDist(ne_w), 0.01, 0.1); + BOOST_CHECK_CLOSE(ne.GetMinSquaredDist(ne_n), 0.01, 0.1); + + Coordinate se_ne{FloatLongitude(100.1), FloatLatitude(-9.9)}; + Coordinate se_nw{FloatLongitude(9.9), FloatLatitude(-9.9)}; + Coordinate se_sw{FloatLongitude(9.9), FloatLatitude(-80.1)}; + Coordinate se_se{FloatLongitude(100.1), FloatLatitude(-80.1)}; + Coordinate se_n{FloatLongitude(55), FloatLatitude(-9.9)}; + Coordinate se_w{FloatLongitude(9.9), FloatLatitude(-45.0)}; + Coordinate se_e{FloatLongitude(100.1), FloatLatitude(-45.0)}; + Coordinate se_s{FloatLongitude(55), FloatLatitude(-80.1)}; + BOOST_CHECK_CLOSE(se.GetMinSquaredDist(se_sw), 0.02, 0.1); + BOOST_CHECK_CLOSE(se.GetMinSquaredDist(se_se), 0.02, 0.1); + BOOST_CHECK_CLOSE(se.GetMinSquaredDist(se_ne), 0.02, 0.1); + BOOST_CHECK_CLOSE(se.GetMinSquaredDist(se_nw), 0.02, 0.1); + BOOST_CHECK_CLOSE(se.GetMinSquaredDist(se_s), 0.01, 0.1); + BOOST_CHECK_CLOSE(se.GetMinSquaredDist(se_e), 0.01, 0.1); + BOOST_CHECK_CLOSE(se.GetMinSquaredDist(se_w), 0.01, 0.1); + BOOST_CHECK_CLOSE(se.GetMinSquaredDist(se_n), 0.01, 0.1); + + Coordinate sw_ne{FloatLongitude(-9.9), FloatLatitude(-9.9)}; + Coordinate sw_nw{FloatLongitude(-100.1), FloatLatitude(-9.9)}; + Coordinate sw_sw{FloatLongitude(-100.1), FloatLatitude(-80.1)}; + Coordinate sw_se{FloatLongitude(-9.9), FloatLatitude(-80.1)}; + Coordinate sw_n{FloatLongitude(-55), FloatLatitude(-9.9)}; + Coordinate sw_w{FloatLongitude(-100.1), FloatLatitude(-45.0)}; + Coordinate sw_e{FloatLongitude(-9.9), FloatLatitude(-45.0)}; + Coordinate sw_s{FloatLongitude(-55), FloatLatitude(-80.1)}; + BOOST_CHECK_CLOSE(sw.GetMinSquaredDist(sw_sw), 0.02, 0.1); + BOOST_CHECK_CLOSE(sw.GetMinSquaredDist(sw_se), 0.02, 0.1); + BOOST_CHECK_CLOSE(sw.GetMinSquaredDist(sw_ne), 0.02, 0.1); + BOOST_CHECK_CLOSE(sw.GetMinSquaredDist(sw_nw), 0.02, 0.1); + BOOST_CHECK_CLOSE(sw.GetMinSquaredDist(sw_s), 0.01, 0.1); + BOOST_CHECK_CLOSE(sw.GetMinSquaredDist(sw_e), 0.01, 0.1); + BOOST_CHECK_CLOSE(sw.GetMinSquaredDist(sw_w), 0.01, 0.1); + BOOST_CHECK_CLOSE(sw.GetMinSquaredDist(sw_n), 0.01, 0.1); } BOOST_AUTO_TEST_SUITE_END() diff --git a/unit_tests/util/static_rtree.cpp b/unit_tests/util/static_rtree.cpp index afbe831f0..a9170be1d 100644 --- a/unit_tests/util/static_rtree.cpp +++ b/unit_tests/util/static_rtree.cpp @@ -1,18 +1,18 @@ -#include "util/coordinate_calculation.hpp" -#include "engine/geospatial_query.hpp" -#include "util/static_rtree.hpp" #include "extractor/edge_based_node.hpp" +#include "engine/geospatial_query.hpp" #include "util/typedefs.hpp" #include "util/rectangle.hpp" #include "util/exception.hpp" +#include "util/coordinate_calculation.hpp" +#include "util/coordinate.hpp" +#include "util/static_rtree.hpp" #include "mocks/mock_datafacade.hpp" -#include #include +#include #include - -#include +#include #include #include @@ -44,8 +44,8 @@ using MiniStaticRTree = StaticRTree, false, 2, // Choosen by a fair W20 dice roll (this value is completely arbitrary) constexpr unsigned RANDOM_SEED = 42; -static const int32_t WORLD_MIN_LAT = -90 * COORDINATE_PRECISION; -static const int32_t WORLD_MAX_LAT = 90 * COORDINATE_PRECISION; +static const int32_t WORLD_MIN_LAT = -85 * COORDINATE_PRECISION; +static const int32_t WORLD_MAX_LAT = 85 * COORDINATE_PRECISION; static const int32_t WORLD_MIN_LON = -180 * COORDINATE_PRECISION; static const int32_t WORLD_MAX_LON = 180 * COORDINATE_PRECISION; @@ -62,18 +62,23 @@ template class LinearSearchNN { std::vector local_edges(edges); - std::nth_element( - local_edges.begin(), local_edges.begin() + num_results, local_edges.end(), - [this, &input_coordinate](const DataT &lhs, const DataT &rhs) - { - double current_ratio = 0.; - Coordinate nearest; - const double lhs_dist = coordinate_calculation::perpendicularDistance( - coords->at(lhs.u), coords->at(lhs.v), input_coordinate, nearest, current_ratio); - const double rhs_dist = coordinate_calculation::perpendicularDistance( - coords->at(rhs.u), coords->at(rhs.v), input_coordinate, nearest, current_ratio); - return lhs_dist < rhs_dist; - }); + auto projected_input = coordinate_calculation::mercator::fromWGS84(input_coordinate); + const auto segment_comparator = [this, &projected_input](const DataT &lhs, const DataT &rhs) + { + using coordinate_calculation::mercator::fromWGS84; + const auto lhs_result = coordinate_calculation::projectPointOnSegment( + fromWGS84(coords->at(lhs.u)), fromWGS84(coords->at(lhs.v)), projected_input); + const auto rhs_result = coordinate_calculation::projectPointOnSegment( + fromWGS84(coords->at(rhs.u)), fromWGS84(coords->at(rhs.v)), projected_input); + const auto lhs_squared_dist = coordinate_calculation::squaredEuclideanDistance( + lhs_result.second, projected_input); + const auto rhs_squared_dist = coordinate_calculation::squaredEuclideanDistance( + rhs_result.second, projected_input); + return lhs_squared_dist < rhs_squared_dist; + }; + + std::nth_element(local_edges.begin(), local_edges.begin() + num_results, local_edges.end(), + segment_comparator); local_edges.resize(num_results); return local_edges; @@ -102,8 +107,6 @@ template struct RandomGraphFixture RandomGraphFixture() : coords(std::make_shared>()) { - BOOST_TEST_MESSAGE("Constructing " << NUM_NODES << " nodes and " << NUM_EDGES << " edges."); - std::mt19937 g(RANDOM_SEED); std::uniform_int_distribution<> lat_udist(WORLD_MIN_LAT, WORLD_MAX_LAT); @@ -189,7 +192,6 @@ void simple_verify_rtree(RTreeT &rtree, const std::shared_ptr> &coords, const std::vector &edges) { - BOOST_TEST_MESSAGE("Verify end points"); for (const auto &e : edges) { const Coordinate &pu = coords->at(e.u); @@ -217,7 +219,6 @@ void sampling_verify_rtree(RTreeT &rtree, queries.emplace_back(FixedLongitude(lon_udist(g)), FixedLatitude(lat_udist(g))); } - BOOST_TEST_MESSAGE("Sampling queries"); for (const auto &q : queries) { auto result_rtree = rtree.Nearest(q, 1); @@ -229,13 +230,15 @@ void sampling_verify_rtree(RTreeT &rtree, auto lsnn_u = result_lsnn.back().u; auto lsnn_v = result_lsnn.back().v; - double current_ratio = 0.; - Coordinate nearest; + Coordinate rtree_nearest; + Coordinate lsnn_nearest; + double ratio; const double rtree_dist = coordinate_calculation::perpendicularDistance( - coords[rtree_u], coords[rtree_v], q, nearest, current_ratio); + coords[rtree_u], coords[rtree_v], q, rtree_nearest, ratio); const double lsnn_dist = coordinate_calculation::perpendicularDistance( - coords[lsnn_u], coords[lsnn_v], q, nearest, current_ratio); - BOOST_CHECK_LE(std::abs(rtree_dist - lsnn_dist), std::numeric_limits::epsilon()); + coords[lsnn_u], coords[lsnn_v], q, lsnn_nearest, ratio); + + BOOST_CHECK_CLOSE(rtree_dist, lsnn_dist, 0.0001); } } @@ -303,18 +306,16 @@ BOOST_AUTO_TEST_CASE(regression_test) using Edge = std::pair; GraphFixture fixture( { - Coord{FloatLongitude{0.0}, FloatLatitude{40.0}}, // - Coord{FloatLongitude{5.0}, FloatLatitude{35.0}}, // - Coord{FloatLongitude{5.0}, - FloatLatitude{ - 5.0, }}, // - Coord{FloatLongitude{10.0}, FloatLatitude{0.0}}, // - Coord{FloatLongitude{10.0}, FloatLatitude{20.0}}, // - Coord{FloatLongitude{5.0}, FloatLatitude{20.0}}, // - Coord{FloatLongitude{100.0}, FloatLatitude{40.0}}, // - Coord{FloatLongitude{105.0}, FloatLatitude{35.0}}, // - Coord{FloatLongitude{105.0}, FloatLatitude{5.0}}, // - Coord{FloatLongitude{110.0}, FloatLatitude{0.0}}, // + Coord{FloatLongitude{0.0}, FloatLatitude{40.0}}, // + Coord{FloatLongitude{5.0}, FloatLatitude{35.0}}, // + Coord{FloatLongitude{5.0}, FloatLatitude{5.0}}, // + Coord{FloatLongitude{10.0}, FloatLatitude{0.0}}, // + Coord{FloatLongitude{10.0}, FloatLatitude{20.0}}, // + Coord{FloatLongitude{5.0}, FloatLatitude{20.0}}, // + Coord{FloatLongitude{100.0}, FloatLatitude{40.0}}, // + Coord{FloatLongitude{105.0}, FloatLatitude{35.0}}, // + Coord{FloatLongitude{105.0}, FloatLatitude{5.0}}, // + Coord{FloatLongitude{110.0}, FloatLatitude{0.0}}, // }, {Edge(0, 1), Edge(2, 3), Edge(4, 5), Edge(6, 7), Edge(8, 9)}); @@ -330,6 +331,13 @@ BOOST_AUTO_TEST_CASE(regression_test) auto result_rtree = rtree.Nearest(input, 1); auto result_ls = lsnn.Nearest(input, 1); + auto distance_rtree = coordinate_calculation::perpendicularDistance( + fixture.coords->at(result_rtree.front().u), fixture.coords->at(result_rtree.front().v), + input); + + auto distance_lsnn = coordinate_calculation::perpendicularDistance( + fixture.coords->at(result_ls.front().u), fixture.coords->at(result_ls.front().v), input); + BOOST_CHECK(result_rtree.size() == 1); BOOST_CHECK(result_ls.size() == 1); @@ -337,69 +345,14 @@ BOOST_AUTO_TEST_CASE(regression_test) BOOST_CHECK_EQUAL(result_ls.front().v, result_rtree.front().v); } -void TestRectangle(double width, double height, double center_lat, double center_lon) -{ - Coordinate center{FloatLongitude(center_lon), FloatLatitude(center_lat)}; - - TestStaticRTree::Rectangle rect; - rect.min_lat = center.lat - FixedLatitude(height / 2.0 * COORDINATE_PRECISION); - rect.max_lat = center.lat + FixedLatitude(height / 2.0 * COORDINATE_PRECISION); - rect.min_lon = center.lon - FixedLongitude(width / 2.0 * COORDINATE_PRECISION); - rect.max_lon = center.lon + FixedLongitude(width / 2.0 * COORDINATE_PRECISION); - - const FixedLongitude lon_offset(5. * COORDINATE_PRECISION); - const FixedLatitude lat_offset(5. * COORDINATE_PRECISION); - Coordinate north(center.lon, rect.max_lat + lat_offset); - Coordinate south(center.lon, rect.min_lat - lat_offset); - Coordinate west(rect.min_lon - lon_offset, center.lat); - Coordinate east(rect.max_lon + lon_offset, center.lat); - Coordinate north_east(rect.max_lon + lon_offset, rect.max_lat + lat_offset); - Coordinate north_west(rect.min_lon - lon_offset, rect.max_lat + lat_offset); - Coordinate south_east(rect.max_lon + lon_offset, rect.min_lat - lat_offset); - Coordinate south_west(rect.min_lon - lon_offset, rect.min_lat - lat_offset); - - /* Distance to line segments of rectangle */ - BOOST_CHECK_EQUAL(rect.GetMinDist(north), coordinate_calculation::greatCircleDistance( - north, Coordinate(north.lon, rect.max_lat))); - BOOST_CHECK_EQUAL(rect.GetMinDist(south), coordinate_calculation::greatCircleDistance( - south, Coordinate(south.lon, rect.min_lat))); - BOOST_CHECK_EQUAL(rect.GetMinDist(west), coordinate_calculation::greatCircleDistance( - west, Coordinate(rect.min_lon, west.lat))); - BOOST_CHECK_EQUAL(rect.GetMinDist(east), coordinate_calculation::greatCircleDistance( - east, Coordinate(rect.max_lon, east.lat))); - - /* Distance to corner points */ - BOOST_CHECK_EQUAL(rect.GetMinDist(north_east), - coordinate_calculation::greatCircleDistance( - north_east, Coordinate(rect.max_lon, rect.max_lat))); - BOOST_CHECK_EQUAL(rect.GetMinDist(north_west), - coordinate_calculation::greatCircleDistance( - north_west, Coordinate(rect.min_lon, rect.max_lat))); - BOOST_CHECK_EQUAL(rect.GetMinDist(south_east), - coordinate_calculation::greatCircleDistance( - south_east, Coordinate(rect.max_lon, rect.min_lat))); - BOOST_CHECK_EQUAL(rect.GetMinDist(south_west), - coordinate_calculation::greatCircleDistance( - south_west, Coordinate(rect.min_lon, rect.min_lat))); -} - -BOOST_AUTO_TEST_CASE(rectangle_test) -{ - TestRectangle(10, 10, 5, 5); - TestRectangle(10, 10, -5, 5); - TestRectangle(10, 10, 5, -5); - TestRectangle(10, 10, -5, -5); - TestRectangle(10, 10, 0, 0); -} - BOOST_AUTO_TEST_CASE(bearing_tests) { using Coord = std::pair; using Edge = std::pair; GraphFixture fixture( { - Coord(FloatLongitude(0.0), FloatLatitude(0.0)), - Coord(FloatLongitude(10.0), FloatLatitude(10.0)), + Coord(FloatLongitude(0.0), FloatLatitude(0.0)), + Coord(FloatLongitude(10.0), FloatLatitude(10.0)), }, {Edge(0, 1), Edge(1, 0)}); @@ -428,9 +381,13 @@ BOOST_AUTO_TEST_CASE(bearing_tests) { auto results = query.NearestPhantomNodes(input, 5, 45, 10); BOOST_CHECK_EQUAL(results.size(), 2); + + BOOST_CHECK(results[0].phantom_node.forward_segment_id.enabled); + BOOST_CHECK(!results[0].phantom_node.reverse_segment_id.enabled); BOOST_CHECK_EQUAL(results[0].phantom_node.forward_segment_id.id, 1); - BOOST_CHECK_EQUAL(results[0].phantom_node.reverse_segment_id.id, SPECIAL_SEGMENTID); - BOOST_CHECK_EQUAL(results[1].phantom_node.forward_segment_id.id, SPECIAL_SEGMENTID); + + BOOST_CHECK(!results[1].phantom_node.forward_segment_id.enabled); + BOOST_CHECK(results[1].phantom_node.reverse_segment_id.enabled); BOOST_CHECK_EQUAL(results[1].phantom_node.reverse_segment_id.id, 1); } @@ -447,9 +404,13 @@ BOOST_AUTO_TEST_CASE(bearing_tests) { auto results = query.NearestPhantomNodesInRange(input, 11000, 45, 10); BOOST_CHECK_EQUAL(results.size(), 2); + + BOOST_CHECK(results[0].phantom_node.forward_segment_id.enabled); + BOOST_CHECK(!results[0].phantom_node.reverse_segment_id.enabled); BOOST_CHECK_EQUAL(results[0].phantom_node.forward_segment_id.id, 1); - BOOST_CHECK_EQUAL(results[0].phantom_node.reverse_segment_id.id, SPECIAL_SEGMENTID); - BOOST_CHECK_EQUAL(results[1].phantom_node.forward_segment_id.id, SPECIAL_SEGMENTID); + + BOOST_CHECK(!results[1].phantom_node.forward_segment_id.enabled); + BOOST_CHECK(results[1].phantom_node.reverse_segment_id.enabled); BOOST_CHECK_EQUAL(results[1].phantom_node.reverse_segment_id.id, 1); } } @@ -461,11 +422,11 @@ BOOST_AUTO_TEST_CASE(bbox_search_tests) GraphFixture fixture( { - Coord(FloatLongitude(0.0), FloatLatitude(0.0)), - Coord(FloatLongitude(1.0), FloatLatitude(1.0)), - Coord(FloatLongitude(2.0), FloatLatitude(2.0)), - Coord(FloatLongitude(3.0), FloatLatitude(3.0)), - Coord(FloatLongitude(4.0), FloatLatitude(4.0)), + Coord(FloatLongitude(0.0), FloatLatitude(0.0)), + Coord(FloatLongitude(1.0), FloatLatitude(1.0)), + Coord(FloatLongitude(2.0), FloatLatitude(2.0)), + Coord(FloatLongitude(3.0), FloatLatitude(3.0)), + Coord(FloatLongitude(4.0), FloatLatitude(4.0)), }, {Edge(0, 1), Edge(1, 2), Edge(2, 3), Edge(3, 4)}); @@ -478,15 +439,15 @@ BOOST_AUTO_TEST_CASE(bbox_search_tests) mockfacade); { - RectangleInt2D bbox = { - FloatLongitude(0.5), FloatLongitude(1.5), FloatLatitude(0.5), FloatLatitude(1.5)}; + RectangleInt2D bbox = {FloatLongitude(0.5), FloatLongitude(1.5), FloatLatitude(0.5), + FloatLatitude(1.5)}; auto results = query.Search(bbox); BOOST_CHECK_EQUAL(results.size(), 2); } { - RectangleInt2D bbox = { - FloatLongitude(1.5), FloatLongitude(3.5), FloatLatitude(1.5), FloatLatitude(3.5)}; + RectangleInt2D bbox = {FloatLongitude(1.5), FloatLongitude(3.5), FloatLatitude(1.5), + FloatLatitude(3.5)}; auto results = query.Search(bbox); BOOST_CHECK_EQUAL(results.size(), 3); }