Open-Harbor/osrm-backend
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 3 Wiki Activity

Fix mathematical assumptions in StaticRTree

Browse Source 
StaticRTree now uses projected coordinates internally. That means we can
use a euclidean distance measure (squared distance) for sorting the
query queue.
This commit is contained in:
Patrick Niklaus 2016-03-28 20:38:19 +02:00
parent f9350a276c
commit 30a9bc3179
12 changed files with 717 additions and 570 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

231
include/engine/geospatial_query.hpp
View File

@ -26,6 +26,7 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
{ {
using EdgeData = typename RTreeT::EdgeData; using EdgeData = typename RTreeT::EdgeData;
using CoordinateList = typename RTreeT::CoordinateList; using CoordinateList = typename RTreeT::CoordinateList;
using CandidateSegment = typename RTreeT::CandidateSegment;
public: public:
GeospatialQuery(RTreeT &rtree_, GeospatialQuery(RTreeT &rtree_,
@ -45,15 +46,17 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
std::vector<PhantomNodeWithDistance> std::vector<PhantomNodeWithDistance>
NearestPhantomNodesInRange(const util::Coordinate input_coordinate, const double max_distance) NearestPhantomNodesInRange(const util::Coordinate input_coordinate, const double max_distance)
{ {
auto results = rtree.Nearest(input_coordinate, auto results =
[](const EdgeData &) rtree.Nearest(input_coordinate,
{ [](const CandidateSegment &)
return std::make_pair(true, true); {
}, return std::make_pair(true, true);
[max_distance](const std::size_t, const double min_dist) },
{ [this, max_distance, input_coordinate](const std::size_t,
return min_dist > max_distance; const CandidateSegment &segment)
}); {
return checkSegmentDistance(input_coordinate, segment, max_distance);
});
return MakePhantomNodes(input_coordinate, results); return MakePhantomNodes(input_coordinate, results);
} }
@ -66,16 +69,17 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
const int bearing, const int bearing,
const int bearing_range) const int bearing_range)
{ {
auto results = auto results = rtree.Nearest(
rtree.Nearest(input_coordinate, input_coordinate,
[this, bearing, bearing_range, max_distance](const EdgeData &data) [this, bearing, bearing_range, max_distance](const CandidateSegment &segment)
{ {
return checkSegmentBearing(data, bearing, bearing_range); return checkSegmentBearing(segment, bearing, bearing_range);
}, },
[max_distance](const std::size_t, const double min_dist) [this, max_distance, input_coordinate](const std::size_t,
{ const CandidateSegment &segment)
return min_dist > max_distance; {
}); return checkSegmentDistance(input_coordinate, segment, max_distance);
});
return MakePhantomNodes(input_coordinate, results); return MakePhantomNodes(input_coordinate, results);
} }
@ -88,15 +92,16 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
const int bearing, const int bearing,
const int bearing_range) const int bearing_range)
{ {
auto results = rtree.Nearest(input_coordinate, auto results =
[this, bearing, bearing_range](const EdgeData &data) rtree.Nearest(input_coordinate,
{ [this, bearing, bearing_range](const CandidateSegment &segment)
return checkSegmentBearing(data, bearing, bearing_range); {
}, return checkSegmentBearing(segment, bearing, bearing_range);
[max_results](const std::size_t num_results, const double) },
{ [max_results](const std::size_t num_results, const CandidateSegment &)
return num_results >= max_results; {
}); return num_results >= max_results;
});
return MakePhantomNodes(input_coordinate, results); return MakePhantomNodes(input_coordinate, results);
} }
@ -111,16 +116,18 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
const int bearing, const int bearing,
const int bearing_range) const int bearing_range)
{ {
auto results = rtree.Nearest( auto results =
input_coordinate, rtree.Nearest(input_coordinate,
[this, bearing, bearing_range](const EdgeData &data) [this, bearing, bearing_range](const CandidateSegment &segment)
{ {
return checkSegmentBearing(data, bearing, bearing_range); return checkSegmentBearing(segment, bearing, bearing_range);
}, },
[max_results, max_distance](const std::size_t num_results, const double min_dist) [this, max_distance, max_results, input_coordinate](
{ const std::size_t num_results, const CandidateSegment &segment)
return num_results >= max_results || min_dist > max_distance; {
}); return num_results >= max_results ||
checkSegmentDistance(input_coordinate, segment, max_distance);
});
return MakePhantomNodes(input_coordinate, results); return MakePhantomNodes(input_coordinate, results);
} }
@ -130,15 +137,16 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
std::vector<PhantomNodeWithDistance> std::vector<PhantomNodeWithDistance>
NearestPhantomNodes(const util::Coordinate input_coordinate, const unsigned max_results) NearestPhantomNodes(const util::Coordinate input_coordinate, const unsigned max_results)
{ {
auto results = rtree.Nearest(input_coordinate, auto results =
[](const EdgeData &) rtree.Nearest(input_coordinate,
{ [](const CandidateSegment &)
return std::make_pair(true, true); {
}, return std::make_pair(true, true);
[max_results](const std::size_t num_results, const double) },
{ [max_results](const std::size_t num_results, const CandidateSegment &)
return num_results >= max_results; {
}); return num_results >= max_results;
});
return MakePhantomNodes(input_coordinate, results); return MakePhantomNodes(input_coordinate, results);
} }
@ -150,16 +158,18 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
const unsigned max_results, const unsigned max_results,
const double max_distance) const double max_distance)
{ {
auto results = rtree.Nearest( auto results =
input_coordinate, rtree.Nearest(input_coordinate,
[](const EdgeData &) [](const CandidateSegment &)
{ {
return std::make_pair(true, true); return std::make_pair(true, true);
}, },
[max_results, max_distance](const std::size_t num_results, const double min_dist) [this, max_distance, max_results, input_coordinate](
{ const std::size_t num_results, const CandidateSegment &segment)
return num_results >= max_results || min_dist > max_distance; {
}); return num_results >= max_results ||
checkSegmentDistance(input_coordinate, segment, max_distance);
});
return MakePhantomNodes(input_coordinate, results); return MakePhantomNodes(input_coordinate, results);
} }
@ -174,20 +184,22 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
bool has_big_component = false; bool has_big_component = false;
auto results = rtree.Nearest( auto results = rtree.Nearest(
input_coordinate, input_coordinate,
[&has_big_component, &has_small_component](const EdgeData &data) [&has_big_component, &has_small_component](const CandidateSegment &segment)
{ {
auto use_segment = auto use_segment = (!has_small_component ||
(!has_small_component || (!has_big_component && !data.component.is_tiny)); (!has_big_component && !segment.data.component.is_tiny));
auto use_directions = std::make_pair(use_segment, use_segment); auto use_directions = std::make_pair(use_segment, use_segment);
has_big_component = has_big_component || !data.component.is_tiny; has_big_component = has_big_component || !segment.data.component.is_tiny;
has_small_component = has_small_component || data.component.is_tiny; has_small_component = has_small_component || segment.data.component.is_tiny;
return use_directions; 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) if (results.size() == 0)
@ -207,23 +219,23 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
{ {
bool has_small_component = false; bool has_small_component = false;
bool has_big_component = false; bool has_big_component = false;
auto results = auto results = rtree.Nearest(
rtree.Nearest(input_coordinate, 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 || auto use_segment = (!has_small_component ||
(!has_big_component && !data.component.is_tiny)); (!has_big_component && !segment.data.component.is_tiny));
auto use_directions = std::make_pair(use_segment, use_segment); auto use_directions = std::make_pair(use_segment, use_segment);
has_big_component = has_big_component || !data.component.is_tiny; has_big_component = has_big_component || !segment.data.component.is_tiny;
has_small_component = has_small_component || data.component.is_tiny; has_small_component = has_small_component || segment.data.component.is_tiny;
return use_directions; 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; return num_results > 0 && has_big_component;
}); });
if (results.size() == 0) if (results.size() == 0)
{ {
@ -245,25 +257,25 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
auto results = rtree.Nearest( auto results = rtree.Nearest(
input_coordinate, input_coordinate,
[this, bearing, bearing_range, &has_big_component, [this, bearing, bearing_range, &has_big_component,
&has_small_component](const EdgeData &data) &has_small_component](const CandidateSegment &segment)
{ {
auto use_segment = auto use_segment = (!has_small_component ||
(!has_small_component || (!has_big_component && !data.component.is_tiny)); (!has_big_component && !segment.data.component.is_tiny));
auto use_directions = std::make_pair(use_segment, use_segment); auto use_directions = std::make_pair(use_segment, use_segment);
if (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) if (use_directions.first || use_directions.second)
{ {
has_big_component = has_big_component || !data.component.is_tiny; has_big_component = has_big_component || !segment.data.component.is_tiny;
has_small_component = has_small_component || data.component.is_tiny; has_small_component = has_small_component || segment.data.component.is_tiny;
} }
} }
return use_directions; 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; return num_results > 0 && has_big_component;
}); });
@ -291,27 +303,29 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
auto results = rtree.Nearest( auto results = rtree.Nearest(
input_coordinate, input_coordinate,
[this, bearing, bearing_range, &has_big_component, [this, bearing, bearing_range, &has_big_component,
&has_small_component](const EdgeData &data) &has_small_component](const CandidateSegment &segment)
{ {
auto use_segment = auto use_segment = (!has_small_component ||
(!has_small_component || (!has_big_component && !data.component.is_tiny)); (!has_big_component && !segment.data.component.is_tiny));
auto use_directions = std::make_pair(use_segment, use_segment); auto use_directions = std::make_pair(use_segment, use_segment);
if (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) if (use_directions.first || use_directions.second)
{ {
has_big_component = has_big_component || !data.component.is_tiny; has_big_component = has_big_component || !segment.data.component.is_tiny;
has_small_component = has_small_component || data.component.is_tiny; has_small_component = has_small_component || segment.data.component.is_tiny;
} }
} }
return use_directions; 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) if (results.size() == 0)
@ -401,15 +415,32 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
return transformed; return transformed;
} }
std::pair<bool, bool> 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<bool, bool> checkSegmentBearing(const CandidateSegment &segment,
const int filter_bearing, const int filter_bearing,
const int filter_bearing_range) const int filter_bearing_range)
{ {
BOOST_ASSERT(segment.forward_segment_id.id != SPECIAL_SEGMENTID || !segment.forward_segment_id.enabled); BOOST_ASSERT(segment.data.forward_segment_id.id != SPECIAL_SEGMENTID ||
BOOST_ASSERT(segment.reverse_segment_id.id != SPECIAL_SEGMENTID || !segment.reverse_segment_id.enabled); !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( 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 const double backward_edge_bearing = (forward_edge_bearing + 180) > 360
? (forward_edge_bearing - 180) ? (forward_edge_bearing - 180)
@ -418,11 +449,11 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
const bool forward_bearing_valid = const bool forward_bearing_valid =
util::bearing::CheckInBounds(std::round(forward_edge_bearing), filter_bearing, util::bearing::CheckInBounds(std::round(forward_edge_bearing), filter_bearing,
filter_bearing_range) && filter_bearing_range) &&
segment.forward_segment_id.enabled; segment.data.forward_segment_id.enabled;
const bool backward_bearing_valid = const bool backward_bearing_valid =
util::bearing::CheckInBounds(std::round(backward_edge_bearing), filter_bearing, util::bearing::CheckInBounds(std::round(backward_edge_bearing), filter_bearing,
filter_bearing_range) && filter_bearing_range) &&
segment.reverse_segment_id.enabled; segment.data.reverse_segment_id.enabled;
return std::make_pair(forward_bearing_valid, backward_bearing_valid); return std::make_pair(forward_bearing_valid, backward_bearing_valid);
} }

22
include/util/coordinate.hpp
View File

@ -78,6 +78,8 @@ inline FloatLongitude toFloating(const FixedLongitude fixed)
return FloatLongitude(floating); return FloatLongitude(floating);
} }
struct FloatCoordinate;
// Coordinate encoded as longitude, latitude // Coordinate encoded as longitude, latitude
struct Coordinate struct Coordinate
{ {
@ -85,6 +87,7 @@ struct Coordinate
FixedLatitude lat; FixedLatitude lat;
Coordinate(); Coordinate();
Coordinate(const FloatCoordinate &other);
Coordinate(const FixedLongitude lon_, const FixedLatitude lat_); Coordinate(const FixedLongitude lon_, const FixedLatitude lat_);
Coordinate(const FloatLongitude lon_, const FloatLatitude lat_); Coordinate(const FloatLongitude lon_, const FloatLatitude lat_);
@ -104,8 +107,27 @@ struct Coordinate
friend std::ostream &operator<<(std::ostream &out, const Coordinate 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 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 Coordinate coordinate);
std::ostream &operator<<(std::ostream &out, const FloatCoordinate coordinate);
} }
} }

