Fix distance calculation consistency. (#6315)
Consolidate great circle distance calculations to use cheap ruler library.
This commit is contained in:
Siarhei Fedartsou
committed by
GitHub
GitHub
parent
8f0cd5cf7b
commit
aadc088084
Vendored
Vendored
Vendored
+96
-60
@@ -1,7 +1,10 @@
|
||||
#pragma once
|
||||
|
||||
#include <mapbox/geometry.hpp>
|
||||
#include <mapbox/geometry/box.hpp>
|
||||
#include <mapbox/geometry/multi_line_string.hpp>
|
||||
#include <mapbox/geometry/polygon.hpp>
|
||||
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <limits>
|
||||
@@ -19,6 +22,14 @@ using point = geometry::point<double>;
|
||||
using polygon = geometry::polygon<double>;
|
||||
|
||||
class CheapRuler {
|
||||
|
||||
// Values that define WGS84 ellipsoid model of the Earth
|
||||
static constexpr double RE = 6378.137; // equatorial radius
|
||||
static constexpr double FE = 1.0 / 298.257223563; // flattening
|
||||
|
||||
static constexpr double E2 = FE * (2 - FE);
|
||||
static constexpr double RAD = M_PI / 180.0;
|
||||
|
||||
public:
|
||||
enum Unit {
|
||||
Kilometers,
|
||||
@@ -63,68 +74,61 @@ public:
|
||||
break;
|
||||
}
|
||||
|
||||
auto cos = std::cos(latitude * M_PI / 180.);
|
||||
auto cos2 = 2. * cos * cos - 1.;
|
||||
auto cos3 = 2. * cos * cos2 - cos;
|
||||
auto cos4 = 2. * cos * cos3 - cos2;
|
||||
auto cos5 = 2. * cos * cos4 - cos3;
|
||||
// Curvature formulas from https://en.wikipedia.org/wiki/Earth_radius#Meridional
|
||||
double mul = RAD * RE * m;
|
||||
double coslat = std::cos(latitude * RAD);
|
||||
double w2 = 1 / (1 - E2 * (1 - coslat * coslat));
|
||||
double w = std::sqrt(w2);
|
||||
|
||||
// multipliers for converting longitude and latitude
|
||||
// degrees into distance (http://1.usa.gov/1Wb1bv7)
|
||||
kx = m * (111.41513 * cos - 0.09455 * cos3 + 0.00012 * cos5);
|
||||
ky = m * (111.13209 - 0.56605 * cos2 + 0.0012 * cos4);
|
||||
// multipliers for converting longitude and latitude degrees into distance
|
||||
kx = mul * w * coslat; // based on normal radius of curvature
|
||||
ky = mul * w * w2 * (1 - E2); // based on meridonal radius of curvature
|
||||
}
|
||||
|
||||
static CheapRuler fromTile(uint32_t y, uint32_t z) {
|
||||
double n = M_PI * (1. - 2. * (y + 0.5) / std::pow(2., z));
|
||||
double latitude = std::atan(0.5 * (std::exp(n) - std::exp(-n))) * 180. / M_PI;
|
||||
assert(z < 32);
|
||||
double n = M_PI * (1. - 2. * (y + 0.5) / double(uint32_t(1) << z));
|
||||
double latitude = std::atan(std::sinh(n)) / RAD;
|
||||
|
||||
return CheapRuler(latitude);
|
||||
}
|
||||
|
||||
double squareDistance(point a, point b) const {
|
||||
auto dx = longDiff(a.x, b.x) * kx;
|
||||
auto dy = (a.y - b.y) * ky;
|
||||
return dx * dx + dy * dy;
|
||||
}
|
||||
|
||||
//
|
||||
// Given two points of the form [x = longitude, y = latitude], returns the distance.
|
||||
//
|
||||
double distance(point a, point b) {
|
||||
auto dx = (a.x - b.x) * kx;
|
||||
auto dy = (a.y - b.y) * ky;
|
||||
|
||||
return std::sqrt(dx * dx + dy * dy);
|
||||
double distance(point a, point b) const {
|
||||
return std::sqrt(squareDistance(a, b));
|
||||
}
|
||||
|
||||
//
|
||||
// Returns the bearing between two points in angles.
|
||||
//
|
||||
double bearing(point a, point b) {
|
||||
auto dx = (b.x - a.x) * kx;
|
||||
double bearing(point a, point b) const {
|
||||
auto dx = longDiff(b.x, a.x) * kx;
|
||||
auto dy = (b.y - a.y) * ky;
|
||||
|
||||
if (!dx && !dy) {
|
||||
return 0.;
|
||||
}
|
||||
|
||||
auto value = std::atan2(dx, dy) * 180. / M_PI;
|
||||
|
||||
if (value > 180.) {
|
||||
value -= 360.;
|
||||
}
|
||||
|
||||
return value;
|
||||
return std::atan2(dx, dy) / RAD;
|
||||
}
|
||||
|
||||
//
|
||||
// Returns a new point given distance and bearing from the starting point.
|
||||
//
|
||||
point destination(point origin, double dist, double bearing_) {
|
||||
auto a = (90. - bearing_) * M_PI / 180.;
|
||||
point destination(point origin, double dist, double bearing_) const {
|
||||
auto a = bearing_ * RAD;
|
||||
|
||||
return offset(origin, std::cos(a) * dist, std::sin(a) * dist);
|
||||
return offset(origin, std::sin(a) * dist, std::cos(a) * dist);
|
||||
}
|
||||
|
||||
//
|
||||
// Returns a new point given easting and northing offsets from the starting point.
|
||||
//
|
||||
point offset(point origin, double dx, double dy) {
|
||||
point offset(point origin, double dx, double dy) const {
|
||||
return point(origin.x + dx / kx, origin.y + dy / ky);
|
||||
}
|
||||
|
||||
@@ -134,8 +138,8 @@ public:
|
||||
double lineDistance(const line_string& points) {
|
||||
double total = 0.;
|
||||
|
||||
for (unsigned i = 0; i < points.size() - 1; ++i) {
|
||||
total += distance(points[i], points[i + 1]);
|
||||
for (size_t i = 1; i < points.size(); ++i) {
|
||||
total += distance(points[i - 1], points[i]);
|
||||
}
|
||||
|
||||
return total;
|
||||
@@ -145,14 +149,15 @@ public:
|
||||
// Given a polygon (an array of rings, where each ring is an array of points),
|
||||
// returns the area.
|
||||
//
|
||||
double area(polygon poly) {
|
||||
double area(polygon poly) const {
|
||||
double sum = 0.;
|
||||
|
||||
for (unsigned i = 0; i < poly.size(); ++i) {
|
||||
auto& ring = poly[i];
|
||||
|
||||
for (unsigned j = 0, len = ring.size(), k = len - 1; j < len; k = j++) {
|
||||
sum += (ring[j].x - ring[k].x) * (ring[j].y + ring[k].y) * (i ? -1. : 1.);
|
||||
sum += longDiff(ring[j].x, ring[k].x) *
|
||||
(ring[j].y + ring[k].y) * (i ? -1. : 1.);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -162,9 +167,13 @@ public:
|
||||
//
|
||||
// Returns the point at a specified distance along the line.
|
||||
//
|
||||
point along(const line_string& line, double dist) {
|
||||
point along(const line_string& line, double dist) const {
|
||||
double sum = 0.;
|
||||
|
||||
if (line.empty()) {
|
||||
return {};
|
||||
}
|
||||
|
||||
if (dist <= 0.) {
|
||||
return line[0];
|
||||
}
|
||||
@@ -184,25 +193,52 @@ public:
|
||||
return line[line.size() - 1];
|
||||
}
|
||||
|
||||
//
|
||||
// Returns the distance from a point `p` to a line segment `a` to `b`.
|
||||
//
|
||||
double pointToSegmentDistance(const point& p, const point& a, const point& b) const {
|
||||
auto t = 0.0;
|
||||
auto x = a.x;
|
||||
auto y = a.y;
|
||||
auto dx = longDiff(b.x, x) * kx;
|
||||
auto dy = (b.y - y) * ky;
|
||||
|
||||
if (dx != 0.0 || dy != 0.0) {
|
||||
t = (longDiff(p.x, x) * kx * dx + (p.y - y) * ky * dy) / (dx * dx + dy * dy);
|
||||
if (t > 1.0) {
|
||||
x = b.x;
|
||||
y = b.y;
|
||||
} else if (t > 0.0) {
|
||||
x += (dx / kx) * t;
|
||||
y += (dy / ky) * t;
|
||||
}
|
||||
}
|
||||
return distance(p, { x, y });
|
||||
}
|
||||
|
||||
//
|
||||
// Returns a tuple of the form <point, index, t> where point is closest point on the line
|
||||
// from the given point, index is the start index of the segment with the closest point,
|
||||
// and t is a parameter from 0 to 1 that indicates where the closest point is on that segment.
|
||||
//
|
||||
std::tuple<point, unsigned, double> pointOnLine(const line_string& line, point p) {
|
||||
std::tuple<point, unsigned, double> pointOnLine(const line_string& line, point p) const {
|
||||
double minDist = std::numeric_limits<double>::infinity();
|
||||
double minX = 0., minY = 0., minI = 0., minT = 0.;
|
||||
|
||||
if (line.empty()) {
|
||||
return std::make_tuple(point(), 0., 0.);
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < line.size() - 1; ++i) {
|
||||
auto t = 0.;
|
||||
auto x = line[i].x;
|
||||
auto y = line[i].y;
|
||||
auto dx = (line[i + 1].x - x) * kx;
|
||||
auto dx = longDiff(line[i + 1].x, x) * kx;
|
||||
auto dy = (line[i + 1].y - y) * ky;
|
||||
|
||||
if (dx != 0. || dy != 0.) {
|
||||
t = ((p.x - x) * kx * dx + (p.y - y) * ky * dy) / (dx * dx + dy * dy);
|
||||
|
||||
t = (longDiff(p.x, x) * kx * dx +
|
||||
(p.y - y) * ky * dy) / (dx * dx + dy * dy);
|
||||
if (t > 1) {
|
||||
x = line[i + 1].x;
|
||||
y = line[i + 1].y;
|
||||
@@ -213,10 +249,7 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
dx = (p.x - x) * kx;
|
||||
dy = (p.y - y) * ky;
|
||||
|
||||
auto sqDist = dx * dx + dy * dy;
|
||||
auto sqDist = squareDistance(p, {x, y});
|
||||
|
||||
if (sqDist < minDist) {
|
||||
minDist = sqDist;
|
||||
@@ -235,7 +268,7 @@ public:
|
||||
// Returns a part of the given line between the start and the stop points (or their closest
|
||||
// points on the line).
|
||||
//
|
||||
line_string lineSlice(point start, point stop, const line_string& line) {
|
||||
line_string lineSlice(point start, point stop, const line_string& line) const {
|
||||
auto getPoint = [](auto tuple) { return std::get<0>(tuple); };
|
||||
auto getIndex = [](auto tuple) { return std::get<1>(tuple); };
|
||||
auto getT = [](auto tuple) { return std::get<2>(tuple); };
|
||||
@@ -273,13 +306,13 @@ public:
|
||||
// Returns a part of the given line between the start and the stop points
|
||||
// indicated by distance along the line.
|
||||
//
|
||||
line_string lineSliceAlong(double start, double stop, const line_string& line) {
|
||||
line_string lineSliceAlong(double start, double stop, const line_string& line) const {
|
||||
double sum = 0.;
|
||||
line_string slice;
|
||||
|
||||
for (unsigned i = 0; i < line.size() - 1; ++i) {
|
||||
auto p0 = line[i];
|
||||
auto p1 = line[i + 1];
|
||||
for (size_t i = 1; i < line.size(); ++i) {
|
||||
auto p0 = line[i - 1];
|
||||
auto p1 = line[i];
|
||||
auto d = distance(p0, p1);
|
||||
|
||||
sum += d;
|
||||
@@ -305,7 +338,7 @@ public:
|
||||
// Given a point, returns a bounding box object ([w, s, e, n])
|
||||
// created from the given point buffered by a given distance.
|
||||
//
|
||||
box bufferPoint(point p, double buffer) {
|
||||
box bufferPoint(point p, double buffer) const {
|
||||
auto v = buffer / ky;
|
||||
auto h = buffer / kx;
|
||||
|
||||
@@ -318,7 +351,7 @@ public:
|
||||
//
|
||||
// Given a bounding box, returns the box buffered by a given distance.
|
||||
//
|
||||
box bufferBBox(box bbox, double buffer) {
|
||||
box bufferBBox(box bbox, double buffer) const {
|
||||
auto v = buffer / ky;
|
||||
auto h = buffer / kx;
|
||||
|
||||
@@ -331,15 +364,15 @@ public:
|
||||
//
|
||||
// Returns true if the given point is inside in the given bounding box, otherwise false.
|
||||
//
|
||||
bool insideBBox(point p, box bbox) {
|
||||
return p.x >= bbox.min.x &&
|
||||
p.x <= bbox.max.x &&
|
||||
p.y >= bbox.min.y &&
|
||||
p.y <= bbox.max.y;
|
||||
static bool insideBBox(point p, box bbox) {
|
||||
return p.y >= bbox.min.y &&
|
||||
p.y <= bbox.max.y &&
|
||||
longDiff(p.x, bbox.min.x) >= 0 &&
|
||||
longDiff(p.x, bbox.max.x) <= 0;
|
||||
}
|
||||
|
||||
static point interpolate(point a, point b, double t) {
|
||||
double dx = b.x - a.x;
|
||||
double dx = longDiff(b.x, a.x);
|
||||
double dy = b.y - a.y;
|
||||
|
||||
return point(a.x + dx * t, a.y + dy * t);
|
||||
@@ -348,6 +381,9 @@ public:
|
||||
private:
|
||||
double ky;
|
||||
double kx;
|
||||
static double longDiff(double a, double b) {
|
||||
return std::remainder(a - b, 360);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace cheap_ruler
|
||||
+82
-6
@@ -1,5 +1,6 @@
|
||||
#include <mapbox/cheap_ruler.hpp>
|
||||
#include <gtest/gtest.h>
|
||||
#include <random>
|
||||
|
||||
#include "fixtures/lines.hpp"
|
||||
#include "fixtures/turf.hpp"
|
||||
@@ -60,6 +61,13 @@ TEST_F(CheapRulerTest, destination) {
|
||||
}
|
||||
|
||||
TEST_F(CheapRulerTest, lineDistance) {
|
||||
{
|
||||
cr::line_string emptyLine {};
|
||||
auto expected = 0.0;
|
||||
auto actual = ruler.lineDistance(emptyLine);
|
||||
assertErr(expected, actual, 0.0);
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < lines.size(); ++i) {
|
||||
auto expected = turf_lineDistance[i];
|
||||
auto actual = ruler.lineDistance(lines[i]);
|
||||
@@ -88,6 +96,16 @@ TEST_F(CheapRulerTest, area) {
|
||||
}
|
||||
|
||||
TEST_F(CheapRulerTest, along) {
|
||||
{
|
||||
cr::point emptyPoint {};
|
||||
cr::line_string emptyLine {};
|
||||
auto expected = emptyPoint;
|
||||
auto actual = ruler.along(emptyLine, 0.0);
|
||||
|
||||
assertErr(expected.x, actual.x, 0.0);
|
||||
assertErr(expected.y, actual.y, 0.0);
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < lines.size(); ++i) {
|
||||
auto expected = turf_along[i];
|
||||
auto actual = ruler.along(lines[i], turf_along_dist[i]);
|
||||
@@ -110,14 +128,23 @@ TEST_F(CheapRulerTest, pointOnLine) {
|
||||
cr::line_string line = {{ -77.031669, 38.878605 }, { -77.029609, 38.881946 }};
|
||||
auto result = ruler.pointOnLine(line, { -77.034076, 38.882017 });
|
||||
|
||||
ASSERT_EQ(std::get<0>(result), cr::point(-77.03052697027461, 38.880457194811896)); // point
|
||||
assertErr(std::get<0>(result).x, -77.03052689033436, 1e-6);
|
||||
assertErr(std::get<0>(result).y, 38.880457324462576, 1e-6);
|
||||
ASSERT_EQ(std::get<1>(result), 0u); // index
|
||||
ASSERT_EQ(std::get<2>(result), 0.5543833618360235); // t
|
||||
assertErr(std::get<2>(result), 0.5544221677861756, 1e-6); // t
|
||||
|
||||
ASSERT_EQ(std::get<2>(ruler.pointOnLine(line, { -80., 38. })), 0.) << "t is not less than 0";
|
||||
ASSERT_EQ(std::get<2>(ruler.pointOnLine(line, { -75., 38. })), 1.) << "t is not bigger than 1";
|
||||
}
|
||||
|
||||
TEST_F(CheapRulerTest, pointToSegmentDistance) {
|
||||
cr::point p{ -77.034076, 38.882017 };
|
||||
cr::point p0{ -77.031669, 38.878605 };
|
||||
cr::point p1{ -77.029609, 38.881946 };
|
||||
const auto distance = ruler.pointToSegmentDistance(p, p0, p1);
|
||||
assertErr(0.37461484020420416, distance, 1e-6);
|
||||
}
|
||||
|
||||
TEST_F(CheapRulerTest, lineSlice) {
|
||||
for (unsigned i = 0; i < lines.size(); ++i) {
|
||||
auto line = lines[i];
|
||||
@@ -127,11 +154,19 @@ TEST_F(CheapRulerTest, lineSlice) {
|
||||
auto expected = turf_lineSlice[i];
|
||||
auto actual = ruler.lineDistance(ruler.lineSlice(start, stop, line));
|
||||
|
||||
assertErr(expected, actual, 1e-5);
|
||||
/// @todo Should update turf_lineSlice and revert maxError back.
|
||||
assertErr(expected, actual, 1e-4);
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(CheapRulerTest, lineSliceAlong) {
|
||||
{
|
||||
cr::line_string emptyLine {};
|
||||
auto expected = ruler.lineDistance(emptyLine);
|
||||
auto actual = ruler.lineDistance(ruler.lineSliceAlong(0.0, 0.0, emptyLine));
|
||||
assertErr(expected, actual, 0.0);
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < lines.size(); ++i) {
|
||||
if (i == 46) {
|
||||
// skip due to Turf bug https://github.com/Turfjs/turf/issues/351
|
||||
@@ -143,7 +178,8 @@ TEST_F(CheapRulerTest, lineSliceAlong) {
|
||||
auto expected = turf_lineSlice[i];
|
||||
auto actual = ruler.lineDistance(ruler.lineSliceAlong(dist * 0.3, dist * 0.7, line));
|
||||
|
||||
assertErr(expected, actual, 1e-5);
|
||||
/// @todo Should update turf_lineSlice and revert maxError back.
|
||||
assertErr(expected, actual, 1e-4);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -154,7 +190,7 @@ TEST_F(CheapRulerTest, lineSliceReverse) {
|
||||
auto stop = ruler.along(line, dist * 0.3);
|
||||
auto actual = ruler.lineDistance(ruler.lineSlice(start, stop, line));
|
||||
|
||||
ASSERT_EQ(actual, 0.018676802802910702);
|
||||
assertErr(0.018676476689649835, actual, 1e-6);
|
||||
}
|
||||
|
||||
TEST_F(CheapRulerTest, bufferPoint) {
|
||||
@@ -173,7 +209,10 @@ TEST_F(CheapRulerTest, bufferBBox) {
|
||||
cr::box bbox({ 30, 38 }, { 40, 39 });
|
||||
cr::box bbox2 = ruler.bufferBBox(bbox, 1);
|
||||
|
||||
ASSERT_EQ(bbox2, cr::box({ 29.989319515875376, 37.99098271225711 }, { 40.01068048412462, 39.00901728774289 }));
|
||||
assertErr(bbox2.min.x, 29.989319515875376, 1e-6);
|
||||
assertErr(bbox2.min.y, 37.99098271225711, 1e-6);
|
||||
assertErr(bbox2.max.x, 40.01068048412462, 1e-6);
|
||||
assertErr(bbox2.max.y, 39.00901728774289, 1e-6);
|
||||
}
|
||||
|
||||
TEST_F(CheapRulerTest, insideBBox) {
|
||||
@@ -193,6 +232,43 @@ TEST_F(CheapRulerTest, fromTile) {
|
||||
assertErr(ruler1.distance(p1, p2), ruler2.distance(p1, p2), 2e-5);
|
||||
}
|
||||
|
||||
TEST_F(CheapRulerTest, longitudeWrap) {
|
||||
std::random_device rd;
|
||||
std::mt19937 gen(rd());
|
||||
std::bernoulli_distribution d(0.5); // true with prob 0.5
|
||||
|
||||
auto r = cr::CheapRuler(50.5);
|
||||
cr::polygon poly(1);
|
||||
auto& ring = poly[0];
|
||||
cr::line_string line;
|
||||
cr::point origin(0, 50.5); // Greenwich
|
||||
auto rad = 1000.0;
|
||||
// construct a regular dodecagon
|
||||
for (int i = -180; i <= 180; i += 30) {
|
||||
auto p = r.destination(origin, rad, i);
|
||||
// shift randomly east/west to the international date line
|
||||
p.x += d(gen) ? 180 : -180;
|
||||
ring.push_back(p);
|
||||
line.push_back(p);
|
||||
}
|
||||
auto p = r.lineDistance(line);
|
||||
auto a = r.area(poly);
|
||||
// cheap_ruler does planar calculations, so the perimeter and area of a
|
||||
// planar regular dodecagon with circumradius rad are used in these checks.
|
||||
// For the record, the results for rad = 1000 km are:
|
||||
// perimeter area
|
||||
// planar 6211.657082 3000000
|
||||
// WGS84 6187.959236 2996317.6328
|
||||
// error 0.38% 0.12%
|
||||
assertErr(12 * rad / sqrt(2 + sqrt(3.0)), p, 1e-12);
|
||||
assertErr(3 * rad * rad, a, 1e-12);
|
||||
for (int j = 1; j < (int)line.size(); ++j) {
|
||||
auto azi = r.bearing(line[j-1], line[j]);
|
||||
// offset expect and actual by 1 to make err criterion absolute
|
||||
assertErr(1, std::remainder(270 - 15 + 30*j - azi, 360) + 1, 1e-12);
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
Reference in New Issue
Block a user