89 lines
2.1 KiB
C++
89 lines
2.1 KiB
C++
#pragma once
|
|
|
|
#include <cassert>
|
|
#include <cmath>
|
|
#include <cstdint>
|
|
#include <limits>
|
|
#include <tuple>
|
|
#include <utility>
|
|
|
|
namespace mapbox {
|
|
|
|
namespace geometry {
|
|
template <typename T>
|
|
struct point
|
|
{
|
|
using coordinate_type = T;
|
|
|
|
constexpr point()
|
|
: x(), y()
|
|
{}
|
|
constexpr point(T x_, T y_)
|
|
: x(x_), y(y_)
|
|
{}
|
|
|
|
T x;
|
|
T y;
|
|
};
|
|
}
|
|
|
|
namespace cheap_ruler {
|
|
|
|
using point = geometry::point<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:
|
|
explicit CheapRuler(double latitude) {
|
|
// Curvature formulas from https://en.wikipedia.org/wiki/Earth_radius#Meridional
|
|
double mul = RAD * RE * 1000;
|
|
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
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
private:
|
|
double ky;
|
|
double kx;
|
|
static double longDiff(double a, double b) {
|
|
return std::remainder(a - b, 360);
|
|
}
|
|
};
|
|
|
|
} // namespace cheap_ruler
|
|
} // namespace mapbox
|