46
include/util/coordinate_calculation.hpp
View File

@ -24,22 +24,26 @@ const constexpr double EARTH_RADIUS_WGS84 = 6378137.0;
namespace detail namespace detail
{ {
// earth circumference devided by 2 // earth circumference devided by 2
const constexpr double MAXEXTENT = EARTH_RADIUS_WGS84 * boost::math::constants::pi<double>(); const constexpr double MAXEXTENT = EARTH_RADIUS_WGS84 * boost::math::constants::pi<double>();
// ^ math functions are not constexpr since they have side-effects (setting errno) :( // ^ 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<double>()); const double MAX_LATITUDE = RAD_TO_DEGREE * (2.0 * std::atan(std::exp(180.0 * DEGREE_TO_RAD)) -
const constexpr double MAX_LONGITUDE = 180.0; boost::math::constants::half_pi<double>());
const constexpr double MAX_LONGITUDE = 180.0;
} }
//! Takes the squared euclidean distance of the input coordinates. Does not return meters!
//! Projects both coordinates and takes the euclidean distance of the projected points double squaredEuclideanDistance(const FloatCoordinate &lhs, const FloatCoordinate &rhs);
// Does not return meters!
double euclideanDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);
double haversineDistance(const Coordinate first_coordinate, const Coordinate second_coordinate); double haversineDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);
double greatCircleDistance(const Coordinate first_coordinate, const Coordinate second_coordinate); double greatCircleDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);
std::pair<double, FloatCoordinate>
projectPointOnSegment(const FloatCoordinate &projected_xy_source,
const FloatCoordinate &projected_xy_target,
const FloatCoordinate &projected_xy_coordinate);
double perpendicularDistance(const Coordinate segment_source, double perpendicularDistance(const Coordinate segment_source,
const Coordinate segment_target, const Coordinate segment_target,
const Coordinate query_location); const Coordinate query_location);
@ -50,20 +54,6 @@ double perpendicularDistance(const Coordinate segment_source,
Coordinate &nearest_location, Coordinate &nearest_location,
double &ratio); double &ratio);
double perpendicularDistanceFromProjectedCoordinate(
const Coordinate segment_source,
const Coordinate segment_target,
const Coordinate query_location,
const std::pair<double, double> projected_xy_coordinate);
double perpendicularDistanceFromProjectedCoordinate(
const Coordinate segment_source,
const Coordinate segment_target,
const Coordinate query_location,
const std::pair<double, double> projected_xy_coordinate,
Coordinate &nearest_location,
double &ratio);
Coordinate centroid(const Coordinate lhs, const Coordinate rhs); Coordinate centroid(const Coordinate lhs, const Coordinate rhs);
double bearing(const Coordinate first_coordinate, const Coordinate second_coordinate); 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); double degreeToPixel(FloatLongitude lon, unsigned zoom);
FloatLatitude yToLat(const double value); FloatLatitude yToLat(const double value);
double latToY(const FloatLatitude latitude); 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 mercator
} // ns coordinate_calculation } // ns coordinate_calculation
} // ns util } // ns util

