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
@@ -0,0 +1,390 @@
|
||||
#pragma once
|
||||
|
||||
#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>
|
||||
#include <tuple>
|
||||
#include <utility>
|
||||
|
||||
namespace mapbox {
|
||||
namespace cheap_ruler {
|
||||
|
||||
using box = geometry::box<double>;
|
||||
using line_string = geometry::line_string<double>;
|
||||
using linear_ring = geometry::linear_ring<double>;
|
||||
using multi_line_string = geometry::multi_line_string<double>;
|
||||
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,
|
||||
Miles,
|
||||
NauticalMiles,
|
||||
Meters,
|
||||
Metres = Meters,
|
||||
Yards,
|
||||
Feet,
|
||||
Inches
|
||||
};
|
||||
|
||||
//
|
||||
// A collection of very fast approximations to common geodesic measurements. Useful
|
||||
// for performance-sensitive code that measures things on a city scale. Point coordinates
|
||||
// are in the [x = longitude, y = latitude] form.
|
||||
//
|
||||
explicit CheapRuler(double latitude, Unit unit = Kilometers) {
|
||||
double m = 0.;
|
||||
|
||||
switch (unit) {
|
||||
case Kilometers:
|
||||
m = 1.;
|
||||
break;
|
||||
case Miles:
|
||||
m = 1000. / 1609.344;
|
||||
break;
|
||||
case NauticalMiles:
|
||||
m = 1000. / 1852.;
|
||||
break;
|
||||
case Meters:
|
||||
m = 1000.;
|
||||
break;
|
||||
case Yards:
|
||||
m = 1000. / 0.9144;
|
||||
break;
|
||||
case Feet:
|
||||
m = 1000. / 0.3048;
|
||||
break;
|
||||
case Inches:
|
||||
m = 1000. / 0.0254;
|
||||
break;
|
||||
}
|
||||
|
||||
// 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
|
||||
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) {
|
||||
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) const {
|
||||
return std::sqrt(squareDistance(a, b));
|
||||
}
|
||||
|
||||
//
|
||||
// Returns the bearing between two points in angles.
|
||||
//
|
||||
double bearing(point a, point b) const {
|
||||
auto dx = longDiff(b.x, a.x) * kx;
|
||||
auto dy = (b.y - a.y) * ky;
|
||||
|
||||
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_) const {
|
||||
auto a = bearing_ * RAD;
|
||||
|
||||
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) const {
|
||||
return point(origin.x + dx / kx, origin.y + dy / ky);
|
||||
}
|
||||
|
||||
//
|
||||
// Given a line (an array of points), returns the total line distance.
|
||||
//
|
||||
double lineDistance(const line_string& points) {
|
||||
double total = 0.;
|
||||
|
||||
for (size_t i = 1; i < points.size(); ++i) {
|
||||
total += distance(points[i - 1], points[i]);
|
||||
}
|
||||
|
||||
return total;
|
||||
}
|
||||
|
||||
//
|
||||
// Given a polygon (an array of rings, where each ring is an array of points),
|
||||
// returns the area.
|
||||
//
|
||||
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 += longDiff(ring[j].x, ring[k].x) *
|
||||
(ring[j].y + ring[k].y) * (i ? -1. : 1.);
|
||||
}
|
||||
}
|
||||
|
||||
return (std::abs(sum) / 2.) * kx * ky;
|
||||
}
|
||||
|
||||
//
|
||||
// Returns the point at a specified distance along the line.
|
||||
//
|
||||
point along(const line_string& line, double dist) const {
|
||||
double sum = 0.;
|
||||
|
||||
if (line.empty()) {
|
||||
return {};
|
||||
}
|
||||
|
||||
if (dist <= 0.) {
|
||||
return line[0];
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < line.size() - 1; ++i) {
|
||||
auto p0 = line[i];
|
||||
auto p1 = line[i + 1];
|
||||
auto d = distance(p0, p1);
|
||||
|
||||
sum += d;
|
||||
|
||||
if (sum > dist) {
|
||||
return interpolate(p0, p1, (dist - (sum - d)) / d);
|
||||
}
|
||||
}
|
||||
|
||||
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) 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 = longDiff(line[i + 1].x, x) * kx;
|
||||
auto dy = (line[i + 1].y - y) * ky;
|
||||
|
||||
if (dx != 0. || dy != 0.) {
|
||||
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;
|
||||
|
||||
} else if (t > 0) {
|
||||
x += (dx / kx) * t;
|
||||
y += (dy / ky) * t;
|
||||
}
|
||||
}
|
||||
|
||||
auto sqDist = squareDistance(p, {x, y});
|
||||
|
||||
if (sqDist < minDist) {
|
||||
minDist = sqDist;
|
||||
minX = x;
|
||||
minY = y;
|
||||
minI = i;
|
||||
minT = t;
|
||||
}
|
||||
}
|
||||
|
||||
return std::make_tuple(
|
||||
point(minX, minY), minI, ::fmax(0., ::fmin(1., minT)));
|
||||
}
|
||||
|
||||
//
|
||||
// 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) 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); };
|
||||
|
||||
auto p1 = pointOnLine(line, start);
|
||||
auto p2 = pointOnLine(line, stop);
|
||||
|
||||
if (getIndex(p1) > getIndex(p2) || (getIndex(p1) == getIndex(p2) && getT(p1) > getT(p2))) {
|
||||
auto tmp = p1;
|
||||
p1 = p2;
|
||||
p2 = tmp;
|
||||
}
|
||||
|
||||
line_string slice = { getPoint(p1) };
|
||||
|
||||
auto l = getIndex(p1) + 1;
|
||||
auto r = getIndex(p2);
|
||||
|
||||
if (line[l] != slice[0] && l <= r) {
|
||||
slice.push_back(line[l]);
|
||||
}
|
||||
|
||||
for (unsigned i = l + 1; i <= r; ++i) {
|
||||
slice.push_back(line[i]);
|
||||
}
|
||||
|
||||
if (line[r] != getPoint(p2)) {
|
||||
slice.push_back(getPoint(p2));
|
||||
}
|
||||
|
||||
return slice;
|
||||
};
|
||||
|
||||
//
|
||||
// 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) const {
|
||||
double sum = 0.;
|
||||
line_string slice;
|
||||
|
||||
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;
|
||||
|
||||
if (sum > start && slice.size() == 0) {
|
||||
slice.push_back(interpolate(p0, p1, (start - (sum - d)) / d));
|
||||
}
|
||||
|
||||
if (sum >= stop) {
|
||||
slice.push_back(interpolate(p0, p1, (stop - (sum - d)) / d));
|
||||
return slice;
|
||||
}
|
||||
|
||||
if (sum > start) {
|
||||
slice.push_back(p1);
|
||||
}
|
||||
}
|
||||
|
||||
return slice;
|
||||
};
|
||||
|
||||
//
|
||||
// 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) const {
|
||||
auto v = buffer / ky;
|
||||
auto h = buffer / kx;
|
||||
|
||||
return box(
|
||||
point(p.x - h, p.y - v),
|
||||
point(p.x + h, p.y + v)
|
||||
);
|
||||
}
|
||||
|
||||
//
|
||||
// Given a bounding box, returns the box buffered by a given distance.
|
||||
//
|
||||
box bufferBBox(box bbox, double buffer) const {
|
||||
auto v = buffer / ky;
|
||||
auto h = buffer / kx;
|
||||
|
||||
return box(
|
||||
point(bbox.min.x - h, bbox.min.y - v),
|
||||
point(bbox.max.x + h, bbox.max.y + v)
|
||||
);
|
||||
}
|
||||
|
||||
//
|
||||
// Returns true if the given point is inside in the given bounding box, otherwise false.
|
||||
//
|
||||
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 = longDiff(b.x, a.x);
|
||||
double dy = b.y - a.y;
|
||||
|
||||
return point(a.x + dx * t, a.y + dy * t);
|
||||
}
|
||||
|
||||
private:
|
||||
double ky;
|
||||
double kx;
|
||||
static double longDiff(double a, double b) {
|
||||
return std::remainder(a - b, 360);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace cheap_ruler
|
||||
} // namespace mapbox
|
||||
Reference in New Issue
Block a user