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.
This commit is contained in:
Patrick Niklaus
@@ -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<double>(toFloating(coordinate_1.lon));
|
||||
const double y1 = mercator::latToY(toFloating(coordinate_1.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;
|
||||
const double dx = static_cast<double>(lhs.lon - rhs.lon);
|
||||
const double dy = static_cast<double>(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<double, FloatCoordinate> 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<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,
|
||||
nearest_location, ratio);
|
||||
if (squared_length < std::numeric_limits<double>::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<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());
|
||||
|
||||
// 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<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());
|
||||
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<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)
|
||||
@@ -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<double>(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<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;
|
||||
|
||||
@@ -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<double>(clamp(util::FloatLongitude(minx))) * DEGREE_TO_PX;
|
||||
miny = latToY(clamp(util::FloatLatitude(miny))) * DEGREE_TO_PX;
|
||||
|
||||
Reference in New Issue
Block a user