52
include/util/rectangle.hpp
View File

@ -80,7 +80,10 @@ struct RectangleInt2D
min_lat > other.max_lat); 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); const bool is_contained = Contains(location);
if (is_contained) if (is_contained)
@ -117,36 +120,36 @@ struct RectangleInt2D
switch (d) switch (d)
{ {
case NORTH: case NORTH:
min_dist = coordinate_calculation::greatCircleDistance( min_dist = coordinate_calculation::squaredEuclideanDistance(
location, Coordinate(location.lon, max_lat)); location, Coordinate(location.lon, max_lat));
break; break;
case SOUTH: case SOUTH:
min_dist = coordinate_calculation::greatCircleDistance( min_dist = coordinate_calculation::squaredEuclideanDistance(
location, Coordinate(location.lon, min_lat)); location, Coordinate(location.lon, min_lat));
break; break;
case WEST: case WEST:
min_dist = coordinate_calculation::greatCircleDistance( min_dist = coordinate_calculation::squaredEuclideanDistance(
location, Coordinate(min_lon, location.lat)); location, Coordinate(min_lon, location.lat));
break; break;
case EAST: case EAST:
min_dist = coordinate_calculation::greatCircleDistance( min_dist = coordinate_calculation::squaredEuclideanDistance(
location, Coordinate(max_lon, location.lat)); location, Coordinate(max_lon, location.lat));
break; break;
case NORTH_EAST: case NORTH_EAST:
min_dist = min_dist =
coordinate_calculation::greatCircleDistance(location, Coordinate(max_lon, max_lat)); coordinate_calculation::squaredEuclideanDistance(location, Coordinate(max_lon, max_lat));
break; break;
case NORTH_WEST: case NORTH_WEST:
min_dist = min_dist =
coordinate_calculation::greatCircleDistance(location, Coordinate(min_lon, max_lat)); coordinate_calculation::squaredEuclideanDistance(location, Coordinate(min_lon, max_lat));
break; break;
case SOUTH_EAST: case SOUTH_EAST:
min_dist = min_dist =
coordinate_calculation::greatCircleDistance(location, Coordinate(max_lon, min_lat)); coordinate_calculation::squaredEuclideanDistance(location, Coordinate(max_lon, min_lat));
break; break;
case SOUTH_WEST: case SOUTH_WEST:
min_dist = min_dist =
coordinate_calculation::greatCircleDistance(location, Coordinate(min_lon, min_lat)); coordinate_calculation::squaredEuclideanDistance(location, Coordinate(min_lon, min_lat));
break; break;
default: default:
break; break;
@ -157,37 +160,6 @@ struct RectangleInt2D
return min_dist; return min_dist;
} }
double GetMinMaxDist(const Coordinate location) const
{
double min_max_dist = std::numeric_limits<double>::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 bool Contains(const Coordinate location) const
{ {
const bool lons_contained = (location.lon >= min_lon) && (location.lon <= max_lon); const bool lons_contained = (location.lon >= min_lon) && (location.lon <= max_lon);

120
include/util/static_rtree.hpp
View File

@ -36,6 +36,8 @@ namespace util
{ {
// Static RTree for serving nearest neighbour queries // 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 <class EdgeDataT, template <class EdgeDataT,
class CoordinateListT = std::vector<Coordinate>, class CoordinateListT = std::vector<Coordinate>,
bool UseSharedMemory = false, bool UseSharedMemory = false,
@ -48,7 +50,11 @@ class StaticRTree
using EdgeData = EdgeDataT; using EdgeData = EdgeDataT;
using CoordinateList = CoordinateListT; 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 struct TreeNode
{ {
@ -86,16 +92,16 @@ class StaticRTree
} }
}; };
using QueryNodeType = mapbox::util::variant<TreeNode, EdgeDataT>; using QueryNodeType = mapbox::util::variant<TreeNode, CandidateSegment>;
struct QueryCandidate struct QueryCandidate
{ {
inline bool operator<(const QueryCandidate &other) const inline bool operator<(const QueryCandidate &other) const
{ {
// Attn: this is reversed order. std::pq is a max pq! // 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; QueryNodeType node;
}; };
@ -315,9 +321,16 @@ class StaticRTree
leaves_stream.read((char *)&m_element_count, sizeof(uint64_t)); 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<EdgeDataT> SearchInBox(const Rectangle &search_rectangle) std::vector<EdgeDataT> 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<EdgeDataT> results; std::vector<EdgeDataT> results;
std::queue<TreeNode> traversal_queue; std::queue<TreeNode> traversal_queue;
@ -377,11 +390,11 @@ class StaticRTree
std::vector<EdgeDataT> Nearest(const Coordinate input_coordinate, const std::size_t max_results) std::vector<EdgeDataT> Nearest(const Coordinate input_coordinate, const std::size_t max_results)
{ {
return Nearest(input_coordinate, return Nearest(input_coordinate,
[](const EdgeDataT &) [](const CandidateSegment &)
{ {
return std::make_pair(true, true); 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; return num_results >= max_results;
}); });
@ -393,9 +406,8 @@ class StaticRTree
Nearest(const Coordinate input_coordinate, const FilterT filter, const TerminationT terminate) Nearest(const Coordinate input_coordinate, const FilterT filter, const TerminationT terminate)
{ {
std::vector<EdgeDataT> results; std::vector<EdgeDataT> results;
std::pair<double, double> projected_coordinate = { auto projected_coordinate = coordinate_calculation::mercator::fromWGS84(input_coordinate);
static_cast<double>(toFloating(input_coordinate.lon)), Coordinate fixed_projected_coordinate{projected_coordinate};
coordinate_calculation::mercator::latToY(toFloating(input_coordinate.lat))};
// initialize queue with root element // initialize queue with root element
std::priority_queue<QueryCandidate> traversal_queue; std::priority_queue<QueryCandidate> traversal_queue;
@ -403,13 +415,7 @@ class StaticRTree
while (!traversal_queue.empty()) while (!traversal_queue.empty())
{ {
const QueryCandidate current_query_node = traversal_queue.top(); QueryCandidate current_query_node = traversal_queue.top();
if (terminate(results.size(), current_query_node.min_dist))
{
traversal_queue = std::priority_queue<QueryCandidate>{};
break;
}
traversal_queue.pop(); traversal_queue.pop();
if (current_query_node.node.template is<TreeNode>()) if (current_query_node.node.template is<TreeNode>())
@ -418,30 +424,34 @@ class StaticRTree
current_query_node.node.template get<TreeNode>(); current_query_node.node.template get<TreeNode>();
if (current_tree_node.child_is_on_disk) if (current_tree_node.child_is_on_disk)
{ {
ExploreLeafNode(current_tree_node.children[0], input_coordinate, ExploreLeafNode(current_tree_node.children[0], projected_coordinate,
projected_coordinate, traversal_queue); traversal_queue);
} }
else else
{ {
ExploreTreeNode(current_tree_node, input_coordinate, traversal_queue); ExploreTreeNode(current_tree_node, fixed_projected_coordinate, traversal_queue);
} }
} }
else else
{ {
// inspecting an actual road segment // inspecting an actual road segment
const auto &current_segment = current_query_node.node.template get<EdgeDataT>(); auto &current_candidate = current_query_node.node.template get<CandidateSegment>();
if (terminate(results.size(), current_candidate))
{
traversal_queue = std::priority_queue<QueryCandidate>{};
break;
}
auto use_segment = filter(current_segment); auto use_segment = filter(current_candidate);
if (!use_segment.first && !use_segment.second) if (!use_segment.first && !use_segment.second)
{ {
continue; 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 // store phantom node in result vector
results.push_back(std::move(current_segment)); results.push_back(std::move(current_candidate.data));
results.back().forward_segment_id.enabled &= use_segment.first;
results.back().reverse_segment_id.enabled &= use_segment.second;
} }
} }
@ -451,8 +461,7 @@ class StaticRTree
private: private:
template <typename QueueT> template <typename QueueT>
void ExploreLeafNode(const std::uint32_t leaf_id, void ExploreLeafNode(const std::uint32_t leaf_id,
const Coordinate input_coordinate, const FloatCoordinate &projected_input_coordinate,
const std::pair<double, double> &projected_coordinate,
QueueT &traversal_queue) QueueT &traversal_queue)
{ {
LeafNode current_leaf_node; LeafNode current_leaf_node;
@ -462,21 +471,30 @@ class StaticRTree
for (const auto i : irange(0u, current_leaf_node.object_count)) for (const auto i : irange(0u, current_leaf_node.object_count))
{ {
auto &current_edge = current_leaf_node.objects[i]; auto &current_edge = current_leaf_node.objects[i];
const float current_perpendicular_distance = auto projected_u =
coordinate_calculation::perpendicularDistanceFromProjectedCoordinate( coordinate_calculation::mercator::fromWGS84((*m_coordinate_list)[current_edge.u]);
m_coordinate_list->at(current_edge.u), m_coordinate_list->at(current_edge.v), auto projected_v =
input_coordinate, projected_coordinate); 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 // distance must be non-negative
BOOST_ASSERT(0.f <= current_perpendicular_distance); BOOST_ASSERT(0. <= squared_distance);
traversal_queue.push( traversal_queue.push(
QueryCandidate{current_perpendicular_distance, std::move(current_edge)}); QueryCandidate{squared_distance, CandidateSegment{Coordinate{projected_nearest},
std::move(current_edge)}});
} }
} }
template <class QueueT> template <class QueueT>
void ExploreTreeNode(const TreeNode &parent, void ExploreTreeNode(const TreeNode &parent,
const Coordinate input_coordinate, const Coordinate fixed_projected_input_coordinate,
QueueT &traversal_queue) QueueT &traversal_queue)
{ {
for (std::uint32_t i = 0; i < parent.child_count; ++i) 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 std::int32_t child_id = parent.children[i];
const auto &child_tree_node = m_search_tree[child_id]; const auto &child_tree_node = m_search_tree[child_id];
const auto &child_rectangle = child_tree_node.minimum_bounding_rectangle; const auto &child_rectangle = child_tree_node.minimum_bounding_rectangle;
const float lower_bound_to_element = child_rectangle.GetMinDist(input_coordinate); const auto squared_lower_bound_to_element =
traversal_queue.push(QueryCandidate{lower_bound_to_element, m_search_tree[child_id]}); 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].u < coordinate_list.size());
BOOST_ASSERT(objects[i].v < 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 = rectangle.min_lon =
std::min(rectangle.min_lon, std::min(coordinate_list[objects[i].u].lon, std::min(rectangle.min_lon, std::min(projected_u.lon, projected_v.lon));
coordinate_list[objects[i].v].lon));
rectangle.max_lon = rectangle.max_lon =
std::max(rectangle.max_lon, std::max(coordinate_list[objects[i].u].lon, std::max(rectangle.max_lon, std::max(projected_u.lon, projected_v.lon));
coordinate_list[objects[i].v].lon));
rectangle.min_lat = rectangle.min_lat =
std::min(rectangle.min_lat, std::min(coordinate_list[objects[i].u].lat, std::min(rectangle.min_lat, std::min(projected_u.lat, projected_v.lat));
coordinate_list[objects[i].v].lat));
rectangle.max_lat = rectangle.max_lat =
std::max(rectangle.max_lat, std::max(coordinate_list[objects[i].u].lat, std::max(rectangle.max_lat, std::max(projected_u.lat, projected_v.lat));
coordinate_list[objects[i].v].lat));
} }
BOOST_ASSERT(rectangle.min_lon != FixedLongitude(std::numeric_limits<int>::min())); BOOST_ASSERT(rectangle.min_lon != FixedLongitude(std::numeric_limits<int>::min()));
BOOST_ASSERT(rectangle.min_lat != FixedLatitude(std::numeric_limits<int>::min())); BOOST_ASSERT(rectangle.min_lat != FixedLatitude(std::numeric_limits<int>::min()));

6
src/engine/plugins/tile.cpp
View File

@ -30,7 +30,7 @@ namespace detail
const constexpr double VECTOR_TILE_EXTENT = 4096.0; const constexpr double VECTOR_TILE_EXTENT = 4096.0;
const constexpr double VECTOR_TILE_BUFFER = 128.0; const constexpr double VECTOR_TILE_BUFFER = 128.0;
// Simple container class for WSG84 coordinates // Simple container class for WGS84 coordinates
template <typename T> struct Point final template <typename T> struct Point final
{ {
Point(T _x, T _y) : x(_x), y(_y) {} Point(T _x, T _y) : x(_x), y(_y) {}
@ -173,8 +173,8 @@ Status TilePlugin::HandleRequest(const api::TileParameters &parameters, std::str
using namespace util::coordinate_calculation; using namespace util::coordinate_calculation;
double min_lon, min_lat, max_lon, max_lat; double min_lon, min_lat, max_lon, max_lat;
// Convert the z,x,y mercator tile coordinates into WSG84 lon/lat values // Convert the z,x,y mercator tile coordinates into WGS84 lon/lat values
mercator::xyzToWSG84(parameters.x, parameters.y, parameters.z, min_lon, min_lat, max_lon, mercator::xyzToWGS84(parameters.x, parameters.y, parameters.z, min_lon, min_lat, max_lon,
max_lat); max_lat);
util::Coordinate southwest{util::FloatLongitude(min_lon), util::FloatLatitude(min_lat)}; util::Coordinate southwest{util::FloatLongitude(min_lon), util::FloatLatitude(min_lat)};

44
src/util/coordinate.cpp
View File

@ -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::Coordinate(const FloatLongitude lon_, const FloatLatitude lat_)
: Coordinate(toFixed(lon_), toFixed(lat_)) : Coordinate(toFixed(lon_), toFixed(lat_))
{ {
@ -39,12 +44,45 @@ bool Coordinate::IsValid() const
lon < FixedLongitude(-180 * COORDINATE_PRECISION)); lon < FixedLongitude(-180 * COORDINATE_PRECISION));
} }
FloatCoordinate::FloatCoordinate()
: lon(std::numeric_limits<double>::min()), lat(std::numeric_limits<double>::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) bool operator==(const Coordinate lhs, const Coordinate rhs)
{ {
return lhs.lat == rhs.lat && lhs.lon == rhs.lon; 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 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) 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) << ")"; << ", lat:" << toFloating(coordinate.lat) << ")";
return out; return out;
} }
std::ostream &operator<<(std::ostream &out, const FloatCoordinate coordinate)
{
out << std::setprecision(12) << "(lon:" << coordinate.lon
<< ", lat:" << coordinate.lat << ")";
return out;
}
} }
} }

190
src/util/coordinate_calculation.cpp
View File

@ -17,16 +17,13 @@ namespace util
namespace coordinate_calculation 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<double>(toFloating(coordinate_1.lon)); const double dx = static_cast<double>(lhs.lon - rhs.lon);
const double y1 = mercator::latToY(toFloating(coordinate_1.lat)); const double dy = static_cast<double>(lhs.lat - rhs.lat);
const double x2 = static_cast<double>(toFloating(coordinate_2.lon));
const double y2 = mercator::latToY(toFloating(coordinate_2.lat));
const double dx = x1 - x2;
const double dy = y1 - y2;
return std::sqrt(dx * dx + dy * dy); return dx * dx + dy * dy;
} }
double haversineDistance(const Coordinate coordinate_1, const Coordinate coordinate_2) 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; return std::hypot(x_value, y_value) * EARTH_RADIUS;
} }
double perpendicularDistance(const Coordinate source_coordinate, std::pair<double, FloatCoordinate> projectPointOnSegment(const FloatCoordinate &source,
const Coordinate target_coordinate, const FloatCoordinate &target,
const Coordinate query_location) const FloatCoordinate &coordinate)
{ {
double ratio; const FloatCoordinate slope_vector{target.lon - source.lon, target.lat - source.lat};
Coordinate nearest_location; 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<double>(slope_vector.lon * rel_coordinate.lon) +
static_cast<double>(slope_vector.lat * rel_coordinate.lat);
// squared length of the slope vector
const auto squared_length = static_cast<double>(slope_vector.lon * slope_vector.lon) +
static_cast<double>(slope_vector.lat * slope_vector.lat);
return perpendicularDistance(source_coordinate, target_coordinate, query_location, if (squared_length < std::numeric_limits<double>::epsilon())
nearest_location, ratio); {
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, double perpendicularDistance(const Coordinate segment_source,
@ -98,108 +119,29 @@ double perpendicularDistance(const Coordinate segment_source,
{ {
using namespace coordinate_calculation; using namespace coordinate_calculation;
return perpendicularDistanceFromProjectedCoordinate(
segment_source, segment_target, query_location,
{static_cast<double>(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<double, double> 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<double, double> projected_xy_coordinate,
Coordinate &nearest_location,
double &ratio)
{
using namespace coordinate_calculation;
BOOST_ASSERT(query_location.IsValid()); BOOST_ASSERT(query_location.IsValid());
// initialize values FloatCoordinate projected_nearest;
const double x = projected_xy_coordinate.second; std::tie(ratio, projected_nearest) =
const double y = projected_xy_coordinate.first; projectPointOnSegment(mercator::fromWGS84(segment_source), mercator::fromWGS84(segment_target), mercator::fromWGS84(query_location));
const double a = mercator::latToY(toFloating(segment_source.lat)); nearest_location = mercator::toWGS84(projected_nearest);
const double b = static_cast<double>(toFloating(segment_source.lon));
const double c = mercator::latToY(toFloating(segment_target.lat));
const double d = static_cast<double>(toFloating(segment_target.lon));
double p, q /*,mX*/, new_y;
if (std::abs(a - c) > std::numeric_limits<double>::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<double>((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<double>::epsilon())
{
ratio = 0.0;
}
else if (std::abs(ratio - 1.0) <= std::numeric_limits<double>::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());
const double approximate_distance = greatCircleDistance(query_location, nearest_location); const double approximate_distance = greatCircleDistance(query_location, nearest_location);
BOOST_ASSERT(0.0 <= approximate_distance); BOOST_ASSERT(0.0 <= approximate_distance);
return 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(const Coordinate lhs, const Coordinate rhs)
{ {
Coordinate centroid; Coordinate centroid;
@ -283,7 +225,9 @@ namespace mercator
{ {
FloatLatitude yToLat(const double y) 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.); return FloatLatitude(normalized_lat - 90.);
} }
@ -292,7 +236,9 @@ double latToY(const FloatLatitude latitude)
{ {
const double normalized_lat = 90. + static_cast<double>(latitude); const double normalized_lat = 90. + static_cast<double>(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) 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; x = (x - b) / shift * 360.0;
// FIXME needs to be simplified // FIXME needs to be simplified
const double g = (y - b) / -(shift / (2 * M_PI)) / DEGREE_TO_RAD; 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<double>(util::coordinate_calculation::mercator::yToLat(g)); y = static_cast<double>(util::coordinate_calculation::mercator::yToLat(g));
} }
@ -333,8 +279,19 @@ double degreeToPixel(FloatLatitude lat, unsigned zoom)
return y; return y;
} }
// Converts a WMS tile coordinate (z,x,y) into a wsg84 bounding box FloatCoordinate fromWGS84(const FloatCoordinate &wgs84_coordinate)
void xyzToWSG84(const int x, const int y, const int z, double &minx, double &miny, double &maxx, double &maxy) {
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<double>(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; 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 // 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; 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<double>(clamp(util::FloatLongitude(minx))) * DEGREE_TO_PX; minx = static_cast<double>(clamp(util::FloatLongitude(minx))) * DEGREE_TO_PX;
miny = latToY(clamp(util::FloatLatitude(miny))) * DEGREE_TO_PX; miny = latToY(clamp(util::FloatLatitude(miny))) * DEGREE_TO_PX;

92
unit_tests/util/coordinate.cpp
View File

@ -1,92 +0,0 @@
#include <boost/test/unit_test.hpp>
#include "util/coordinate_calculation.hpp"
#include <osrm/coordinate.hpp>
#include <cmath>
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);
}

191
unit_tests/util/coordinate_calculation.cpp Normal file
View File

@ -0,0 +1,191 @@
#include <boost/test/unit_test.hpp>
#include "util/coordinate_calculation.hpp"
#include <osrm/coordinate.hpp>
#include <cmath>
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<double>(start.lon + FloatLongitude(ratio) * diff.lon), static_cast<double>(nearest.lon), 0.1);
BOOST_CHECK_CLOSE(static_cast<double>(start.lat + FloatLatitude(ratio) * diff.lat), static_cast<double>(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()

110
unit_tests/util/rectangle.cpp
View File

@ -26,48 +26,82 @@ BOOST_AUTO_TEST_CASE(get_min_dist_test)
// | // |
// +- -80 // +- -80
// | // |
RectangleInt2D nw{FloatLongitude(10), FloatLongitude(100), FloatLatitude(10), RectangleInt2D ne{FloatLongitude(10), FloatLongitude(100), FloatLatitude(10),
FloatLatitude(80)}; FloatLatitude(80)};
// RectangleInt2D ne {FloatLongitude(-100), FloatLongitude(-10), FloatLatitude(10), RectangleInt2D nw{FloatLongitude(-100), FloatLongitude(-10), FloatLatitude(10),
// FloatLatitude(80)}; FloatLatitude(80)};
// RectangleInt2D sw {FloatLongitude(10), FloatLongitude(100), FloatLatitude(-80), RectangleInt2D se{FloatLongitude(10), FloatLongitude(100), FloatLatitude(-80),
// FloatLatitude(-10)}; FloatLatitude(-10)};
RectangleInt2D se{FloatLongitude(-100), FloatLongitude(-10), FloatLatitude(-80), RectangleInt2D sw{FloatLongitude(-100), FloatLongitude(-10), FloatLatitude(-80),
FloatLatitude(-10)}; FloatLatitude(-10)};
Coordinate nw_sw{FloatLongitude(9.9), FloatLatitude(9.9)}; Coordinate nw_sw{FloatLongitude(-100.1), FloatLatitude(9.9)};
Coordinate nw_se{FloatLongitude(100.1), FloatLatitude(9.9)}; Coordinate nw_se{FloatLongitude(-9.9), FloatLatitude(9.9)};
Coordinate nw_ne{FloatLongitude(100.1), FloatLatitude(80.1)}; Coordinate nw_ne{FloatLongitude(-9.9), FloatLatitude(80.1)};
Coordinate nw_nw{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_s{FloatLongitude(-55), FloatLatitude(9.9)};
Coordinate nw_e{FloatLongitude(100.1), FloatLatitude(45.0)}; Coordinate nw_e{FloatLongitude(-9.9), FloatLatitude(45.0)};
Coordinate nw_w{FloatLongitude(9.9), FloatLatitude(45.0)}; Coordinate nw_w{FloatLongitude(-100.1), FloatLatitude(45.0)};
Coordinate nw_n{FloatLongitude(55), FloatLatitude(80.1)}; Coordinate nw_n{FloatLongitude(-55), FloatLatitude(80.1)};
BOOST_CHECK_CLOSE(nw.GetMinDist(nw_sw), 15611.9, 0.1); BOOST_CHECK_CLOSE(nw.GetMinSquaredDist(nw_sw), 0.02, 0.1);
BOOST_CHECK_CLOSE(nw.GetMinDist(nw_se), 15611.9, 0.1); BOOST_CHECK_CLOSE(nw.GetMinSquaredDist(nw_se), 0.02, 0.1);
BOOST_CHECK_CLOSE(nw.GetMinDist(nw_ne), 11287.4, 0.1); BOOST_CHECK_CLOSE(nw.GetMinSquaredDist(nw_ne), 0.02, 0.1);
BOOST_CHECK_CLOSE(nw.GetMinDist(nw_nw), 11287.4, 0.1); BOOST_CHECK_CLOSE(nw.GetMinSquaredDist(nw_nw), 0.02, 0.1);
BOOST_CHECK_CLOSE(nw.GetMinDist(nw_s), 11122.6, 0.1); BOOST_CHECK_CLOSE(nw.GetMinSquaredDist(nw_s), 0.01, 0.1);
BOOST_CHECK_CLOSE(nw.GetMinDist(nw_e), 7864.89, 0.1); BOOST_CHECK_CLOSE(nw.GetMinSquaredDist(nw_e), 0.01, 0.1);
BOOST_CHECK_CLOSE(nw.GetMinDist(nw_w), 7864.89, 0.1); BOOST_CHECK_CLOSE(nw.GetMinSquaredDist(nw_w), 0.01, 0.1);
BOOST_CHECK_CLOSE(nw.GetMinDist(nw_n), 11122.6, 0.1); BOOST_CHECK_CLOSE(nw.GetMinSquaredDist(nw_n), 0.01, 0.1);
Coordinate se_ne{FloatLongitude(-9.9), FloatLatitude(-9.9)}; Coordinate ne_sw{FloatLongitude(9.9), FloatLatitude(9.9)};
Coordinate se_nw{FloatLongitude(-100.1), FloatLatitude(-9.9)}; Coordinate ne_se{FloatLongitude(100.1), FloatLatitude(9.9)};
Coordinate se_sw{FloatLongitude(-100.1), FloatLatitude(-80.1)}; Coordinate ne_ne{FloatLongitude(100.1), FloatLatitude(80.1)};
Coordinate se_se{FloatLongitude(-9.9), FloatLatitude(-80.1)}; Coordinate ne_nw{FloatLongitude(9.9), FloatLatitude(80.1)};
Coordinate se_n{FloatLongitude(-55), FloatLatitude(-9.9)}; Coordinate ne_s{FloatLongitude(55), FloatLatitude(9.9)};
Coordinate se_w{FloatLongitude(-100.1), FloatLatitude(-45.0)}; Coordinate ne_e{FloatLongitude(100.1), FloatLatitude(45.0)};
Coordinate se_e{FloatLongitude(-9.9), FloatLatitude(-45.0)}; Coordinate ne_w{FloatLongitude(9.9), FloatLatitude(45.0)};
Coordinate se_s{FloatLongitude(-55), FloatLatitude(-80.1)}; Coordinate ne_n{FloatLongitude(55), FloatLatitude(80.1)};
BOOST_CHECK_CLOSE(se.GetMinDist(se_sw), 11287.4, 0.1); BOOST_CHECK_CLOSE(ne.GetMinSquaredDist(ne_sw), 0.02, 0.1);
BOOST_CHECK_CLOSE(se.GetMinDist(se_se), 11287.4, 0.1); BOOST_CHECK_CLOSE(ne.GetMinSquaredDist(ne_se), 0.02, 0.1);
BOOST_CHECK_CLOSE(se.GetMinDist(se_ne), 15611.9, 0.1); BOOST_CHECK_CLOSE(ne.GetMinSquaredDist(ne_ne), 0.02, 0.1);
BOOST_CHECK_CLOSE(se.GetMinDist(se_nw), 15611.9, 0.1); BOOST_CHECK_CLOSE(ne.GetMinSquaredDist(ne_nw), 0.02, 0.1);
BOOST_CHECK_CLOSE(se.GetMinDist(se_s), 11122.6, 0.1); BOOST_CHECK_CLOSE(ne.GetMinSquaredDist(ne_s), 0.01, 0.1);
BOOST_CHECK_CLOSE(se.GetMinDist(se_e), 7864.89, 0.1); BOOST_CHECK_CLOSE(ne.GetMinSquaredDist(ne_e), 0.01, 0.1);
BOOST_CHECK_CLOSE(se.GetMinDist(se_w), 7864.89, 0.1); BOOST_CHECK_CLOSE(ne.GetMinSquaredDist(ne_w), 0.01, 0.1);
BOOST_CHECK_CLOSE(se.GetMinDist(se_n), 11122.6, 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() BOOST_AUTO_TEST_SUITE_END()

183
unit_tests/util/static_rtree.cpp
View File

@ -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 "extractor/edge_based_node.hpp"
#include "engine/geospatial_query.hpp"
#include "util/typedefs.hpp" #include "util/typedefs.hpp"
#include "util/rectangle.hpp" #include "util/rectangle.hpp"
#include "util/exception.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 "mocks/mock_datafacade.hpp"
#include <boost/functional/hash.hpp>
#include <boost/test/unit_test.hpp> #include <boost/test/unit_test.hpp>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/test_case_template.hpp> #include <boost/test/test_case_template.hpp>
#include <boost/functional/hash.hpp>
#include <osrm/coordinate.hpp>
#include <cstdint> #include <cstdint>
#include <cmath> #include <cmath>
@ -44,8 +44,8 @@ using MiniStaticRTree = StaticRTree<TestData, std::vector<Coordinate>, false, 2,
// Choosen by a fair W20 dice roll (this value is completely arbitrary) // Choosen by a fair W20 dice roll (this value is completely arbitrary)
constexpr unsigned RANDOM_SEED = 42; constexpr unsigned RANDOM_SEED = 42;
static const int32_t WORLD_MIN_LAT = -90 * COORDINATE_PRECISION; static const int32_t WORLD_MIN_LAT = -85 * COORDINATE_PRECISION;
static const int32_t WORLD_MAX_LAT = 90 * 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_MIN_LON = -180 * COORDINATE_PRECISION;
static const int32_t WORLD_MAX_LON = 180 * COORDINATE_PRECISION; static const int32_t WORLD_MAX_LON = 180 * COORDINATE_PRECISION;
@ -62,18 +62,23 @@ template <typename DataT> class LinearSearchNN
{ {
std::vector<DataT> local_edges(edges); std::vector<DataT> local_edges(edges);
std::nth_element( auto projected_input = coordinate_calculation::mercator::fromWGS84(input_coordinate);
local_edges.begin(), local_edges.begin() + num_results, local_edges.end(), const auto segment_comparator = [this, &projected_input](const DataT &lhs, const DataT &rhs)
[this, &input_coordinate](const DataT &lhs, const DataT &rhs) {
{ using coordinate_calculation::mercator::fromWGS84;
double current_ratio = 0.; const auto lhs_result = coordinate_calculation::projectPointOnSegment(
Coordinate nearest; fromWGS84(coords->at(lhs.u)), fromWGS84(coords->at(lhs.v)), projected_input);
const double lhs_dist = coordinate_calculation::perpendicularDistance( const auto rhs_result = coordinate_calculation::projectPointOnSegment(
coords->at(lhs.u), coords->at(lhs.v), input_coordinate, nearest, current_ratio); fromWGS84(coords->at(rhs.u)), fromWGS84(coords->at(rhs.v)), projected_input);
const double rhs_dist = coordinate_calculation::perpendicularDistance( const auto lhs_squared_dist = coordinate_calculation::squaredEuclideanDistance(
coords->at(rhs.u), coords->at(rhs.v), input_coordinate, nearest, current_ratio); lhs_result.second, projected_input);
return lhs_dist < rhs_dist; 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); local_edges.resize(num_results);
return local_edges; return local_edges;
@ -102,8 +107,6 @@ template <unsigned NUM_NODES, unsigned NUM_EDGES> struct RandomGraphFixture
RandomGraphFixture() : coords(std::make_shared<std::vector<Coordinate>>()) RandomGraphFixture() : coords(std::make_shared<std::vector<Coordinate>>())
{ {
BOOST_TEST_MESSAGE("Constructing " << NUM_NODES << " nodes and " << NUM_EDGES << " edges.");
std::mt19937 g(RANDOM_SEED); std::mt19937 g(RANDOM_SEED);
std::uniform_int_distribution<> lat_udist(WORLD_MIN_LAT, WORLD_MAX_LAT); 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<std::vector<Coordinate>> &coords, const std::shared_ptr<std::vector<Coordinate>> &coords,
const std::vector<TestData> &edges) const std::vector<TestData> &edges)
{ {
BOOST_TEST_MESSAGE("Verify end points");
for (const auto &e : edges) for (const auto &e : edges)
{ {
const Coordinate &pu = coords->at(e.u); 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))); queries.emplace_back(FixedLongitude(lon_udist(g)), FixedLatitude(lat_udist(g)));
} }
BOOST_TEST_MESSAGE("Sampling queries");
for (const auto &q : queries) for (const auto &q : queries)
{ {
auto result_rtree = rtree.Nearest(q, 1); 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_u = result_lsnn.back().u;
auto lsnn_v = result_lsnn.back().v; auto lsnn_v = result_lsnn.back().v;
double current_ratio = 0.; Coordinate rtree_nearest;
Coordinate nearest; Coordinate lsnn_nearest;
double ratio;
const double rtree_dist = coordinate_calculation::perpendicularDistance( 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( const double lsnn_dist = coordinate_calculation::perpendicularDistance(
coords[lsnn_u], coords[lsnn_v], q, nearest, current_ratio); coords[lsnn_u], coords[lsnn_v], q, lsnn_nearest, ratio);
BOOST_CHECK_LE(std::abs(rtree_dist - lsnn_dist), std::numeric_limits<double>::epsilon());
BOOST_CHECK_CLOSE(rtree_dist, lsnn_dist, 0.0001);
} }
} }
@ -303,18 +306,16 @@ BOOST_AUTO_TEST_CASE(regression_test)
using Edge = std::pair<unsigned, unsigned>; using Edge = std::pair<unsigned, unsigned>;
GraphFixture fixture( GraphFixture fixture(
{ {
Coord{FloatLongitude{0.0}, FloatLatitude{40.0}}, // Coord{FloatLongitude{0.0}, FloatLatitude{40.0}}, //
Coord{FloatLongitude{5.0}, FloatLatitude{35.0}}, // Coord{FloatLongitude{5.0}, FloatLatitude{35.0}}, //
Coord{FloatLongitude{5.0}, Coord{FloatLongitude{5.0}, FloatLatitude{5.0}}, //
FloatLatitude{ Coord{FloatLongitude{10.0}, FloatLatitude{0.0}}, //
5.0, }}, // Coord{FloatLongitude{10.0}, FloatLatitude{20.0}}, //
Coord{FloatLongitude{10.0}, FloatLatitude{0.0}}, // Coord{FloatLongitude{5.0}, FloatLatitude{20.0}}, //
Coord{FloatLongitude{10.0}, FloatLatitude{20.0}}, // Coord{FloatLongitude{100.0}, FloatLatitude{40.0}}, //
Coord{FloatLongitude{5.0}, FloatLatitude{20.0}}, // Coord{FloatLongitude{105.0}, FloatLatitude{35.0}}, //
Coord{FloatLongitude{100.0}, FloatLatitude{40.0}}, // Coord{FloatLongitude{105.0}, FloatLatitude{5.0}}, //
Coord{FloatLongitude{105.0}, FloatLatitude{35.0}}, // Coord{FloatLongitude{110.0}, FloatLatitude{0.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)}); {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_rtree = rtree.Nearest(input, 1);
auto result_ls = lsnn.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_rtree.size() == 1);
BOOST_CHECK(result_ls.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); 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) BOOST_AUTO_TEST_CASE(bearing_tests)
{ {
using Coord = std::pair<FloatLongitude, FloatLatitude>; using Coord = std::pair<FloatLongitude, FloatLatitude>;
using Edge = std::pair<unsigned, unsigned>; using Edge = std::pair<unsigned, unsigned>;
GraphFixture fixture( GraphFixture fixture(
{ {
Coord(FloatLongitude(0.0), FloatLatitude(0.0)), Coord(FloatLongitude(0.0), FloatLatitude(0.0)),
Coord(FloatLongitude(10.0), FloatLatitude(10.0)), Coord(FloatLongitude(10.0), FloatLatitude(10.0)),
}, },
{Edge(0, 1), Edge(1, 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); auto results = query.NearestPhantomNodes(input, 5, 45, 10);
BOOST_CHECK_EQUAL(results.size(), 2); 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.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); 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); auto results = query.NearestPhantomNodesInRange(input, 11000, 45, 10);
BOOST_CHECK_EQUAL(results.size(), 2); 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.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); 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( GraphFixture fixture(
{ {
Coord(FloatLongitude(0.0), FloatLatitude(0.0)), Coord(FloatLongitude(0.0), FloatLatitude(0.0)),
Coord(FloatLongitude(1.0), FloatLatitude(1.0)), Coord(FloatLongitude(1.0), FloatLatitude(1.0)),
Coord(FloatLongitude(2.0), FloatLatitude(2.0)), Coord(FloatLongitude(2.0), FloatLatitude(2.0)),
Coord(FloatLongitude(3.0), FloatLatitude(3.0)), Coord(FloatLongitude(3.0), FloatLatitude(3.0)),
Coord(FloatLongitude(4.0), FloatLatitude(4.0)), Coord(FloatLongitude(4.0), FloatLatitude(4.0)),
}, },
{Edge(0, 1), Edge(1, 2), Edge(2, 3), Edge(3, 4)}); {Edge(0, 1), Edge(1, 2), Edge(2, 3), Edge(3, 4)});
@ -478,15 +439,15 @@ BOOST_AUTO_TEST_CASE(bbox_search_tests)
mockfacade); mockfacade);
{ {
RectangleInt2D bbox = { RectangleInt2D bbox = {FloatLongitude(0.5), FloatLongitude(1.5), FloatLatitude(0.5),
FloatLongitude(0.5), FloatLongitude(1.5), FloatLatitude(0.5), FloatLatitude(1.5)}; FloatLatitude(1.5)};
auto results = query.Search(bbox); auto results = query.Search(bbox);
BOOST_CHECK_EQUAL(results.size(), 2); BOOST_CHECK_EQUAL(results.size(), 2);
} }
{ {
RectangleInt2D bbox = { RectangleInt2D bbox = {FloatLongitude(1.5), FloatLongitude(3.5), FloatLatitude(1.5),
FloatLongitude(1.5), FloatLongitude(3.5), FloatLatitude(1.5), FloatLatitude(3.5)}; FloatLatitude(3.5)};
auto results = query.Search(bbox); auto results = query.Search(bbox);
BOOST_CHECK_EQUAL(results.size(), 3); BOOST_CHECK_EQUAL(results.size(), 3);
} }