henry/SimCore
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ADD: new track message, Entity class and Position class

Browse Source
This commit is contained in:
Henry Winkel
2022-12-20 17:20:35 +01:00
parent 469ecfb099
commit 98ebb563a8
2114 changed files with 482360 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
libs/geographiclib/src/.gitignore vendored Normal file
View File

@@ -0,0 +1,9 @@
libGeographic.a
.deps
.libs
Makefile.in
Makefile
TAGS
CMakeFiles
cmake_install.cmake
*.pro.user*

21
libs/geographiclib/src/Accumulator.cpp Normal file
View File

@@ -0,0 +1,21 @@
/**
* \file Accumulator.cpp
* \brief Implementation for GeographicLib::Accumulator class
*
* Copyright (c) Charles Karney (2013-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/Accumulator.hpp>
namespace GeographicLib {
/// \cond SKIP
// Need to instantiate Accumulator to get the code into the shared library.
template class GEOGRAPHICLIB_EXPORT Accumulator<Math::real>;
/// \endcond
} // namespace GeographicLib

556
libs/geographiclib/src/AlbersEqualArea.cpp Normal file
View File

@@ -0,0 +1,556 @@
/**
* \file AlbersEqualArea.cpp
* \brief Implementation for GeographicLib::AlbersEqualArea class
*
* Copyright (c) Charles Karney (2010-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/AlbersEqualArea.hpp>
#if defined(_MSC_VER)
// Squelch warnings about constant conditional and enum-float expressions
# pragma warning (disable: 4127 5055)
#endif
namespace GeographicLib {
using namespace std;
AlbersEqualArea::AlbersEqualArea(real a, real f, real stdlat, real k0)
: eps_(numeric_limits<real>::epsilon())
, epsx_(Math::sq(eps_))
, epsx2_(Math::sq(epsx_))
, tol_(sqrt(eps_))
, tol0_(tol_ * sqrt(sqrt(eps_)))
, _a(a)
, _f(f)
, _fm(1 - _f)
, _e2(_f * (2 - _f))
, _e(sqrt(fabs(_e2)))
, _e2m(1 - _e2)
, _qZ(1 + _e2m * atanhee(real(1)))
, _qx(_qZ / ( 2 * _e2m ))
{
if (!(isfinite(_a) && _a > 0))
throw GeographicErr("Equatorial radius is not positive");
if (!(isfinite(_f) && _f < 1))
throw GeographicErr("Polar semi-axis is not positive");
if (!(isfinite(k0) && k0 > 0))
throw GeographicErr("Scale is not positive");
if (!(fabs(stdlat) <= Math::qd))
throw GeographicErr("Standard latitude not in [-" + to_string(Math::qd)
+ "d, " + to_string(Math::qd) + "d]");
real sphi, cphi;
Math::sincosd(stdlat, sphi, cphi);
Init(sphi, cphi, sphi, cphi, k0);
}
AlbersEqualArea::AlbersEqualArea(real a, real f, real stdlat1, real stdlat2,
real k1)
: eps_(numeric_limits<real>::epsilon())
, epsx_(Math::sq(eps_))
, epsx2_(Math::sq(epsx_))
, tol_(sqrt(eps_))
, tol0_(tol_ * sqrt(sqrt(eps_)))
, _a(a)
, _f(f)
, _fm(1 - _f)
, _e2(_f * (2 - _f))
, _e(sqrt(fabs(_e2)))
, _e2m(1 - _e2)
, _qZ(1 + _e2m * atanhee(real(1)))
, _qx(_qZ / ( 2 * _e2m ))
{
if (!(isfinite(_a) && _a > 0))
throw GeographicErr("Equatorial radius is not positive");
if (!(isfinite(_f) && _f < 1))
throw GeographicErr("Polar semi-axis is not positive");
if (!(isfinite(k1) && k1 > 0))
throw GeographicErr("Scale is not positive");
if (!(fabs(stdlat1) <= Math::qd))
throw GeographicErr("Standard latitude 1 not in [-"
+ to_string(Math::qd) + "d, "
+ to_string(Math::qd) + "d]");
if (!(fabs(stdlat2) <= Math::qd))
throw GeographicErr("Standard latitude 2 not in [-"
+ to_string(Math::qd) + "d, "
+ to_string(Math::qd) + "d]");
real sphi1, cphi1, sphi2, cphi2;
Math::sincosd(stdlat1, sphi1, cphi1);
Math::sincosd(stdlat2, sphi2, cphi2);
Init(sphi1, cphi1, sphi2, cphi2, k1);
}
AlbersEqualArea::AlbersEqualArea(real a, real f,
real sinlat1, real coslat1,
real sinlat2, real coslat2,
real k1)
: eps_(numeric_limits<real>::epsilon())
, epsx_(Math::sq(eps_))
, epsx2_(Math::sq(epsx_))
, tol_(sqrt(eps_))
, tol0_(tol_ * sqrt(sqrt(eps_)))
, _a(a)
, _f(f)
, _fm(1 - _f)
, _e2(_f * (2 - _f))
, _e(sqrt(fabs(_e2)))
, _e2m(1 - _e2)
, _qZ(1 + _e2m * atanhee(real(1)))
, _qx(_qZ / ( 2 * _e2m ))
{
if (!(isfinite(_a) && _a > 0))
throw GeographicErr("Equatorial radius is not positive");
if (!(isfinite(_f) && _f < 1))
throw GeographicErr("Polar semi-axis is not positive");
if (!(isfinite(k1) && k1 > 0))
throw GeographicErr("Scale is not positive");
if (signbit(coslat1))
throw GeographicErr("Standard latitude 1 not in [-"
+ to_string(Math::qd) + "d, "
+ to_string(Math::qd) + "d]");
if (signbit(coslat2))
throw GeographicErr("Standard latitude 2 not in [-"
+ to_string(Math::qd) + "d, "
+ to_string(Math::qd) + "d]");
if (!(fabs(sinlat1) <= 1 && coslat1 <= 1) || (coslat1 == 0 && sinlat1 == 0))
throw GeographicErr("Bad sine/cosine of standard latitude 1");
if (!(fabs(sinlat2) <= 1 && coslat2 <= 1) || (coslat2 == 0 && sinlat2 == 0))
throw GeographicErr("Bad sine/cosine of standard latitude 2");
if (coslat1 == 0 && coslat2 == 0 && sinlat1 * sinlat2 <= 0)
throw GeographicErr
("Standard latitudes cannot be opposite poles");
Init(sinlat1, coslat1, sinlat2, coslat2, k1);
}
void AlbersEqualArea::Init(real sphi1, real cphi1,
real sphi2, real cphi2, real k1) {
{
real r;
r = hypot(sphi1, cphi1);
sphi1 /= r; cphi1 /= r;
r = hypot(sphi2, cphi2);
sphi2 /= r; cphi2 /= r;
}
bool polar = (cphi1 == 0);
cphi1 = fmax(epsx_, cphi1); // Avoid singularities at poles
cphi2 = fmax(epsx_, cphi2);
// Determine hemisphere of tangent latitude
_sign = sphi1 + sphi2 >= 0 ? 1 : -1;
// Internally work with tangent latitude positive
sphi1 *= _sign; sphi2 *= _sign;
if (sphi1 > sphi2) {
swap(sphi1, sphi2); swap(cphi1, cphi2); // Make phi1 < phi2
}
real
tphi1 = sphi1/cphi1, tphi2 = sphi2/cphi2;
// q = (1-e^2)*(sphi/(1-e^2*sphi^2) - atanhee(sphi))
// qZ = q(pi/2) = (1 + (1-e^2)*atanhee(1))
// atanhee(x) = atanh(e*x)/e
// q = sxi * qZ
// dq/dphi = 2*(1-e^2)*cphi/(1-e^2*sphi^2)^2
//
// n = (m1^2-m2^2)/(q2-q1) -> sin(phi0) for phi1, phi2 -> phi0
// C = m1^2 + n*q1 = (m1^2*q2-m2^2*q1)/(q2-q1)
// let
// rho(pi/2)/rho(-pi/2) = (1-s)/(1+s)
// s = n*qZ/C
// = qZ * (m1^2-m2^2)/(m1^2*q2-m2^2*q1)
// = qZ * (scbet2^2 - scbet1^2)/(scbet2^2*q2 - scbet1^2*q1)
// = (scbet2^2 - scbet1^2)/(scbet2^2*sxi2 - scbet1^2*sxi1)
// = (tbet2^2 - tbet1^2)/(scbet2^2*sxi2 - scbet1^2*sxi1)
// 1-s = -((1-sxi2)*scbet2^2 - (1-sxi1)*scbet1^2)/
// (scbet2^2*sxi2 - scbet1^2*sxi1)
//
// Define phi0 to give same value of s, i.e.,
// s = sphi0 * qZ / (m0^2 + sphi0*q0)
// = sphi0 * scbet0^2 / (1/qZ + sphi0 * scbet0^2 * sxi0)
real tphi0, C;
if (polar || tphi1 == tphi2) {
tphi0 = tphi2;
C = 1; // ignored
} else {
real
tbet1 = _fm * tphi1, scbet12 = 1 + Math::sq(tbet1),
tbet2 = _fm * tphi2, scbet22 = 1 + Math::sq(tbet2),
txi1 = txif(tphi1), cxi1 = 1/hyp(txi1), sxi1 = txi1 * cxi1,
txi2 = txif(tphi2), cxi2 = 1/hyp(txi2), sxi2 = txi2 * cxi2,
dtbet2 = _fm * (tbet1 + tbet2),
es1 = 1 - _e2 * Math::sq(sphi1), es2 = 1 - _e2 * Math::sq(sphi2),
/*
dsxi = ( (_e2 * sq(sphi2 + sphi1) + es2 + es1) / (2 * es2 * es1) +
Datanhee(sphi2, sphi1) ) * Dsn(tphi2, tphi1, sphi2, sphi1) /
( 2 * _qx ),
*/
dsxi = ( (1 + _e2 * sphi1 * sphi2) / (es2 * es1) +
Datanhee(sphi2, sphi1) ) * Dsn(tphi2, tphi1, sphi2, sphi1) /
( 2 * _qx ),
den = (sxi2 + sxi1) * dtbet2 + (scbet22 + scbet12) * dsxi,
// s = (sq(tbet2) - sq(tbet1)) / (scbet22*sxi2 - scbet12*sxi1)
s = 2 * dtbet2 / den,
// 1-s = -(sq(scbet2)*(1-sxi2) - sq(scbet1)*(1-sxi1)) /
// (scbet22*sxi2 - scbet12*sxi1)
// Write
// sq(scbet)*(1-sxi) = sq(scbet)*(1-sphi) * (1-sxi)/(1-sphi)
sm1 = -Dsn(tphi2, tphi1, sphi2, sphi1) *
( -( ((sphi2 <= 0 ? (1 - sxi2) / (1 - sphi2) :
Math::sq(cxi2/cphi2) * (1 + sphi2) / (1 + sxi2)) +
(sphi1 <= 0 ? (1 - sxi1) / (1 - sphi1) :
Math::sq(cxi1/cphi1) * (1 + sphi1) / (1 + sxi1))) ) *
(1 + _e2 * (sphi1 + sphi2 + sphi1 * sphi2)) /
(1 + (sphi1 + sphi2 + sphi1 * sphi2)) +
(scbet22 * (sphi2 <= 0 ? 1 - sphi2 :
Math::sq(cphi2) / ( 1 + sphi2)) +
scbet12 * (sphi1 <= 0 ? 1 - sphi1 : Math::sq(cphi1) / ( 1 + sphi1)))
* (_e2 * (1 + sphi1 + sphi2 + _e2 * sphi1 * sphi2)/(es1 * es2)
+_e2m * DDatanhee(sphi1, sphi2) ) / _qZ ) / den;
// C = (scbet22*sxi2 - scbet12*sxi1) / (scbet22 * scbet12 * (sx2 - sx1))
C = den / (2 * scbet12 * scbet22 * dsxi);
tphi0 = (tphi2 + tphi1)/2;
real stol = tol0_ * fmax(real(1), fabs(tphi0));
for (int i = 0; i < 2*numit0_ || GEOGRAPHICLIB_PANIC; ++i) {
// Solve (scbet0^2 * sphi0) / (1/qZ + scbet0^2 * sphi0 * sxi0) = s
// for tphi0 by Newton's method on
// v(tphi0) = (scbet0^2 * sphi0) - s * (1/qZ + scbet0^2 * sphi0 * sxi0)
// = 0
// Alt:
// (scbet0^2 * sphi0) / (1/qZ - scbet0^2 * sphi0 * (1-sxi0))
// = s / (1-s)
// w(tphi0) = (1-s) * (scbet0^2 * sphi0)
// - s * (1/qZ - scbet0^2 * sphi0 * (1-sxi0))
// = (1-s) * (scbet0^2 * sphi0)
// - S/qZ * (1 - scbet0^2 * sphi0 * (qZ-q0))
// Now
// qZ-q0 = (1+e2*sphi0)*(1-sphi0)/(1-e2*sphi0^2) +
// (1-e2)*atanhee((1-sphi0)/(1-e2*sphi0))
// In limit sphi0 -> 1, qZ-q0 -> 2*(1-sphi0)/(1-e2), so wrte
// qZ-q0 = 2*(1-sphi0)/(1-e2) + A + B
// A = (1-sphi0)*( (1+e2*sphi0)/(1-e2*sphi0^2) - (1+e2)/(1-e2) )
// = -e2 *(1-sphi0)^2 * (2+(1+e2)*sphi0) / ((1-e2)*(1-e2*sphi0^2))
// B = (1-e2)*atanhee((1-sphi0)/(1-e2*sphi0)) - (1-sphi0)
// = (1-sphi0)*(1-e2)/(1-e2*sphi0)*
// ((atanhee(x)/x-1) - e2*(1-sphi0)/(1-e2))
// x = (1-sphi0)/(1-e2*sphi0), atanhee(x)/x = atanh(e*x)/(e*x)
//
// 1 - scbet0^2 * sphi0 * (qZ-q0)
// = 1 - scbet0^2 * sphi0 * (2*(1-sphi0)/(1-e2) + A + B)
// = D - scbet0^2 * sphi0 * (A + B)
// D = 1 - scbet0^2 * sphi0 * 2*(1-sphi0)/(1-e2)
// = (1-sphi0)*(1-e2*(1+2*sphi0*(1+sphi0)))/((1-e2)*(1+sphi0))
// dD/dsphi0 = -2*(1-e2*sphi0^2*(2*sphi0+3))/((1-e2)*(1+sphi0)^2)
// d(A+B)/dsphi0 = 2*(1-sphi0^2)*e2*(2-e2*(1+sphi0^2))/
// ((1-e2)*(1-e2*sphi0^2)^2)
real
scphi02 = 1 + Math::sq(tphi0), scphi0 = sqrt(scphi02),
// sphi0m = 1-sin(phi0) = 1/( sec(phi0) * (tan(phi0) + sec(phi0)) )
sphi0 = tphi0 / scphi0, sphi0m = 1/(scphi0 * (tphi0 + scphi0)),
// scbet0^2 * sphi0
g = (1 + Math::sq( _fm * tphi0 )) * sphi0,
// dg/dsphi0 = dg/dtphi0 * scphi0^3
dg = _e2m * scphi02 * (1 + 2 * Math::sq(tphi0)) + _e2,
D = sphi0m * (1 - _e2*(1 + 2*sphi0*(1+sphi0))) / (_e2m * (1+sphi0)),
// dD/dsphi0
dD = -2 * (1 - _e2*Math::sq(sphi0) * (2*sphi0+3)) /
(_e2m * Math::sq(1+sphi0)),
A = -_e2 * Math::sq(sphi0m) * (2+(1+_e2)*sphi0) /
(_e2m*(1-_e2*Math::sq(sphi0))),
B = (sphi0m * _e2m / (1 - _e2*sphi0) *
(atanhxm1(_e2 *
Math::sq(sphi0m / (1-_e2*sphi0))) - _e2*sphi0m/_e2m)),
// d(A+B)/dsphi0
dAB = (2 * _e2 * (2 - _e2 * (1 + Math::sq(sphi0))) /
(_e2m * Math::sq(1 - _e2*Math::sq(sphi0)) * scphi02)),
u = sm1 * g - s/_qZ * ( D - g * (A + B) ),
// du/dsphi0
du = sm1 * dg - s/_qZ * (dD - dg * (A + B) - g * dAB),
dtu = -u/du * (scphi0 * scphi02);
tphi0 += dtu;
if (!(fabs(dtu) >= stol))
break;
}
}
_txi0 = txif(tphi0); _scxi0 = hyp(_txi0); _sxi0 = _txi0 / _scxi0;
_n0 = tphi0/hyp(tphi0);
_m02 = 1 / (1 + Math::sq(_fm * tphi0));
_nrho0 = polar ? 0 : _a * sqrt(_m02);
_k0 = sqrt(tphi1 == tphi2 ? 1 : C / (_m02 + _n0 * _qZ * _sxi0)) * k1;
_k2 = Math::sq(_k0);
_lat0 = _sign * atan(tphi0)/Math::degree();
}
const AlbersEqualArea& AlbersEqualArea::CylindricalEqualArea() {
static const AlbersEqualArea
cylindricalequalarea(Constants::WGS84_a(), Constants::WGS84_f(),
real(0), real(1), real(0), real(1), real(1));
return cylindricalequalarea;
}
const AlbersEqualArea& AlbersEqualArea::AzimuthalEqualAreaNorth() {
static const AlbersEqualArea
azimuthalequalareanorth(Constants::WGS84_a(), Constants::WGS84_f(),
real(1), real(0), real(1), real(0), real(1));
return azimuthalequalareanorth;
}
const AlbersEqualArea& AlbersEqualArea::AzimuthalEqualAreaSouth() {
static const AlbersEqualArea
azimuthalequalareasouth(Constants::WGS84_a(), Constants::WGS84_f(),
real(-1), real(0), real(-1), real(0), real(1));
return azimuthalequalareasouth;
}
Math::real AlbersEqualArea::txif(real tphi) const {
// sxi = ( sphi/(1-e2*sphi^2) + atanhee(sphi) ) /
// ( 1/(1-e2) + atanhee(1) )
//
// txi = ( sphi/(1-e2*sphi^2) + atanhee(sphi) ) /
// sqrt( ( (1+e2*sphi)*(1-sphi)/( (1-e2*sphi^2) * (1-e2) ) +
// atanhee((1-sphi)/(1-e2*sphi)) ) *
// ( (1-e2*sphi)*(1+sphi)/( (1-e2*sphi^2) * (1-e2) ) +
// atanhee((1+sphi)/(1+e2*sphi)) ) )
// = ( tphi/(1-e2*sphi^2) + atanhee(sphi, e2)/cphi ) /
// sqrt(
// ( (1+e2*sphi)/( (1-e2*sphi^2) * (1-e2) ) + Datanhee(1, sphi) ) *
// ( (1-e2*sphi)/( (1-e2*sphi^2) * (1-e2) ) + Datanhee(1, -sphi) ) )
//
// This function maintains odd parity
real
cphi = 1 / sqrt(1 + Math::sq(tphi)),
sphi = tphi * cphi,
es1 = _e2 * sphi,
es2m1 = 1 - es1 * sphi, // 1 - e2 * sphi^2
es2m1a = _e2m * es2m1; // (1 - e2 * sphi^2) * (1 - e2)
return ( tphi / es2m1 + atanhee(sphi) / cphi ) /
sqrt( ( (1 + es1) / es2m1a + Datanhee(1, sphi) ) *
( (1 - es1) / es2m1a + Datanhee(1, -sphi) ) );
}
Math::real AlbersEqualArea::tphif(real txi) const {
real
tphi = txi,
stol = tol_ * fmax(real(1), fabs(txi));
// CHECK: min iterations = 1, max iterations = 2; mean = 1.99
for (int i = 0; i < numit_ || GEOGRAPHICLIB_PANIC; ++i) {
// dtxi/dtphi = (scxi/scphi)^3 * 2*(1-e^2)/(qZ*(1-e^2*sphi^2)^2)
real
txia = txif(tphi),
tphi2 = Math::sq(tphi),
scphi2 = 1 + tphi2,
scterm = scphi2/(1 + Math::sq(txia)),
dtphi = (txi - txia) * scterm * sqrt(scterm) *
_qx * Math::sq(1 - _e2 * tphi2 / scphi2);
tphi += dtphi;
if (!(fabs(dtphi) >= stol))
break;
}
return tphi;
}
// return atanh(sqrt(x))/sqrt(x) - 1 = x/3 + x^2/5 + x^3/7 + ...
// typical x < e^2 = 2*f
Math::real AlbersEqualArea::atanhxm1(real x) {
real s = 0;
if (fabs(x) < real(0.5)) {
static const real lg2eps_ = -log2(numeric_limits<real>::epsilon() / 2);
int e;
frexp(x, &e);
e = -e;
// x = [0.5,1) * 2^(-e)
// estimate n s.t. x^n/(2*n+1) < x/3 * epsilon/2
// a stronger condition is x^(n-1) < epsilon/2
// taking log2 of both sides, a stronger condition is
// (n-1)*(-e) < -lg2eps or (n-1)*e > lg2eps or n > ceiling(lg2eps/e)+1
int n = x == 0 ? 1 : int(ceil(lg2eps_ / e)) + 1;
while (n--) // iterating from n-1 down to 0
s = x * s + (n ? 1 : 0)/Math::real(2*n + 1);
} else {
real xs = sqrt(fabs(x));
s = (x > 0 ? atanh(xs) : atan(xs)) / xs - 1;
}
return s;
}
// return (Datanhee(1,y) - Datanhee(1,x))/(y-x)
Math::real AlbersEqualArea::DDatanhee(real x, real y) const {
// This function is called with x = sphi1, y = sphi2, phi1 <= phi2, sphi2
// >= 0, abs(sphi1) <= phi2. However for safety's sake we enforce x <= y.
if (y < x) swap(x, y); // ensure that x <= y
real q1 = fabs(_e2),
q2 = fabs(2 * _e / _e2m * (1 - x));
return
x <= 0 || !(fmin(q1, q2) < real(0.75)) ? DDatanhee0(x, y) :
(q1 < q2 ? DDatanhee1(x, y) : DDatanhee2(x, y));
}
// Rearrange difference so that 1 - x is in the denominator, then do a
// straight divided difference.
Math::real AlbersEqualArea::DDatanhee0(real x, real y) const {
return (Datanhee(1, y) - Datanhee(x, y))/(1 - x);
}
// The expansion for e2 small
Math::real AlbersEqualArea::DDatanhee1(real x, real y) const {
// The series in e2 is
// sum( c[l] * e2^l, l, 1, N)
// where
// c[l] = sum( x^i * y^j; i >= 0, j >= 0, i+j < 2*l) / (2*l + 1)
// = ( (x-y) - (1-y) * x^(2*l+1) + (1-x) * y^(2*l+1) ) /
// ( (2*l+1) * (x-y) * (1-y) * (1-x) )
// For x = y = 1, c[l] = l
//
// In the limit x,y -> 1,
//
// DDatanhee -> e2/(1-e2)^2 = sum(l * e2^l, l, 1, inf)
//
// Use if e2 is sufficiently small.
real s = 0;
real z = 1, k = 1, t = 0, c = 0, en = 1;
while (true) {
t = y * t + z; c += t; z *= x;
t = y * t + z; c += t; z *= x;
k += 2; en *= _e2;
// Here en[l] = e2^l, k[l] = 2*l + 1,
// c[l] = sum( x^i * y^j; i >= 0, j >= 0, i+j < 2*l) / (2*l + 1)
// Taylor expansion is
// s = sum( c[l] * e2^l, l, 1, N)
real ds = en * c / k;
s += ds;
if (!(fabs(ds) > fabs(s) * eps_/2))
break; // Iterate until the added term is sufficiently small
}
return s;
}
// The expansion for x (and y) close to 1
Math::real AlbersEqualArea::DDatanhee2(real x, real y) const {
// If x and y are both close to 1, expand in Taylor series in dx = 1-x and
// dy = 1-y:
//
// DDatanhee = sum(C_m * (dx^(m+1) - dy^(m+1)) / (dx - dy), m, 0, inf)
//
// where
//
// C_m = sum( (m+2)!! / (m+2-2*k)!! *
// ((m+1)/2)! / ((m+1)/2-k)! /
// (k! * (2*k-1)!!) *
// e2^((m+1)/2+k),
// k, 0, (m+1)/2) * (-1)^m / ((m+2) * (1-e2)^(m+2))
// for m odd, and
//
// C_m = sum( 2 * (m+1)!! / (m+1-2*k)!! *
// (m/2+1)! / (m/2-k)! /
// (k! * (2*k+1)!!) *
// e2^(m/2+1+k),
// k, 0, m/2)) * (-1)^m / ((m+2) * (1-e2)^(m+2))
// for m even.
//
// Here i!! is the double factorial extended to negative i with
// i!! = (i+2)!!/(i+2).
//
// Note that
// (dx^(m+1) - dy^(m+1)) / (dx - dy) =
// dx^m + dx^(m-1)*dy ... + dx*dy^(m-1) + dy^m
//
// Leading (m = 0) term is e2 / (1 - e2)^2
//
// Magnitude of mth term relative to the leading term scales as
//
// 2*(2*e/(1-e2)*dx)^m
//
// So use series if (2*e/(1-e2)*dx) is sufficiently small
real s, dx = 1 - x, dy = 1 - y, xy = 1, yy = 1, ee = _e2 / Math::sq(_e2m);
s = ee;
for (int m = 1; ; ++m) {
real c = m + 2, t = c;
yy *= dy; // yy = dy^m
xy = dx * xy + yy;
// Now xy = dx^m + dx^(m-1)*dy ... + dx*dy^(m-1) + dy^m
// = (dx^(m+1) - dy^(m+1)) / (dx - dy)
// max value = (m+1) * max(dx,dy)^m
ee /= -_e2m;
if (m % 2 == 0) ee *= _e2;
// Now ee = (-1)^m * e2^(floor(m/2)+1) / (1-e2)^(m+2)
int kmax = (m+1)/2;
for (int k = kmax - 1; k >= 0; --k) {
// max coeff is less than 2^(m+1)
c *= (k + 1) * (2 * (k + m - 2*kmax) + 3);
c /= (kmax - k) * (2 * (kmax - k) + 1);
// Horner sum for inner _e2 series
t = _e2 * t + c;
}
// Straight sum for outer m series
real ds = t * ee * xy / (m + 2);
s = s + ds;
if (!(fabs(ds) > fabs(s) * eps_/2))
break; // Iterate until the added term is sufficiently small
}
return s;
}
void AlbersEqualArea::Forward(real lon0, real lat, real lon,
real& x, real& y, real& gamma, real& k) const {
lon = Math::AngDiff(lon0, lon);
lat *= _sign;
real sphi, cphi;
Math::sincosd(Math::LatFix(lat) * _sign, sphi, cphi);
cphi = fmax(epsx_, cphi);
real
lam = lon * Math::degree(),
tphi = sphi/cphi, txi = txif(tphi), sxi = txi/hyp(txi),
dq = _qZ * Dsn(txi, _txi0, sxi, _sxi0) * (txi - _txi0),
drho = - _a * dq / (sqrt(_m02 - _n0 * dq) + _nrho0 / _a),
theta = _k2 * _n0 * lam, stheta = sin(theta), ctheta = cos(theta),
t = _nrho0 + _n0 * drho;
x = t * (_n0 != 0 ? stheta / _n0 : _k2 * lam) / _k0;
y = (_nrho0 *
(_n0 != 0 ?
(ctheta < 0 ? 1 - ctheta : Math::sq(stheta)/(1 + ctheta)) / _n0 :
0)
- drho * ctheta) / _k0;
k = _k0 * (t != 0 ? t * hyp(_fm * tphi) / _a : 1);
y *= _sign;
gamma = _sign * theta / Math::degree();
}
void AlbersEqualArea::Reverse(real lon0, real x, real y,
real& lat, real& lon,
real& gamma, real& k) const {
y *= _sign;
real
nx = _k0 * _n0 * x, ny = _k0 * _n0 * y, y1 = _nrho0 - ny,
den = hypot(nx, y1) + _nrho0, // 0 implies origin with polar aspect
drho = den != 0 ? (_k0*x*nx - 2*_k0*y*_nrho0 + _k0*y*ny) / den : 0,
// dsxia = scxi0 * dsxi
dsxia = - _scxi0 * (2 * _nrho0 + _n0 * drho) * drho /
(Math::sq(_a) * _qZ),
txi = (_txi0 + dsxia) / sqrt(fmax(1 - dsxia * (2*_txi0 + dsxia), epsx2_)),
tphi = tphif(txi),
theta = atan2(nx, y1),
lam = _n0 != 0 ? theta / (_k2 * _n0) : x / (y1 * _k0);
gamma = _sign * theta / Math::degree();
lat = Math::atand(_sign * tphi);
lon = lam / Math::degree();
lon = Math::AngNormalize(lon + Math::AngNormalize(lon0));
k = _k0 * (den != 0 ? (_nrho0 + _n0 * drho) * hyp(_fm * tphi) / _a : 1);
}
void AlbersEqualArea::SetScale(real lat, real k) {
if (!(isfinite(k) && k > 0))
throw GeographicErr("Scale is not positive");
if (!(fabs(lat) < Math::qd))
throw GeographicErr("Latitude for SetScale not in (-"
+ to_string(Math::qd) + "d, "
+ to_string(Math::qd) + "d)");
real x, y, gamma, kold;
Forward(0, lat, 0, x, y, gamma, kold);
k /= kold;
_k0 *= k;
_k2 = Math::sq(_k0);
}
} // namespace GeographicLib

41
libs/geographiclib/src/AzimuthalEquidistant.cpp Normal file
View File

@@ -0,0 +1,41 @@
/**
* \file AzimuthalEquidistant.cpp
* \brief Implementation for GeographicLib::AzimuthalEquidistant class
*
* Copyright (c) Charles Karney (2009-2020) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/AzimuthalEquidistant.hpp>
namespace GeographicLib {
using namespace std;
AzimuthalEquidistant::AzimuthalEquidistant(const Geodesic& earth)
: eps_(real(0.01) * sqrt(numeric_limits<real>::min()))
, _earth(earth) {}
void AzimuthalEquidistant::Forward(real lat0, real lon0, real lat, real lon,
real& x, real& y,
real& azi, real& rk) const {
real sig, s, azi0, m;
sig = _earth.Inverse(lat0, lon0, lat, lon, s, azi0, azi, m);
Math::sincosd(azi0, x, y);
x *= s; y *= s;
rk = !(sig <= eps_) ? m / s : 1;
}
void AzimuthalEquidistant::Reverse(real lat0, real lon0, real x, real y,
real& lat, real& lon,
real& azi, real& rk) const {
real
azi0 = Math::atan2d(x, y),
s = hypot(x, y);
real sig, m;
sig = _earth.Direct(lat0, lon0, azi0, s, lat, lon, azi, m);
rk = !(sig <= eps_) ? m / s : 1;
}
} // namespace GeographicLib

189
libs/geographiclib/src/CMakeLists.txt Normal file
View File

@@ -0,0 +1,189 @@
# Build the library...
# Include all the .cpp files in the library.
set (SOURCES
Accumulator.cpp
AlbersEqualArea.cpp
AzimuthalEquidistant.cpp
CassiniSoldner.cpp
CircularEngine.cpp
DMS.cpp
DST.cpp
Ellipsoid.cpp
EllipticFunction.cpp
GARS.cpp
GeoCoords.cpp
Geocentric.cpp
Geodesic.cpp
GeodesicExact.cpp
GeodesicLine.cpp
GeodesicLineExact.cpp
Geohash.cpp
Geoid.cpp
Georef.cpp
Gnomonic.cpp
GravityCircle.cpp
GravityModel.cpp
LambertConformalConic.cpp
LocalCartesian.cpp
MGRS.cpp
MagneticCircle.cpp
MagneticModel.cpp
Math.cpp
NormalGravity.cpp
OSGB.cpp
PolarStereographic.cpp
PolygonArea.cpp
Rhumb.cpp
SphericalEngine.cpp
TransverseMercator.cpp
TransverseMercatorExact.cpp
UTMUPS.cpp
Utility.cpp
)
set (HEADERS
kissfft.hh
${PROJECT_BINARY_DIR}/include/GeographicLib/Config.h
../include/GeographicLib/Accumulator.hpp
../include/GeographicLib/AlbersEqualArea.hpp
../include/GeographicLib/AzimuthalEquidistant.hpp
../include/GeographicLib/CassiniSoldner.hpp
../include/GeographicLib/CircularEngine.hpp
../include/GeographicLib/Constants.hpp
../include/GeographicLib/DMS.hpp
../include/GeographicLib/Ellipsoid.hpp
../include/GeographicLib/EllipticFunction.hpp
../include/GeographicLib/GARS.hpp
../include/GeographicLib/GeoCoords.hpp
../include/GeographicLib/Geocentric.hpp
../include/GeographicLib/Geodesic.hpp
../include/GeographicLib/GeodesicExact.hpp
../include/GeographicLib/GeodesicLine.hpp
../include/GeographicLib/GeodesicLineExact.hpp
../include/GeographicLib/Geohash.hpp
../include/GeographicLib/Geoid.hpp
../include/GeographicLib/Georef.hpp
../include/GeographicLib/Gnomonic.hpp
../include/GeographicLib/GravityCircle.hpp
../include/GeographicLib/GravityModel.hpp
../include/GeographicLib/LambertConformalConic.hpp
../include/GeographicLib/LocalCartesian.hpp
../include/GeographicLib/MGRS.hpp
../include/GeographicLib/MagneticCircle.hpp
../include/GeographicLib/MagneticModel.hpp
../include/GeographicLib/Math.hpp
../include/GeographicLib/NearestNeighbor.hpp
../include/GeographicLib/NormalGravity.hpp
../include/GeographicLib/OSGB.hpp
../include/GeographicLib/PolarStereographic.hpp
../include/GeographicLib/PolygonArea.hpp
../include/GeographicLib/Rhumb.hpp
../include/GeographicLib/SphericalEngine.hpp
../include/GeographicLib/SphericalHarmonic.hpp
../include/GeographicLib/SphericalHarmonic1.hpp
../include/GeographicLib/SphericalHarmonic2.hpp
../include/GeographicLib/TransverseMercator.hpp
../include/GeographicLib/TransverseMercatorExact.hpp
../include/GeographicLib/UTMUPS.hpp
../include/GeographicLib/Utility.hpp
)
# Define the library and specify whether it is shared or not.
if (GEOGRAPHICLIB_SHARED_LIB)
add_library (${PROJECT_SHARED_LIBRARIES} SHARED ${SOURCES} ${HEADERS})
add_library (${PROJECT_NAME}::${PROJECT_SHARED_LIBRARIES}
ALIAS ${PROJECT_SHARED_LIBRARIES})
endif ()
if (GEOGRAPHICLIB_STATIC_LIB)
add_library (${PROJECT_STATIC_LIBRARIES} STATIC ${SOURCES} ${HEADERS})
add_library (${PROJECT_NAME}::${PROJECT_STATIC_LIBRARIES}
ALIAS ${PROJECT_STATIC_LIBRARIES})
endif ()
add_library (${PROJECT_INTERFACE_LIBRARIES} INTERFACE)
add_library (${PROJECT_NAME}::${PROJECT_INTERFACE_LIBRARIES}
ALIAS ${PROJECT_INTERFACE_LIBRARIES})
target_link_libraries (${PROJECT_INTERFACE_LIBRARIES}
INTERFACE ${PROJECT_LIBRARIES})
# Set the version number on the library
if (MSVC)
if (GEOGRAPHICLIB_SHARED_LIB)
set_target_properties (${PROJECT_SHARED_LIBRARIES} PROPERTIES
VERSION "${LIBVERSION_BUILD}" OUTPUT_NAME ${LIBNAME}
IMPORT_SUFFIX -i.lib)
target_compile_definitions (${PROJECT_SHARED_LIBRARIES}
PUBLIC GEOGRAPHICLIB_SHARED_LIB=1)
endif ()
if (GEOGRAPHICLIB_STATIC_LIB)
set_target_properties (${PROJECT_STATIC_LIBRARIES} PROPERTIES
VERSION "${LIBVERSION_BUILD}" OUTPUT_NAME ${LIBNAME})
target_compile_definitions (${PROJECT_STATIC_LIBRARIES}
PUBLIC GEOGRAPHICLIB_SHARED_LIB=0)
endif ()
else ()
set_target_properties (
${PROJECT_SHARED_LIBRARIES} ${PROJECT_STATIC_LIBRARIES} PROPERTIES
VERSION "${LIBVERSION_BUILD}" SOVERSION "${LIBVERSION_API}"
OUTPUT_NAME ${LIBNAME})
if (APPLE AND GEOGRAPHICLIB_PRECISION EQUAL 5)
if (GEOGRAPHICLIB_SHARED_LIB)
target_link_libraries (${PROJECT_SHARED_LIBRARIES} ${HIGHPREC_LIBRARIES})
endif ()
if (GEOGRAPHICLIB_STATIC_LIB)
target_link_libraries (${PROJECT_STATIC_LIBRARIES} ${HIGHPREC_LIBRARIES})
endif ()
endif ()
endif ()
if (GEOGRAPHICLIB_SHARED_LIB)
target_include_directories (${PROJECT_SHARED_LIBRARIES} PUBLIC
$<BUILD_INTERFACE:${PROJECT_BINARY_DIR}/include>
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:${INCDIR}>)
endif ()
if (GEOGRAPHICLIB_STATIC_LIB)
target_include_directories (${PROJECT_STATIC_LIBRARIES} PUBLIC
$<BUILD_INTERFACE:${PROJECT_BINARY_DIR}/include>
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:${INCDIR}>)
endif ()
# Specify where the library is installed, adding it to the export targets
if (LIBDIR)
install (TARGETS ${PROJECT_ALL_LIBRARIES}
EXPORT targets
# A potentially useful option. However it's only supported in recent
# versions of cmake (2.8.12 and later?). So comment out for now.
# INCLUDES DESTINATION include
RUNTIME DESTINATION ${DLLDIR}
LIBRARY DESTINATION ${LIBDIR}
ARCHIVE DESTINATION ${LIBDIR})
endif ()
if (MSVC AND PACKAGE_DEBUG_LIBS AND LIBDIR)
if (GEOGRAPHICLIB_SHARED_LIB)
install (FILES
"${PROJECT_BINARY_DIR}/lib/Debug/${LIBNAME}${CMAKE_DEBUG_POSTFIX}-i.lib"
DESTINATION ${LIBDIR} CONFIGURATIONS Release)
install (PROGRAMS
"${PROJECT_BINARY_DIR}/bin/Debug/${LIBNAME}${CMAKE_DEBUG_POSTFIX}.dll"
DESTINATION ${DLLDIR} CONFIGURATIONS Release)
endif ()
if (GEOGRAPHICLIB_STATIC_LIB)
install (FILES
"${PROJECT_BINARY_DIR}/lib/Debug/${LIBNAME}${CMAKE_DEBUG_POSTFIX}.lib"
DESTINATION ${LIBDIR} CONFIGURATIONS Release)
endif ()
endif ()
if (MSVC AND GEOGRAPHICLIB_SHARED_LIB)
install (FILES $<TARGET_PDB_FILE:${PROJECT_SHARED_LIBRARIES}>
DESTINATION bin OPTIONAL)
endif ()
# Put the library into a folder in the IDE
set_target_properties (
${PROJECT_SHARED_LIBRARIES} ${PROJECT_STATIC_LIBRARIES}
PROPERTIES FOLDER library)

94
libs/geographiclib/src/CassiniSoldner.cpp Normal file
View File

@@ -0,0 +1,94 @@
/**
* \file CassiniSoldner.cpp
* \brief Implementation for GeographicLib::CassiniSoldner class
*
* Copyright (c) Charles Karney (2009-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/CassiniSoldner.hpp>
#if defined(_MSC_VER)
// Squelch warnings about enum-float expressions
# pragma warning (disable: 5055)
#endif
namespace GeographicLib {
using namespace std;
CassiniSoldner::CassiniSoldner(const Geodesic& earth)
: _earth(earth) {}
CassiniSoldner::CassiniSoldner(real lat0, real lon0, const Geodesic& earth)
: _earth(earth)
{ Reset(lat0, lon0); }
void CassiniSoldner::Reset(real lat0, real lon0) {
_meridian = _earth.Line(lat0, lon0, real(0),
Geodesic::LATITUDE | Geodesic::LONGITUDE |
Geodesic::DISTANCE | Geodesic::DISTANCE_IN |
Geodesic::AZIMUTH);
real f = _earth.Flattening();
Math::sincosd(LatitudeOrigin(), _sbet0, _cbet0);
_sbet0 *= (1 - f);
Math::norm(_sbet0, _cbet0);
}
void CassiniSoldner::Forward(real lat, real lon, real& x, real& y,
real& azi, real& rk) const {
if (!Init())
return;
real dlon = Math::AngDiff(LongitudeOrigin(), lon);
real sig12, s12, azi1, azi2;
sig12 = _earth.Inverse(lat, -fabs(dlon), lat, fabs(dlon), s12, azi1, azi2);
sig12 *= real(0.5);
s12 *= real(0.5);
if (s12 == 0) {
real da = Math::AngDiff(azi1, azi2)/2;
if (fabs(dlon) <= Math::qd) {
azi1 = Math::qd - da;
azi2 = Math::qd + da;
} else {
azi1 = -Math::qd - da;
azi2 = -Math::qd + da;
}
}
if (signbit(dlon)) {
azi2 = azi1;
s12 = -s12;
sig12 = -sig12;
}
x = s12;
azi = Math::AngNormalize(azi2);
GeodesicLine perp(_earth.Line(lat, dlon, azi, Geodesic::GEODESICSCALE));
real t;
perp.GenPosition(true, -sig12,
Geodesic::GEODESICSCALE,
t, t, t, t, t, t, rk, t);
real salp0, calp0;
Math::sincosd(perp.EquatorialAzimuth(), salp0, calp0);
real
sbet1 = lat >=0 ? calp0 : -calp0,
cbet1 = fabs(dlon) <= Math::qd ? fabs(salp0) : -fabs(salp0),
sbet01 = sbet1 * _cbet0 - cbet1 * _sbet0,
cbet01 = cbet1 * _cbet0 + sbet1 * _sbet0,
sig01 = atan2(sbet01, cbet01) / Math::degree();
_meridian.GenPosition(true, sig01,
Geodesic::DISTANCE,
t, t, t, y, t, t, t, t);
}
void CassiniSoldner::Reverse(real x, real y, real& lat, real& lon,
real& azi, real& rk) const {
if (!Init())
return;
real lat1, lon1;
real azi0, t;
_meridian.Position(y, lat1, lon1, azi0);
_earth.Direct(lat1, lon1, azi0 + Math::qd, x, lat, lon, azi, rk, t);
}
} // namespace GeographicLib

96
libs/geographiclib/src/CircularEngine.cpp Normal file
View File

@@ -0,0 +1,96 @@
/**
* \file CircularEngine.cpp
* \brief Implementation for GeographicLib::CircularEngine class
*
* Copyright (c) Charles Karney (2011) <charles@karney.com> and licensed under
* the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/CircularEngine.hpp>
namespace GeographicLib {
using namespace std;
Math::real CircularEngine::Value(bool gradp, real sl, real cl,
real& gradx, real& grady, real& gradz) const
{
gradp = _gradp && gradp;
const vector<real>& root( SphericalEngine::sqrttable() );
// Initialize outer sum
real vc = 0, vc2 = 0, vs = 0, vs2 = 0; // v [N + 1], v [N + 2]
// vr, vt, vl and similar w variable accumulate the sums for the
// derivatives wrt r, theta, and lambda, respectively.
real vrc = 0, vrc2 = 0, vrs = 0, vrs2 = 0; // vr[N + 1], vr[N + 2]
real vtc = 0, vtc2 = 0, vts = 0, vts2 = 0; // vt[N + 1], vt[N + 2]
real vlc = 0, vlc2 = 0, vls = 0, vls2 = 0; // vl[N + 1], vl[N + 2]
for (int m = _mM; m >= 0; --m) { // m = M .. 0
// Now Sc[m] = wc, Ss[m] = ws
// Sc'[m] = wtc, Ss'[m] = wtc
if (m) {
real v, A, B; // alpha[m], beta[m + 1]
switch (_norm) {
case FULL:
v = root[2] * root[2 * m + 3] / root[m + 1];
A = cl * v * _uq;
B = - v * root[2 * m + 5] / (root[8] * root[m + 2]) * _uq2;
break;
case SCHMIDT:
v = root[2] * root[2 * m + 1] / root[m + 1];
A = cl * v * _uq;
B = - v * root[2 * m + 3] / (root[8] * root[m + 2]) * _uq2;
break;
default:
A = B = 0;
}
v = A * vc + B * vc2 + _wc[m] ; vc2 = vc ; vc = v;
v = A * vs + B * vs2 + _ws[m] ; vs2 = vs ; vs = v;
if (gradp) {
v = A * vrc + B * vrc2 + _wrc[m]; vrc2 = vrc; vrc = v;
v = A * vrs + B * vrs2 + _wrs[m]; vrs2 = vrs; vrs = v;
v = A * vtc + B * vtc2 + _wtc[m]; vtc2 = vtc; vtc = v;
v = A * vts + B * vts2 + _wts[m]; vts2 = vts; vts = v;
v = A * vlc + B * vlc2 + m*_ws[m]; vlc2 = vlc; vlc = v;
v = A * vls + B * vls2 - m*_wc[m]; vls2 = vls; vls = v;
}
} else {
real A, B, qs;
switch (_norm) {
case FULL:
A = root[3] * _uq; // F[1]/(q*cl) or F[1]/(q*sl)
B = - root[15]/2 * _uq2; // beta[1]/q
break;
case SCHMIDT:
A = _uq;
B = - root[3]/2 * _uq2;
break;
default:
A = B = 0;
}
qs = _q / SphericalEngine::scale();
vc = qs * (_wc[m] + A * (cl * vc + sl * vs ) + B * vc2);
if (gradp) {
qs /= _r;
// The components of the gradient in circular coordinates are
// r: dV/dr
// theta: 1/r * dV/dtheta
// lambda: 1/(r*u) * dV/dlambda
vrc = - qs * (_wrc[m] + A * (cl * vrc + sl * vrs) + B * vrc2);
vtc = qs * (_wtc[m] + A * (cl * vtc + sl * vts) + B * vtc2);
vlc = qs / _u * ( A * (cl * vlc + sl * vls) + B * vlc2);
}
}
}
if (gradp) {
// Rotate into cartesian (geocentric) coordinates
gradx = cl * (_u * vrc + _t * vtc) - sl * vlc;
grady = sl * (_u * vrc + _t * vtc) + cl * vlc;
gradz = _t * vrc - _u * vtc ;
}
return vc;
}
} // namespace GeographicLib

515
libs/geographiclib/src/DMS.cpp Normal file
View File

@@ -0,0 +1,515 @@
/**
* \file DMS.cpp
* \brief Implementation for GeographicLib::DMS class
*
* Copyright (c) Charles Karney (2008-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/DMS.hpp>
#include <GeographicLib/Utility.hpp>
#if defined(_MSC_VER)
// Squelch warnings about constant conditional and enum-float expressions
# pragma warning (disable: 4127 5055)
#endif
namespace GeographicLib {
using namespace std;
const char* const DMS::hemispheres_ = "SNWE";
const char* const DMS::signs_ = "-+";
const char* const DMS::digits_ = "0123456789";
const char* const DMS::dmsindicators_ = "D'\":";
const char* const DMS::components_[] = {"degrees", "minutes", "seconds"};
// Replace all occurrences of pat by c. If c is NULL remove pat.
void DMS::replace(std::string& s, const std::string& pat, char c) {
string::size_type p = 0;
int count = c ? 1 : 0;
while (true) {
p = s.find(pat, p);
if (p == string::npos)
break;
s.replace(p, pat.length(), count, c);
}
}
Math::real DMS::Decode(const std::string& dms, flag& ind) {
// Here's a table of the allowed characters
// S unicode dec UTF-8 descripton
// DEGREE
// d U+0064 100 64 d
// D U+0044 68 44 D
// ° U+00b0 176 c2 b0 degree symbol
// º U+00ba 186 c2 ba alt symbol
// ⁰ U+2070 8304 e2 81 b0 sup zero
// ˚ U+02da 730 cb 9a ring above
// ∘ U+2218 8728 e2 88 98 compose function
// * U+002a 42 2a GRiD symbol for degrees
// MINUTES
// ' U+0027 39 27 apostrophe
// ` U+0060 96 60 grave accent
// ′ U+2032 8242 e2 80 b2 prime
// ‵ U+2035 8245 e2 80 b5 back prime
// ´ U+00b4 180 c2 b4 acute accent
// ‘ U+2018 8216 e2 80 98 left single quote (also ext ASCII 0x91)
// ’ U+2019 8217 e2 80 99 right single quote (also ext ASCII 0x92)
// ‛ U+201b 8219 e2 80 9b reversed-9 single quote
// ʹ U+02b9 697 ca b9 modifier letter prime
// ˊ U+02ca 714 cb 8a modifier letter acute accent
// ˋ U+02cb 715 cb 8b modifier letter grave accent
// SECONDS
// " U+0022 34 22 quotation mark
// ″ U+2033 8243 e2 80 b3 double prime
// ‶ U+2036 8246 e2 80 b6 reversed double prime
// ˝ U+02dd 733 cb 9d double acute accent
// “ U+201c 8220 e2 80 9c left double quote (also ext ASCII 0x93)
// ” U+201d 8221 e2 80 9d right double quote (also ext ASCII 0x94)
// ‟ U+201f 8223 e2 80 9f reversed-9 double quote
// ʺ U+02ba 698 ca ba modifier letter double prime
// PLUS
// + U+002b 43 2b plus sign
// ➕ U+2795 10133 e2 9e 95 heavy plus
// U+2064 8292 e2 81 a4 invisible plus |⁤|
// MINUS
// - U+002d 45 2d hyphen
// ‐ U+2010 8208 e2 80 90 dash
// ‑ U+2011 8209 e2 80 91 non-breaking hyphen
// – U+2013 8211 e2 80 93 en dash (also ext ASCII 0x96)
// — U+2014 8212 e2 80 94 em dash (also ext ASCII 0x97)
// − U+2212 8722 e2 88 92 minus sign
// ➖ U+2796 10134 e2 9e 96 heavy minus
// IGNORED
//   U+00a0 160 c2 a0 non-breaking space
// U+2007 8199 e2 80 87 figure space | |
// U+2009 8201 e2 80 89 thin space | |
// U+200a 8202 e2 80 8a hair space | |
// U+200b 8203 e2 80 8b invisible space |​|
//   U+202f 8239 e2 80 af narrow space | |
// U+2063 8291 e2 81 a3 invisible separator |⁣|
// « U+00ab 171 c2 ab left guillemot (for cgi-bin)
// » U+00bb 187 c2 bb right guillemot (for cgi-bin)
string dmsa = dms;
replace(dmsa, "\xc2\xb0", 'd' ); // U+00b0 degree symbol
replace(dmsa, "\xc2\xba", 'd' ); // U+00ba alt symbol
replace(dmsa, "\xe2\x81\xb0", 'd' ); // U+2070 sup zero
replace(dmsa, "\xcb\x9a", 'd' ); // U+02da ring above
replace(dmsa, "\xe2\x88\x98", 'd' ); // U+2218 compose function
replace(dmsa, "\xe2\x80\xb2", '\''); // U+2032 prime
replace(dmsa, "\xe2\x80\xb5", '\''); // U+2035 back prime
replace(dmsa, "\xc2\xb4", '\''); // U+00b4 acute accent
replace(dmsa, "\xe2\x80\x98", '\''); // U+2018 left single quote
replace(dmsa, "\xe2\x80\x99", '\''); // U+2019 right single quote
replace(dmsa, "\xe2\x80\x9b", '\''); // U+201b reversed-9 single quote
replace(dmsa, "\xca\xb9", '\''); // U+02b9 modifier letter prime
replace(dmsa, "\xcb\x8a", '\''); // U+02ca modifier letter acute accent
replace(dmsa, "\xcb\x8b", '\''); // U+02cb modifier letter grave accent
replace(dmsa, "\xe2\x80\xb3", '"' ); // U+2033 double prime
replace(dmsa, "\xe2\x80\xb6", '"' ); // U+2036 reversed double prime
replace(dmsa, "\xcb\x9d", '"' ); // U+02dd double acute accent
replace(dmsa, "\xe2\x80\x9c", '"' ); // U+201c left double quote
replace(dmsa, "\xe2\x80\x9d", '"' ); // U+201d right double quote
replace(dmsa, "\xe2\x80\x9f", '"' ); // U+201f reversed-9 double quote
replace(dmsa, "\xca\xba", '"' ); // U+02ba modifier letter double prime
replace(dmsa, "\xe2\x9e\x95", '+' ); // U+2795 heavy plus
replace(dmsa, "\xe2\x81\xa4", '+' ); // U+2064 invisible plus
replace(dmsa, "\xe2\x80\x90", '-' ); // U+2010 dash
replace(dmsa, "\xe2\x80\x91", '-' ); // U+2011 non-breaking hyphen
replace(dmsa, "\xe2\x80\x93", '-' ); // U+2013 en dash
replace(dmsa, "\xe2\x80\x94", '-' ); // U+2014 em dash
replace(dmsa, "\xe2\x88\x92", '-' ); // U+2212 minus sign
replace(dmsa, "\xe2\x9e\x96", '-' ); // U+2796 heavy minus
replace(dmsa, "\xc2\xa0", '\0'); // U+00a0 non-breaking space
replace(dmsa, "\xe2\x80\x87", '\0'); // U+2007 figure space
replace(dmsa, "\xe2\x80\x89", '\0'); // U+2007 thin space
replace(dmsa, "\xe2\x80\x8a", '\0'); // U+200a hair space
replace(dmsa, "\xe2\x80\x8b", '\0'); // U+200b invisible space
replace(dmsa, "\xe2\x80\xaf", '\0'); // U+202f narrow space
replace(dmsa, "\xe2\x81\xa3", '\0'); // U+2063 invisible separator
replace(dmsa, "\xb0", 'd' ); // 0xb0 bare degree symbol
replace(dmsa, "\xba", 'd' ); // 0xba bare alt symbol
replace(dmsa, "*", 'd' ); // GRiD symbol for degree
replace(dmsa, "`", '\''); // grave accent
replace(dmsa, "\xb4", '\''); // 0xb4 bare acute accent
// Don't implement these alternatives; they are only relevant for cgi-bin
// replace(dmsa, "\x91", '\''); // 0x91 ext ASCII left single quote
// replace(dmsa, "\x92", '\''); // 0x92 ext ASCII right single quote
// replace(dmsa, "\x93", '"' ); // 0x93 ext ASCII left double quote
// replace(dmsa, "\x94", '"' ); // 0x94 ext ASCII right double quote
// replace(dmsa, "\x96", '-' ); // 0x96 ext ASCII en dash
// replace(dmsa, "\x97", '-' ); // 0x97 ext ASCII em dash
replace(dmsa, "\xa0", '\0'); // 0xa0 bare non-breaking space
replace(dmsa, "''", '"' ); // '' -> "
string::size_type
beg = 0,
end = unsigned(dmsa.size());
while (beg < end && isspace(dmsa[beg]))
++beg;
while (beg < end && isspace(dmsa[end - 1]))
--end;
// The trimmed string in [beg, end)
real v = -0.0; // So "-0" returns -0.0
int i = 0;
flag ind1 = NONE;
// p is pointer to the next piece that needs decoding
for (string::size_type p = beg, pb; p < end; p = pb, ++i) {
string::size_type pa = p;
// Skip over initial hemisphere letter (for i == 0)
if (i == 0 && Utility::lookup(hemispheres_, dmsa[pa]) >= 0)
++pa;
// Skip over initial sign (checking for it if i == 0)
if (i > 0 || (pa < end && Utility::lookup(signs_, dmsa[pa]) >= 0))
++pa;
// Find next sign
pb = min(dmsa.find_first_of(signs_, pa), end);
flag ind2 = NONE;
v += InternalDecode(dmsa.substr(p, pb - p), ind2);
if (ind1 == NONE)
ind1 = ind2;
else if (!(ind2 == NONE || ind1 == ind2))
throw GeographicErr("Incompatible hemisphere specifier in " +
dmsa.substr(beg, pb - beg));
}
if (i == 0)
throw GeographicErr("Empty or incomplete DMS string " +
dmsa.substr(beg, end - beg));
ind = ind1;
return v;
}
Math::real DMS::InternalDecode(const string& dmsa, flag& ind) {
string errormsg;
do { // Executed once (provides the ability to break)
int sign = 1;
unsigned
beg = 0,
end = unsigned(dmsa.size());
flag ind1 = NONE;
int k = -1;
if (end > beg && (k = Utility::lookup(hemispheres_, dmsa[beg])) >= 0) {
ind1 = (k / 2) ? LONGITUDE : LATITUDE;
sign = k % 2 ? 1 : -1;
++beg;
}
if (end > beg && (k = Utility::lookup(hemispheres_, dmsa[end-1])) >= 0) {
if (k >= 0) {
if (ind1 != NONE) {
if (toupper(dmsa[beg - 1]) == toupper(dmsa[end - 1]))
errormsg = "Repeated hemisphere indicators "
+ Utility::str(dmsa[beg - 1])
+ " in " + dmsa.substr(beg - 1, end - beg + 1);
else
errormsg = "Contradictory hemisphere indicators "
+ Utility::str(dmsa[beg - 1]) + " and "
+ Utility::str(dmsa[end - 1]) + " in "
+ dmsa.substr(beg - 1, end - beg + 1);
break;
}
ind1 = (k / 2) ? LONGITUDE : LATITUDE;
sign = k % 2 ? 1 : -1;
--end;
}
}
if (end > beg && (k = Utility::lookup(signs_, dmsa[beg])) >= 0) {
if (k >= 0) {
sign *= k ? 1 : -1;
++beg;
}
}
if (end == beg) {
errormsg = "Empty or incomplete DMS string " + dmsa;
break;
}
real ipieces[] = {0, 0, 0};
real fpieces[] = {0, 0, 0};
unsigned npiece = 0;
real icurrent = 0;
real fcurrent = 0;
unsigned ncurrent = 0, p = beg;
bool pointseen = false;
unsigned digcount = 0, intcount = 0;
while (p < end) {
char x = dmsa[p++];
if ((k = Utility::lookup(digits_, x)) >= 0) {
++ncurrent;
if (digcount > 0)
++digcount; // Count of decimal digits
else {
icurrent = 10 * icurrent + k;
++intcount;
}
} else if (x == '.') {
if (pointseen) {
errormsg = "Multiple decimal points in "
+ dmsa.substr(beg, end - beg);
break;
}
pointseen = true;
digcount = 1;
} else if ((k = Utility::lookup(dmsindicators_, x)) >= 0) {
if (k >= 3) {
if (p == end) {
errormsg = "Illegal for : to appear at the end of " +
dmsa.substr(beg, end - beg);
break;
}
k = npiece;
}
if (unsigned(k) == npiece - 1) {
errormsg = "Repeated " + string(components_[k]) +
" component in " + dmsa.substr(beg, end - beg);
break;
} else if (unsigned(k) < npiece) {
errormsg = string(components_[k]) + " component follows "
+ string(components_[npiece - 1]) + " component in "
+ dmsa.substr(beg, end - beg);
break;
}
if (ncurrent == 0) {
errormsg = "Missing numbers in " + string(components_[k]) +
" component of " + dmsa.substr(beg, end - beg);
break;
}
if (digcount > 0) {
istringstream s(dmsa.substr(p - intcount - digcount - 1,
intcount + digcount));
s >> fcurrent;
icurrent = 0;
}
ipieces[k] = icurrent;
fpieces[k] = icurrent + fcurrent;
if (p < end) {
npiece = k + 1;
icurrent = fcurrent = 0;
ncurrent = digcount = intcount = 0;
}
} else if (Utility::lookup(signs_, x) >= 0) {
errormsg = "Internal sign in DMS string "
+ dmsa.substr(beg, end - beg);
break;
} else {
errormsg = "Illegal character " + Utility::str(x) + " in DMS string "
+ dmsa.substr(beg, end - beg);
break;
}
}
if (!errormsg.empty())
break;
if (Utility::lookup(dmsindicators_, dmsa[p - 1]) < 0) {
if (npiece >= 3) {
errormsg = "Extra text following seconds in DMS string "
+ dmsa.substr(beg, end - beg);
break;
}
if (ncurrent == 0) {
errormsg = "Missing numbers in trailing component of "
+ dmsa.substr(beg, end - beg);
break;
}
if (digcount > 0) {
istringstream s(dmsa.substr(p - intcount - digcount,
intcount + digcount));
s >> fcurrent;
icurrent = 0;
}
ipieces[npiece] = icurrent;
fpieces[npiece] = icurrent + fcurrent;
}
if (pointseen && digcount == 0) {
errormsg = "Decimal point in non-terminal component of "
+ dmsa.substr(beg, end - beg);
break;
}
// Note that we accept 59.999999... even though it rounds to 60.
if (ipieces[1] >= Math::dm || fpieces[1] > Math::dm ) {
errormsg = "Minutes " + Utility::str(fpieces[1])
+ " not in range [0, " + to_string(Math::dm) + ")";
break;
}
if (ipieces[2] >= Math::ms || fpieces[2] > Math::ms) {
errormsg = "Seconds " + Utility::str(fpieces[2])
+ " not in range [0, " + to_string(Math::ms) + ")";
break;
}
ind = ind1;
// Assume check on range of result is made by calling routine (which
// might be able to offer a better diagnostic).
return real(sign) *
( fpieces[2] != 0 ?
(Math::ms*(Math::dm*fpieces[0] + fpieces[1]) + fpieces[2])/Math::ds :
( fpieces[1] != 0 ?
(Math::dm*fpieces[0] + fpieces[1]) / Math::dm : fpieces[0] ) );
} while (false);
real val = Utility::nummatch<real>(dmsa);
if (val == 0)
throw GeographicErr(errormsg);
else
ind = NONE;
return val;
}
void DMS::DecodeLatLon(const string& stra, const string& strb,
real& lat, real& lon,
bool longfirst) {
real a, b;
flag ia, ib;
a = Decode(stra, ia);
b = Decode(strb, ib);
if (ia == NONE && ib == NONE) {
// Default to lat, long unless longfirst
ia = longfirst ? LONGITUDE : LATITUDE;
ib = longfirst ? LATITUDE : LONGITUDE;
} else if (ia == NONE)
ia = flag(LATITUDE + LONGITUDE - ib);
else if (ib == NONE)
ib = flag(LATITUDE + LONGITUDE - ia);
if (ia == ib)
throw GeographicErr("Both " + stra + " and "
+ strb + " interpreted as "
+ (ia == LATITUDE ? "latitudes" : "longitudes"));
real
lat1 = ia == LATITUDE ? a : b,
lon1 = ia == LATITUDE ? b : a;
if (fabs(lat1) > Math::qd)
throw GeographicErr("Latitude " + Utility::str(lat1)
+ "d not in [-" + to_string(Math::qd)
+ "d, " + to_string(Math::qd) + "d]");
lat = lat1;
lon = lon1;
}
Math::real DMS::DecodeAngle(const string& angstr) {
flag ind;
real ang = Decode(angstr, ind);
if (ind != NONE)
throw GeographicErr("Arc angle " + angstr
+ " includes a hemisphere, N/E/W/S");
return ang;
}
Math::real DMS::DecodeAzimuth(const string& azistr) {
flag ind;
real azi = Decode(azistr, ind);
if (ind == LATITUDE)
throw GeographicErr("Azimuth " + azistr
+ " has a latitude hemisphere, N/S");
return Math::AngNormalize(azi);
}
string DMS::Encode(real angle, component trailing, unsigned prec, flag ind,
char dmssep) {
// Assume check on range of input angle has been made by calling
// routine (which might be able to offer a better diagnostic).
if (!isfinite(angle))
return angle < 0 ? string("-inf") :
(angle > 0 ? string("inf") : string("nan"));
// 15 - 2 * trailing = ceiling(log10(2^53/90/60^trailing)).
// This suffices to give full real precision for numbers in [-90,90]
prec = min(15 + Math::extra_digits() - 2 * unsigned(trailing), prec);
real scale = trailing == MINUTE ? Math::dm :
(trailing == SECOND ? Math::ds : 1);
if (ind == AZIMUTH) {
angle = Math::AngNormalize(angle);
// Only angles strictly less than 0 can become 360; since +/-180 are
// folded together, we convert -0 to +0 (instead of 360).
if (angle < 0)
angle += Math::td;
else
angle = Math::real(0) + angle;
}
int sign = signbit(angle) ? -1 : 1;
angle *= sign;
// Break off integer part to preserve precision and avoid overflow in
// manipulation of fractional part for MINUTE and SECOND
real
idegree = trailing == DEGREE ? 0 : floor(angle),
fdegree = (angle - idegree) * scale;
string s = Utility::str(fdegree, prec), degree, minute, second;
switch (trailing) {
case DEGREE:
degree = s;
break;
default: // case MINUTE: case SECOND:
string::size_type p = s.find_first_of('.');
long long i;
if (p == 0)
i = 0;
else {
i = stoll(s);
if (p == string::npos)
s.clear();
else
s = s.substr(p);
}
// Now i in [0,Math::dm] or [0,Math::ds] for MINUTE/DEGREE
switch (trailing) {
case MINUTE:
minute = to_string(i % Math::dm) + s; i /= Math::dm;
degree = Utility::str(i + idegree, 0); // no overflow since i in [0,1]
break;
default: // case SECOND:
second = to_string(i % Math::ms) + s; i /= Math::ms;
minute = to_string(i % Math::dm) ; i /= Math::dm;
degree = Utility::str(i + idegree, 0); // no overflow since i in [0,1]
break;
}
break;
}
// No glue together degree+minute+second with
// sign + zero-fill + delimiters + hemisphere
ostringstream str;
if (prec) ++prec; // Extra width for decimal point
if (ind == NONE && sign < 0)
str << '-';
str << setfill('0');
switch (trailing) {
case DEGREE:
if (ind != NONE)
str << setw(1 + min(int(ind), 2) + prec);
str << degree;
// Don't include degree designator (d) if it is the trailing component.
break;
case MINUTE:
if (ind != NONE)
str << setw(1 + min(int(ind), 2));
str << degree << (dmssep ? dmssep : char(tolower(dmsindicators_[0])))
<< setw(2 + prec) << minute;
if (!dmssep)
str << char(tolower(dmsindicators_[1]));
break;
default: // case SECOND:
if (ind != NONE)
str << setw(1 + min(int(ind), 2));
str << degree << (dmssep ? dmssep : char(tolower(dmsindicators_[0])))
<< setw(2)
<< minute << (dmssep ? dmssep : char(tolower(dmsindicators_[1])))
<< setw(2 + prec) << second;
if (!dmssep)
str << char(tolower(dmsindicators_[2]));
break;
}
if (ind != NONE && ind != AZIMUTH)
str << hemispheres_[(ind == LATITUDE ? 0 : 2) + (sign < 0 ? 0 : 1)];
return str.str();
}
} // namespace GeographicLib

150
libs/geographiclib/src/DST.cpp Normal file
View File

@@ -0,0 +1,150 @@
/**
* \file DST.cpp
* \brief Implementation for GeographicLib::DST class
*
* Copyright (c) Charles Karney (2022) <charles@karney.com> and licensed under
* the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/DST.hpp>
#include <vector>
#include "kissfft.hh"
namespace GeographicLib {
using namespace std;
DST::DST(int N)
: _N(N < 0 ? 0 : N)
, _fft(make_shared<fft_t>(fft_t(2 * _N, false)))
{}
void DST::reset(int N) {
N = N < 0 ? 0 : N;
if (N == _N) return;
_N = N;
_fft->assign(2 * _N, false);
}
void DST::fft_transform(real data[], real F[], bool centerp) const {
// Implement DST-III (centerp = false) or DST-IV (centerp = true).
// Elements (0,N], resp. [0,N), of data should be set on input for centerp
// = false, resp. true. F must have a size of at least N and on output
// elements [0,N) of F contain the transform.
if (_N == 0) return;
if (centerp) {
for (int i = 0; i < _N; ++i) {
data[_N+i] = data[_N-1-i];
data[2*_N+i] = -data[i];
data[3*_N+i] = -data[_N-1-i];
}
} else {
data[0] = 0; // set [0]
for (int i = 1; i < _N; ++i) data[_N+i] = data[_N-i]; // set [N+1,2*N-1]
for (int i = 0; i < 2*_N; ++i) data[2*_N+i] = -data[i]; // [2*N, 4*N-1]
}
vector<complex<real>> ctemp(2*_N);
_fft->transform_real(data, ctemp.data());
if (centerp) {
real d = -Math::pi()/(4*_N);
for (int i = 0, j = 1; i < _N; ++i, j+=2)
ctemp[j] *= exp(complex<real>(0, j*d));
}
for (int i = 0, j = 1; i < _N; ++i, j+=2) {
F[i] = -ctemp[j].imag() / (2*_N);
}
}
void DST::fft_transform2(real data[], real F[]) const {
// Elements [0,N), of data should be set to the N grid center values and F
// should have size of at least 2*N. On input elements [0,N) of F contain
// the size N transform; on output elements [0,2*N) of F contain the size
// 2*N transform.
fft_transform(data, F+_N, true);
// Copy DST-IV order N tx to [0,N) elements of data
for (int i = 0; i < _N; ++i) data[i] = F[i+_N];
for (int i = _N; i < 2*_N; ++i)
// (DST-IV order N - DST-III order N) / 2
F[i] = (data[2*_N-1-i] - F[2*_N-1-i])/2;
for (int i = 0; i < _N; ++i)
// (DST-IV order N + DST-III order N) / 2
F[i] = (data[i] + F[i])/2;
}
void DST::transform(function<real(real)> f, real F[]) const {
vector<real> data(4 * _N);
real d = Math::pi()/(2 * _N);
for (int i = 1; i <= _N; ++i)
data[i] = f( i * d );
fft_transform(data.data(), F, false);
}
void DST::refine(function<real(real)> f, real F[]) const {
vector<real> data(4 * _N);
real d = Math::pi()/(4 * _N);
for (int i = 0; i < _N; ++i)
data[i] = f( (2*i + 1) * d );
fft_transform2(data.data(), F);
}
Math::real DST::eval(real sinx, real cosx, const real F[], int N) {
// Evaluate
// y = sum(F[i] * sin((2*i+1) * x), i, 0, N-1)
// using Clenshaw summation.
// Approx operation count = (N + 5) mult and (2 * N + 2) add
real
ar = 2 * (cosx - sinx) * (cosx + sinx), // 2 * cos(2 * x)
y0 = N & 1 ? F[--N] : 0, y1 = 0; // accumulators for sum
// Now N is even
while (N > 0) {
// Unroll loop x 2, so accumulators return to their original role
y1 = ar * y0 - y1 + F[--N];
y0 = ar * y1 - y0 + F[--N];
}
return sinx * (y0 + y1); // sin(x) * (y0 + y1)
}
Math::real DST::integral(real sinx, real cosx, const real F[], int N) {
// Evaluate
// y = -sum(F[i]/(2*i+1) * cos((2*i+1) * x), i, 0, N-1)
// using Clenshaw summation.
// Approx operation count = (N + 5) mult and (2 * N + 2) add
real
ar = 2 * (cosx - sinx) * (cosx + sinx), // 2 * cos(2 * x)
y0 = 0, y1 = 0; // accumulators for sum
for (--N; N >= 0; --N) {
real t = ar * y0 - y1 + F[N]/(2*N+1);
y1 = y0; y0 = t;
}
return cosx * (y1 - y0); // cos(x) * (y1 - y0)
}
Math::real DST::integral(real sinx, real cosx, real siny, real cosy,
const real F[], int N) {
// return integral(siny, cosy, F, N) - integral(sinx, cosx, F, N);
real
// 2*cos(y-x)*cos(y+x) -> 2 * cos(2 * x)
ac = +2 * (cosy * cosx + siny * sinx) * (cosy * cosx - siny * sinx),
// -2*sin(y-x)*sin(y+x) -> 0
as = -2 * (siny * cosx - cosy * sinx) * (siny * cosx + cosy * sinx),
y0 = 0, y1 = 0, z0 = 0, z1 = 0; // accumulators for sum
for (--N; N >= 0; --N) {
real
ty = ac * y0 + as * z0 - y1 + F[N]/(2*N+1),
tz = as * y0 + ac * z0 - z1;
y1 = y0; y0 = ty;
z1 = z0; z0 = tz;
}
// B[0] - B[1] = [y0-y1, z0-z1]
// F[0] = [cosy + cosx, cosy - cosx] -> [2 * cosx, 0]
// (B[0] - B[1]) . F[0]
// = [(y0 - y1) * (cosy + cosx) + (z0 - z1) * (cosy - cosx),
// (y0 - y1) * (cosy - cosx) + (z0 - z1) * (cosy + cosx),
// return -(2nd element)
return (y1 - y0) * (cosy - cosx) + (z1 - z0) * (cosy + cosx);
}
} // namespace GeographicLib

129
libs/geographiclib/src/Ellipsoid.cpp Normal file
View File

@@ -0,0 +1,129 @@
/**
* \file Ellipsoid.cpp
* \brief Implementation for GeographicLib::Ellipsoid class
*
* Copyright (c) Charles Karney (2012-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/Ellipsoid.hpp>
#if defined(_MSC_VER)
// Squelch warnings about enum-float expressions
# pragma warning (disable: 5055)
#endif
namespace GeographicLib {
using namespace std;
Ellipsoid::Ellipsoid(real a, real f)
: stol_(real(0.01) * sqrt(numeric_limits<real>::epsilon()))
, _a(a)
, _f(f)
, _f1(1 - _f)
, _f12(Math::sq(_f1))
, _e2(_f * (2 - _f))
, _es((_f < 0 ? -1 : 1) * sqrt(fabs(_e2)))
, _e12(_e2 / (1 - _e2))
, _n(_f / (2 - _f))
, _b(_a * _f1)
, _tm(_a, _f, real(1))
, _ell(-_e12)
, _au(_a, _f, real(0), real(1), real(0), real(1), real(1))
{}
const Ellipsoid& Ellipsoid::WGS84() {
static const Ellipsoid wgs84(Constants::WGS84_a(), Constants::WGS84_f());
return wgs84;
}
Math::real Ellipsoid::QuarterMeridian() const
{ return _b * _ell.E(); }
Math::real Ellipsoid::Area() const {
return 4 * Math::pi() *
((Math::sq(_a) + Math::sq(_b) *
(_e2 == 0 ? 1 :
(_e2 > 0 ? atanh(sqrt(_e2)) : atan(sqrt(-_e2))) /
sqrt(fabs(_e2))))/2);
}
Math::real Ellipsoid::ParametricLatitude(real phi) const
{ return Math::atand(_f1 * Math::tand(Math::LatFix(phi))); }
Math::real Ellipsoid::InverseParametricLatitude(real beta) const
{ return Math::atand(Math::tand(Math::LatFix(beta)) / _f1); }
Math::real Ellipsoid::GeocentricLatitude(real phi) const
{ return Math::atand(_f12 * Math::tand(Math::LatFix(phi))); }
Math::real Ellipsoid::InverseGeocentricLatitude(real theta) const
{ return Math::atand(Math::tand(Math::LatFix(theta)) / _f12); }
Math::real Ellipsoid::RectifyingLatitude(real phi) const {
return fabs(phi) == Math::qd ? phi:
Math::qd * MeridianDistance(phi) / QuarterMeridian();
}
Math::real Ellipsoid::InverseRectifyingLatitude(real mu) const {
if (fabs(mu) == Math::qd)
return mu;
return InverseParametricLatitude(_ell.Einv(mu * _ell.E() / Math::qd) /
Math::degree());
}
Math::real Ellipsoid::AuthalicLatitude(real phi) const
{ return Math::atand(_au.txif(Math::tand(Math::LatFix(phi)))); }
Math::real Ellipsoid::InverseAuthalicLatitude(real xi) const
{ return Math::atand(_au.tphif(Math::tand(Math::LatFix(xi)))); }
Math::real Ellipsoid::ConformalLatitude(real phi) const
{ return Math::atand(Math::taupf(Math::tand(Math::LatFix(phi)), _es)); }
Math::real Ellipsoid::InverseConformalLatitude(real chi) const
{ return Math::atand(Math::tauf(Math::tand(Math::LatFix(chi)), _es)); }
Math::real Ellipsoid::IsometricLatitude(real phi) const
{ return asinh(Math::taupf(Math::tand(Math::LatFix(phi)), _es)) /
Math::degree(); }
Math::real Ellipsoid::InverseIsometricLatitude(real psi) const
{ return Math::atand(Math::tauf(sinh(psi * Math::degree()), _es)); }
Math::real Ellipsoid::CircleRadius(real phi) const {
return fabs(phi) == Math::qd ? 0 :
// a * cos(beta)
_a / hypot(real(1), _f1 * Math::tand(Math::LatFix(phi)));
}
Math::real Ellipsoid::CircleHeight(real phi) const {
real tbeta = _f1 * Math::tand(phi);
// b * sin(beta)
return _b * tbeta / hypot(real(1),
_f1 * Math::tand(Math::LatFix(phi)));
}
Math::real Ellipsoid::MeridianDistance(real phi) const
{ return _b * _ell.Ed( ParametricLatitude(phi) ); }
Math::real Ellipsoid::MeridionalCurvatureRadius(real phi) const {
real v = 1 - _e2 * Math::sq(Math::sind(Math::LatFix(phi)));
return _a * (1 - _e2) / (v * sqrt(v));
}
Math::real Ellipsoid::TransverseCurvatureRadius(real phi) const {
real v = 1 - _e2 * Math::sq(Math::sind(Math::LatFix(phi)));
return _a / sqrt(v);
}
Math::real Ellipsoid::NormalCurvatureRadius(real phi, real azi) const {
real calp, salp,
v = 1 - _e2 * Math::sq(Math::sind(Math::LatFix(phi)));
Math::sincosd(azi, salp, calp);
return _a / (sqrt(v) * (Math::sq(calp) * v / (1 - _e2) + Math::sq(salp)));
}
} // namespace GeographicLib

572
libs/geographiclib/src/EllipticFunction.cpp Normal file
View File

@@ -0,0 +1,572 @@
/**
* \file EllipticFunction.cpp
* \brief Implementation for GeographicLib::EllipticFunction class
*
* Copyright (c) Charles Karney (2008-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/EllipticFunction.hpp>
#if defined(_MSC_VER)
// Squelch warnings about constant conditional and enum-float expressions
# pragma warning (disable: 4127 5055)
#endif
namespace GeographicLib {
using namespace std;
/*
* Implementation of methods given in
*
* B. C. Carlson
* Computation of elliptic integrals
* Numerical Algorithms 10, 13-26 (1995)
*/
Math::real EllipticFunction::RF(real x, real y, real z) {
// Carlson, eqs 2.2 - 2.7
static const real tolRF =
pow(3 * numeric_limits<real>::epsilon() * real(0.01), 1/real(8));
real
A0 = (x + y + z)/3,
An = A0,
Q = fmax(fmax(fabs(A0-x), fabs(A0-y)), fabs(A0-z)) / tolRF,
x0 = x,
y0 = y,
z0 = z,
mul = 1;
while (Q >= mul * fabs(An)) {
// Max 6 trips
real lam = sqrt(x0)*sqrt(y0) + sqrt(y0)*sqrt(z0) + sqrt(z0)*sqrt(x0);
An = (An + lam)/4;
x0 = (x0 + lam)/4;
y0 = (y0 + lam)/4;
z0 = (z0 + lam)/4;
mul *= 4;
}
real
X = (A0 - x) / (mul * An),
Y = (A0 - y) / (mul * An),
Z = - (X + Y),
E2 = X*Y - Z*Z,
E3 = X*Y*Z;
// https://dlmf.nist.gov/19.36.E1
// Polynomial is
// (1 - E2/10 + E3/14 + E2^2/24 - 3*E2*E3/44
// - 5*E2^3/208 + 3*E3^2/104 + E2^2*E3/16)
// convert to Horner form...
return (E3 * (6930 * E3 + E2 * (15015 * E2 - 16380) + 17160) +
E2 * ((10010 - 5775 * E2) * E2 - 24024) + 240240) /
(240240 * sqrt(An));
}
Math::real EllipticFunction::RF(real x, real y) {
// Carlson, eqs 2.36 - 2.38
static const real tolRG0 =
real(2.7) * sqrt((numeric_limits<real>::epsilon() * real(0.01)));
real xn = sqrt(x), yn = sqrt(y);
if (xn < yn) swap(xn, yn);
while (fabs(xn-yn) > tolRG0 * xn) {
// Max 4 trips
real t = (xn + yn) /2;
yn = sqrt(xn * yn);
xn = t;
}
return Math::pi() / (xn + yn);
}
Math::real EllipticFunction::RC(real x, real y) {
// Defined only for y != 0 and x >= 0.
return ( !(x >= y) ? // x < y and catch nans
// https://dlmf.nist.gov/19.2.E18
atan(sqrt((y - x) / x)) / sqrt(y - x) :
( x == y ? 1 / sqrt(y) :
asinh( y > 0 ?
// https://dlmf.nist.gov/19.2.E19
// atanh(sqrt((x - y) / x))
sqrt((x - y) / y) :
// https://dlmf.nist.gov/19.2.E20
// atanh(sqrt(x / (x - y)))
sqrt(-x / y) ) / sqrt(x - y) ) );
}
Math::real EllipticFunction::RG(real x, real y, real z) {
return (x == 0 ? RG(y, z) :
(y == 0 ? RG(z, x) :
(z == 0 ? RG(x, y) :
// Carlson, eq 1.7
(z * RF(x, y, z) - (x-z) * (y-z) * RD(x, y, z) / 3
+ sqrt(x * y / z)) / 2 )));
}
Math::real EllipticFunction::RG(real x, real y) {
// Carlson, eqs 2.36 - 2.39
static const real tolRG0 =
real(2.7) * sqrt((numeric_limits<real>::epsilon() * real(0.01)));
real
x0 = sqrt(fmax(x, y)),
y0 = sqrt(fmin(x, y)),
xn = x0,
yn = y0,
s = 0,
mul = real(0.25);
while (fabs(xn-yn) > tolRG0 * xn) {
// Max 4 trips
real t = (xn + yn) /2;
yn = sqrt(xn * yn);
xn = t;
mul *= 2;
t = xn - yn;
s += mul * t * t;
}
return (Math::sq( (x0 + y0)/2 ) - s) * Math::pi() / (2 * (xn + yn));
}
Math::real EllipticFunction::RJ(real x, real y, real z, real p) {
// Carlson, eqs 2.17 - 2.25
static const real
tolRD = pow(real(0.2) * (numeric_limits<real>::epsilon() * real(0.01)),
1/real(8));
real
A0 = (x + y + z + 2*p)/5,
An = A0,
delta = (p-x) * (p-y) * (p-z),
Q = fmax(fmax(fabs(A0-x), fabs(A0-y)),
fmax(fabs(A0-z), fabs(A0-p))) / tolRD,
x0 = x,
y0 = y,
z0 = z,
p0 = p,
mul = 1,
mul3 = 1,
s = 0;
while (Q >= mul * fabs(An)) {
// Max 7 trips
real
lam = sqrt(x0)*sqrt(y0) + sqrt(y0)*sqrt(z0) + sqrt(z0)*sqrt(x0),
d0 = (sqrt(p0)+sqrt(x0)) * (sqrt(p0)+sqrt(y0)) * (sqrt(p0)+sqrt(z0)),
e0 = delta/(mul3 * Math::sq(d0));
s += RC(1, 1 + e0)/(mul * d0);
An = (An + lam)/4;
x0 = (x0 + lam)/4;
y0 = (y0 + lam)/4;
z0 = (z0 + lam)/4;
p0 = (p0 + lam)/4;
mul *= 4;
mul3 *= 64;
}
real
X = (A0 - x) / (mul * An),
Y = (A0 - y) / (mul * An),
Z = (A0 - z) / (mul * An),
P = -(X + Y + Z) / 2,
E2 = X*Y + X*Z + Y*Z - 3*P*P,
E3 = X*Y*Z + 2*P * (E2 + 2*P*P),
E4 = (2*X*Y*Z + P * (E2 + 3*P*P)) * P,
E5 = X*Y*Z*P*P;
// https://dlmf.nist.gov/19.36.E2
// Polynomial is
// (1 - 3*E2/14 + E3/6 + 9*E2^2/88 - 3*E4/22 - 9*E2*E3/52 + 3*E5/26
// - E2^3/16 + 3*E3^2/40 + 3*E2*E4/20 + 45*E2^2*E3/272
// - 9*(E3*E4+E2*E5)/68)
return ((471240 - 540540 * E2) * E5 +
(612612 * E2 - 540540 * E3 - 556920) * E4 +
E3 * (306306 * E3 + E2 * (675675 * E2 - 706860) + 680680) +
E2 * ((417690 - 255255 * E2) * E2 - 875160) + 4084080) /
(4084080 * mul * An * sqrt(An)) + 6 * s;
}
Math::real EllipticFunction::RD(real x, real y, real z) {
// Carlson, eqs 2.28 - 2.34
static const real
tolRD = pow(real(0.2) * (numeric_limits<real>::epsilon() * real(0.01)),
1/real(8));
real
A0 = (x + y + 3*z)/5,
An = A0,
Q = fmax(fmax(fabs(A0-x), fabs(A0-y)), fabs(A0-z)) / tolRD,
x0 = x,
y0 = y,
z0 = z,
mul = 1,
s = 0;
while (Q >= mul * fabs(An)) {
// Max 7 trips
real lam = sqrt(x0)*sqrt(y0) + sqrt(y0)*sqrt(z0) + sqrt(z0)*sqrt(x0);
s += 1/(mul * sqrt(z0) * (z0 + lam));
An = (An + lam)/4;
x0 = (x0 + lam)/4;
y0 = (y0 + lam)/4;
z0 = (z0 + lam)/4;
mul *= 4;
}
real
X = (A0 - x) / (mul * An),
Y = (A0 - y) / (mul * An),
Z = -(X + Y) / 3,
E2 = X*Y - 6*Z*Z,
E3 = (3*X*Y - 8*Z*Z)*Z,
E4 = 3 * (X*Y - Z*Z) * Z*Z,
E5 = X*Y*Z*Z*Z;
// https://dlmf.nist.gov/19.36.E2
// Polynomial is
// (1 - 3*E2/14 + E3/6 + 9*E2^2/88 - 3*E4/22 - 9*E2*E3/52 + 3*E5/26
// - E2^3/16 + 3*E3^2/40 + 3*E2*E4/20 + 45*E2^2*E3/272
// - 9*(E3*E4+E2*E5)/68)
return ((471240 - 540540 * E2) * E5 +
(612612 * E2 - 540540 * E3 - 556920) * E4 +
E3 * (306306 * E3 + E2 * (675675 * E2 - 706860) + 680680) +
E2 * ((417690 - 255255 * E2) * E2 - 875160) + 4084080) /
(4084080 * mul * An * sqrt(An)) + 3 * s;
}
void EllipticFunction::Reset(real k2, real alpha2,
real kp2, real alphap2) {
// Accept nans here (needed for GeodesicExact)
if (k2 > 1)
throw GeographicErr("Parameter k2 is not in (-inf, 1]");
if (alpha2 > 1)
throw GeographicErr("Parameter alpha2 is not in (-inf, 1]");
if (kp2 < 0)
throw GeographicErr("Parameter kp2 is not in [0, inf)");
if (alphap2 < 0)
throw GeographicErr("Parameter alphap2 is not in [0, inf)");
_k2 = k2;
_kp2 = kp2;
_alpha2 = alpha2;
_alphap2 = alphap2;
_eps = _k2/Math::sq(sqrt(_kp2) + 1);
// Values of complete elliptic integrals for k = 0,1 and alpha = 0,1
// K E D
// k = 0: pi/2 pi/2 pi/4
// k = 1: inf 1 inf
// Pi G H
// k = 0, alpha = 0: pi/2 pi/2 pi/4
// k = 1, alpha = 0: inf 1 1
// k = 0, alpha = 1: inf inf pi/2
// k = 1, alpha = 1: inf inf inf
//
// Pi(0, k) = K(k)
// G(0, k) = E(k)
// H(0, k) = K(k) - D(k)
// Pi(0, k) = K(k)
// G(0, k) = E(k)
// H(0, k) = K(k) - D(k)
// Pi(alpha2, 0) = pi/(2*sqrt(1-alpha2))
// G(alpha2, 0) = pi/(2*sqrt(1-alpha2))
// H(alpha2, 0) = pi/(2*(1 + sqrt(1-alpha2)))
// Pi(alpha2, 1) = inf
// H(1, k) = K(k)
// G(alpha2, 1) = H(alpha2, 1) = RC(1, alphap2)
if (_k2 != 0) {
// Complete elliptic integral K(k), Carlson eq. 4.1
// https://dlmf.nist.gov/19.25.E1
_kKc = _kp2 != 0 ? RF(_kp2, 1) : Math::infinity();
// Complete elliptic integral E(k), Carlson eq. 4.2
// https://dlmf.nist.gov/19.25.E1
_eEc = _kp2 != 0 ? 2 * RG(_kp2, 1) : 1;
// D(k) = (K(k) - E(k))/k^2, Carlson eq.4.3
// https://dlmf.nist.gov/19.25.E1
_dDc = _kp2 != 0 ? RD(0, _kp2, 1) / 3 : Math::infinity();
} else {
_kKc = _eEc = Math::pi()/2; _dDc = _kKc/2;
}
if (_alpha2 != 0) {
// https://dlmf.nist.gov/19.25.E2
real rj = (_kp2 != 0 && _alphap2 != 0) ? RJ(0, _kp2, 1, _alphap2) :
Math::infinity(),
// Only use rc if _kp2 = 0.
rc = _kp2 != 0 ? 0 :
(_alphap2 != 0 ? RC(1, _alphap2) : Math::infinity());
// Pi(alpha^2, k)
_pPic = _kp2 != 0 ? _kKc + _alpha2 * rj / 3 : Math::infinity();
// G(alpha^2, k)
_gGc = _kp2 != 0 ? _kKc + (_alpha2 - _k2) * rj / 3 : rc;
// H(alpha^2, k)
_hHc = _kp2 != 0 ? _kKc - (_alphap2 != 0 ? _alphap2 * rj : 0) / 3 : rc;
} else {
_pPic = _kKc; _gGc = _eEc;
// Hc = Kc - Dc but this involves large cancellations if k2 is close to
// 1. So write (for alpha2 = 0)
// Hc = int(cos(phi)^2/sqrt(1-k2*sin(phi)^2),phi,0,pi/2)
// = 1/sqrt(1-k2) * int(sin(phi)^2/sqrt(1-k2/kp2*sin(phi)^2,...)
// = 1/kp * D(i*k/kp)
// and use D(k) = RD(0, kp2, 1) / 3
// so Hc = 1/kp * RD(0, 1/kp2, 1) / 3
// = kp2 * RD(0, 1, kp2) / 3
// using https://dlmf.nist.gov/19.20.E18
// Equivalently
// RF(x, 1) - RD(0, x, 1)/3 = x * RD(0, 1, x)/3 for x > 0
// For k2 = 1 and alpha2 = 0, we have
// Hc = int(cos(phi),...) = 1
_hHc = _kp2 != 0 ? _kp2 * RD(0, 1, _kp2) / 3 : 1;
}
}
/*
* Implementation of methods given in
*
* R. Bulirsch
* Numerical Calculation of Elliptic Integrals and Elliptic Functions
* Numericshe Mathematik 7, 78-90 (1965)
*/
void EllipticFunction::sncndn(real x, real& sn, real& cn, real& dn) const {
// Bulirsch's sncndn routine, p 89.
static const real tolJAC =
sqrt(numeric_limits<real>::epsilon() * real(0.01));
if (_kp2 != 0) {
real mc = _kp2, d = 0;
if (signbit(_kp2)) {
d = 1 - mc;
mc /= -d;
d = sqrt(d);
x *= d;
}
real c = 0; // To suppress warning about uninitialized variable
real m[num_], n[num_];
unsigned l = 0;
for (real a = 1; l < num_ || GEOGRAPHICLIB_PANIC; ++l) {
// This converges quadratically. Max 5 trips
m[l] = a;
n[l] = mc = sqrt(mc);
c = (a + mc) / 2;
if (!(fabs(a - mc) > tolJAC * a)) {
++l;
break;
}
mc *= a;
a = c;
}
x *= c;
sn = sin(x);
cn = cos(x);
dn = 1;
if (sn != 0) {
real a = cn / sn;
c *= a;
while (l--) {
real b = m[l];
a *= c;
c *= dn;
dn = (n[l] + a) / (b + a);
a = c / b;
}
a = 1 / sqrt(c*c + 1);
sn = signbit(sn) ? -a : a;
cn = c * sn;
if (signbit(_kp2)) {
swap(cn, dn);
sn /= d;
}
}
} else {
sn = tanh(x);
dn = cn = 1 / cosh(x);
}
}
Math::real EllipticFunction::F(real sn, real cn, real dn) const {
// Carlson, eq. 4.5 and
// https://dlmf.nist.gov/19.25.E5
real cn2 = cn*cn, dn2 = dn*dn,
fi = cn2 != 0 ? fabs(sn) * RF(cn2, dn2, 1) : K();
// Enforce usual trig-like symmetries
if (signbit(cn))
fi = 2 * K() - fi;
return copysign(fi, sn);
}
Math::real EllipticFunction::E(real sn, real cn, real dn) const {
real
cn2 = cn*cn, dn2 = dn*dn, sn2 = sn*sn,
ei = cn2 != 0 ?
fabs(sn) * ( _k2 <= 0 ?
// Carlson, eq. 4.6 and
// https://dlmf.nist.gov/19.25.E9
RF(cn2, dn2, 1) - _k2 * sn2 * RD(cn2, dn2, 1) / 3 :
( _kp2 >= 0 ?
// https://dlmf.nist.gov/19.25.E10
_kp2 * RF(cn2, dn2, 1) +
_k2 * _kp2 * sn2 * RD(cn2, 1, dn2) / 3 +
_k2 * fabs(cn) / dn :
// https://dlmf.nist.gov/19.25.E11
- _kp2 * sn2 * RD(dn2, 1, cn2) / 3 +
dn / fabs(cn) ) ) :
E();
// Enforce usual trig-like symmetries
if (signbit(cn))
ei = 2 * E() - ei;
return copysign(ei, sn);
}
Math::real EllipticFunction::D(real sn, real cn, real dn) const {
// Carlson, eq. 4.8 and
// https://dlmf.nist.gov/19.25.E13
real
cn2 = cn*cn, dn2 = dn*dn, sn2 = sn*sn,
di = cn2 != 0 ? fabs(sn) * sn2 * RD(cn2, dn2, 1) / 3 : D();
// Enforce usual trig-like symmetries
if (signbit(cn))
di = 2 * D() - di;
return copysign(di, sn);
}
Math::real EllipticFunction::Pi(real sn, real cn, real dn) const {
// Carlson, eq. 4.7 and
// https://dlmf.nist.gov/19.25.E14
real
cn2 = cn*cn, dn2 = dn*dn, sn2 = sn*sn,
pii = cn2 != 0 ? fabs(sn) * (RF(cn2, dn2, 1) +
_alpha2 * sn2 *
RJ(cn2, dn2, 1, cn2 + _alphap2 * sn2) / 3) :
Pi();
// Enforce usual trig-like symmetries
if (signbit(cn))
pii = 2 * Pi() - pii;
return copysign(pii, sn);
}
Math::real EllipticFunction::G(real sn, real cn, real dn) const {
real
cn2 = cn*cn, dn2 = dn*dn, sn2 = sn*sn,
gi = cn2 != 0 ? fabs(sn) * (RF(cn2, dn2, 1) +
(_alpha2 - _k2) * sn2 *
RJ(cn2, dn2, 1, cn2 + _alphap2 * sn2) / 3) :
G();
// Enforce usual trig-like symmetries
if (signbit(cn))
gi = 2 * G() - gi;
return copysign(gi, sn);
}
Math::real EllipticFunction::H(real sn, real cn, real dn) const {
real
cn2 = cn*cn, dn2 = dn*dn, sn2 = sn*sn,
// WARNING: large cancellation if k2 = 1, alpha2 = 0, and phi near pi/2
hi = cn2 != 0 ? fabs(sn) * (RF(cn2, dn2, 1) -
_alphap2 * sn2 *
RJ(cn2, dn2, 1, cn2 + _alphap2 * sn2) / 3) :
H();
// Enforce usual trig-like symmetries
if (signbit(cn))
hi = 2 * H() - hi;
return copysign(hi, sn);
}
Math::real EllipticFunction::deltaF(real sn, real cn, real dn) const {
// Function is periodic with period pi
if (signbit(cn)) { cn = -cn; sn = -sn; }
return F(sn, cn, dn) * (Math::pi()/2) / K() - atan2(sn, cn);
}
Math::real EllipticFunction::deltaE(real sn, real cn, real dn) const {
// Function is periodic with period pi
if (signbit(cn)) { cn = -cn; sn = -sn; }
return E(sn, cn, dn) * (Math::pi()/2) / E() - atan2(sn, cn);
}
Math::real EllipticFunction::deltaPi(real sn, real cn, real dn) const {
// Function is periodic with period pi
if (signbit(cn)) { cn = -cn; sn = -sn; }
return Pi(sn, cn, dn) * (Math::pi()/2) / Pi() - atan2(sn, cn);
}
Math::real EllipticFunction::deltaD(real sn, real cn, real dn) const {
// Function is periodic with period pi
if (signbit(cn)) { cn = -cn; sn = -sn; }
return D(sn, cn, dn) * (Math::pi()/2) / D() - atan2(sn, cn);
}
Math::real EllipticFunction::deltaG(real sn, real cn, real dn) const {
// Function is periodic with period pi
if (signbit(cn)) { cn = -cn; sn = -sn; }
return G(sn, cn, dn) * (Math::pi()/2) / G() - atan2(sn, cn);
}
Math::real EllipticFunction::deltaH(real sn, real cn, real dn) const {
// Function is periodic with period pi
if (signbit(cn)) { cn = -cn; sn = -sn; }
return H(sn, cn, dn) * (Math::pi()/2) / H() - atan2(sn, cn);
}
Math::real EllipticFunction::F(real phi) const {
real sn = sin(phi), cn = cos(phi), dn = Delta(sn, cn);
return fabs(phi) < Math::pi() ? F(sn, cn, dn) :
(deltaF(sn, cn, dn) + phi) * K() / (Math::pi()/2);
}
Math::real EllipticFunction::E(real phi) const {
real sn = sin(phi), cn = cos(phi), dn = Delta(sn, cn);
return fabs(phi) < Math::pi() ? E(sn, cn, dn) :
(deltaE(sn, cn, dn) + phi) * E() / (Math::pi()/2);
}
Math::real EllipticFunction::Ed(real ang) const {
// ang - Math::AngNormalize(ang) is (nearly) an exact multiple of 360
real n = round((ang - Math::AngNormalize(ang))/Math::td);
real sn, cn;
Math::sincosd(ang, sn, cn);
return E(sn, cn, Delta(sn, cn)) + 4 * E() * n;
}
Math::real EllipticFunction::Pi(real phi) const {
real sn = sin(phi), cn = cos(phi), dn = Delta(sn, cn);
return fabs(phi) < Math::pi() ? Pi(sn, cn, dn) :
(deltaPi(sn, cn, dn) + phi) * Pi() / (Math::pi()/2);
}
Math::real EllipticFunction::D(real phi) const {
real sn = sin(phi), cn = cos(phi), dn = Delta(sn, cn);
return fabs(phi) < Math::pi() ? D(sn, cn, dn) :
(deltaD(sn, cn, dn) + phi) * D() / (Math::pi()/2);
}
Math::real EllipticFunction::G(real phi) const {
real sn = sin(phi), cn = cos(phi), dn = Delta(sn, cn);
return fabs(phi) < Math::pi() ? G(sn, cn, dn) :
(deltaG(sn, cn, dn) + phi) * G() / (Math::pi()/2);
}
Math::real EllipticFunction::H(real phi) const {
real sn = sin(phi), cn = cos(phi), dn = Delta(sn, cn);
return fabs(phi) < Math::pi() ? H(sn, cn, dn) :
(deltaH(sn, cn, dn) + phi) * H() / (Math::pi()/2);
}
Math::real EllipticFunction::Einv(real x) const {
static const real tolJAC =
sqrt(numeric_limits<real>::epsilon() * real(0.01));
real n = floor(x / (2 * _eEc) + real(0.5));
x -= 2 * _eEc * n; // x now in [-ec, ec)
// Linear approximation
real phi = Math::pi() * x / (2 * _eEc); // phi in [-pi/2, pi/2)
// First order correction
phi -= _eps * sin(2 * phi) / 2;
// For kp2 close to zero use asin(x/_eEc) or
// J. P. Boyd, Applied Math. and Computation 218, 7005-7013 (2012)
// https://doi.org/10.1016/j.amc.2011.12.021
for (int i = 0; i < num_ || GEOGRAPHICLIB_PANIC; ++i) {
real
sn = sin(phi),
cn = cos(phi),
dn = Delta(sn, cn),
err = (E(sn, cn, dn) - x)/dn;
phi -= err;
if (!(fabs(err) > tolJAC))
break;
}
return n * Math::pi() + phi;
}
Math::real EllipticFunction::deltaEinv(real stau, real ctau) const {
// Function is periodic with period pi
if (signbit(ctau)) { ctau = -ctau; stau = -stau; }
real tau = atan2(stau, ctau);
return Einv( tau * E() / (Math::pi()/2) ) - tau;
}
} // namespace GeographicLib

130
libs/geographiclib/src/GARS.cpp Normal file
View File

@@ -0,0 +1,130 @@
/**
* \file GARS.cpp
* \brief Implementation for GeographicLib::GARS class
*
* Copyright (c) Charles Karney (2015-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/GARS.hpp>
#include <GeographicLib/Utility.hpp>
#if defined(_MSC_VER)
// Squelch warnings about enum-float expressions
# pragma warning (disable: 5055)
#endif
namespace GeographicLib {
using namespace std;
const char* const GARS::digits_ = "0123456789";
const char* const GARS::letters_ = "ABCDEFGHJKLMNPQRSTUVWXYZ";
void GARS::Forward(real lat, real lon, int prec, string& gars) {
using std::isnan; // Needed for Centos 7, ubuntu 14
if (fabs(lat) > Math::qd)
throw GeographicErr("Latitude " + Utility::str(lat)
+ "d not in [-" + to_string(Math::qd)
+ "d, " + to_string(Math::qd) + "d]");
if (isnan(lat) || isnan(lon)) {
gars = "INVALID";
return;
}
lon = Math::AngNormalize(lon);
if (lon == Math::hd) lon = -Math::hd; // lon now in [-180,180)
if (lat == Math::qd) lat *= (1 - numeric_limits<real>::epsilon() / 2);
prec = max(0, min(int(maxprec_), prec));
int
x = int(floor(lon * m_)) - lonorig_ * m_,
y = int(floor(lat * m_)) - latorig_ * m_,
ilon = x * mult1_ / m_,
ilat = y * mult1_ / m_;
x -= ilon * m_ / mult1_; y -= ilat * m_ / mult1_;
char gars1[maxlen_];
++ilon;
for (int c = lonlen_; c--;) {
gars1[c] = digits_[ ilon % baselon_]; ilon /= baselon_;
}
for (int c = latlen_; c--;) {
gars1[lonlen_ + c] = letters_[ilat % baselat_]; ilat /= baselat_;
}
if (prec > 0) {
ilon = x / mult3_; ilat = y / mult3_;
gars1[baselen_] = digits_[mult2_ * (mult2_ - 1 - ilat) + ilon + 1];
if (prec > 1) {
ilon = x % mult3_; ilat = y % mult3_;
gars1[baselen_ + 1] = digits_[mult3_ * (mult3_ - 1 - ilat) + ilon + 1];
}
}
gars.resize(baselen_ + prec);
copy(gars1, gars1 + baselen_ + prec, gars.begin());
}
void GARS::Reverse(const string& gars, real& lat, real& lon,
int& prec, bool centerp) {
int len = int(gars.length());
if (len >= 3 &&
toupper(gars[0]) == 'I' &&
toupper(gars[1]) == 'N' &&
toupper(gars[2]) == 'V') {
lat = lon = Math::NaN();
return;
}
if (len < baselen_)
throw GeographicErr("GARS must have at least 5 characters " + gars);
if (len > maxlen_)
throw GeographicErr("GARS can have at most 7 characters " + gars);
int prec1 = len - baselen_;
int ilon = 0;
for (int c = 0; c < lonlen_; ++c) {
int k = Utility::lookup(digits_, gars[c]);
if (k < 0)
throw GeographicErr("GARS must start with 3 digits " + gars);
ilon = ilon * baselon_ + k;
}
if (!(ilon >= 1 && ilon <= 2 * Math::td))
throw GeographicErr("Initial digits in GARS must lie in [1, 720] " +
gars);
--ilon;
int ilat = 0;
for (int c = 0; c < latlen_; ++c) {
int k = Utility::lookup(letters_, gars[lonlen_ + c]);
if (k < 0)
throw GeographicErr("Illegal letters in GARS " + gars.substr(3,2));
ilat = ilat * baselat_ + k;
}
if (!(ilat < Math::td))
throw GeographicErr("GARS letters must lie in [AA, QZ] " + gars);
real
unit = mult1_,
lat1 = ilat + latorig_ * unit,
lon1 = ilon + lonorig_ * unit;
if (prec1 > 0) {
int k = Utility::lookup(digits_, gars[baselen_]);
if (!(k >= 1 && k <= mult2_ * mult2_))
throw GeographicErr("6th character in GARS must [1, 4] " + gars);
--k;
unit *= mult2_;
lat1 = mult2_ * lat1 + (mult2_ - 1 - k / mult2_);
lon1 = mult2_ * lon1 + (k % mult2_);
if (prec1 > 1) {
k = Utility::lookup(digits_, gars[baselen_ + 1]);
if (!(k >= 1 /* && k <= mult3_ * mult3_ */))
throw GeographicErr("7th character in GARS must [1, 9] " + gars);
--k;
unit *= mult3_;
lat1 = mult3_ * lat1 + (mult3_ - 1 - k / mult3_);
lon1 = mult3_ * lon1 + (k % mult3_);
}
}
if (centerp) {
unit *= 2; lat1 = 2 * lat1 + 1; lon1 = 2 * lon1 + 1;
}
lat = lat1 / unit;
lon = lon1 / unit;
prec = prec1;
}
} // namespace GeographicLib

165
libs/geographiclib/src/GeoCoords.cpp Normal file
View File

@@ -0,0 +1,165 @@
/**
* \file GeoCoords.cpp
* \brief Implementation for GeographicLib::GeoCoords class
*
* Copyright (c) Charles Karney (2008-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/GeoCoords.hpp>
#include <GeographicLib/MGRS.hpp>
#include <GeographicLib/DMS.hpp>
#include <GeographicLib/Utility.hpp>
namespace GeographicLib {
using namespace std;
void GeoCoords::Reset(const std::string& s, bool centerp, bool longfirst) {
vector<string> sa;
const char* spaces = " \t\n\v\f\r,"; // Include comma as a space
for (string::size_type pos0 = 0, pos1; pos0 != string::npos;) {
pos1 = s.find_first_not_of(spaces, pos0);
if (pos1 == string::npos)
break;
pos0 = s.find_first_of(spaces, pos1);
sa.push_back(s.substr(pos1, pos0 == string::npos ? pos0 : pos0 - pos1));
}
if (sa.size() == 1) {
int prec;
MGRS::Reverse(sa[0], _zone, _northp, _easting, _northing, prec, centerp);
UTMUPS::Reverse(_zone, _northp, _easting, _northing,
_lat, _long, _gamma, _k);
} else if (sa.size() == 2) {
DMS::DecodeLatLon(sa[0], sa[1], _lat, _long, longfirst);
UTMUPS::Forward( _lat, _long,
_zone, _northp, _easting, _northing, _gamma, _k);
} else if (sa.size() == 3) {
unsigned zoneind, coordind;
if (sa[0].size() > 0 && isalpha(sa[0][sa[0].size() - 1])) {
zoneind = 0;
coordind = 1;
} else if (sa[2].size() > 0 && isalpha(sa[2][sa[2].size() - 1])) {
zoneind = 2;
coordind = 0;
} else
throw GeographicErr("Neither " + sa[0] + " nor " + sa[2]
+ " of the form UTM/UPS Zone + Hemisphere"
+ " (ex: 38n, 09s, n)");
UTMUPS::DecodeZone(sa[zoneind], _zone, _northp);
for (unsigned i = 0; i < 2; ++i)
(i ? _northing : _easting) = Utility::val<real>(sa[coordind + i]);
UTMUPS::Reverse(_zone, _northp, _easting, _northing,
_lat, _long, _gamma, _k);
FixHemisphere();
} else
throw GeographicErr("Coordinate requires 1, 2, or 3 elements");
CopyToAlt();
}
string GeoCoords::GeoRepresentation(int prec, bool longfirst) const {
using std::isnan; // Needed for Centos 7, ubuntu 14
prec = max(0, min(9 + Math::extra_digits(), prec) + 5);
return Utility::str(longfirst ? _long : _lat, prec) +
" " + Utility::str(longfirst ? _lat : _long, prec);
}
string GeoCoords::DMSRepresentation(int prec, bool longfirst,
char dmssep) const {
prec = max(0, min(10 + Math::extra_digits(), prec) + 5);
return DMS::Encode(longfirst ? _long : _lat, unsigned(prec),
longfirst ? DMS::LONGITUDE : DMS::LATITUDE, dmssep) +
" " + DMS::Encode(longfirst ? _lat : _long, unsigned(prec),
longfirst ? DMS::LATITUDE : DMS::LONGITUDE, dmssep);
}
string GeoCoords::MGRSRepresentation(int prec) const {
// Max precision is um
prec = max(-1, min(6, prec) + 5);
string mgrs;
MGRS::Forward(_zone, _northp, _easting, _northing, _lat, prec, mgrs);
return mgrs;
}
string GeoCoords::AltMGRSRepresentation(int prec) const {
// Max precision is um
prec = max(-1, min(6, prec) + 5);
string mgrs;
MGRS::Forward(_alt_zone, _northp, _alt_easting, _alt_northing, _lat, prec,
mgrs);
return mgrs;
}
void GeoCoords::UTMUPSString(int zone, bool northp,
real easting, real northing, int prec,
bool abbrev, string& utm) {
ostringstream os;
prec = max(-5, min(9 + Math::extra_digits(), prec));
// Need extra real because, since C++11, pow(float, int) returns double
real scale = prec < 0 ? real(pow(real(10), -prec)) : real(1);
os << UTMUPS::EncodeZone(zone, northp, abbrev) << fixed << setfill('0');
if (isfinite(easting)) {
os << " " << Utility::str(easting / scale, max(0, prec));
if (prec < 0 && fabs(easting / scale) > real(0.5))
os << setw(-prec) << 0;
} else
os << " nan";
if (isfinite(northing)) {
os << " " << Utility::str(northing / scale, max(0, prec));
if (prec < 0 && fabs(northing / scale) > real(0.5))
os << setw(-prec) << 0;
} else
os << " nan";
utm = os.str();
}
string GeoCoords::UTMUPSRepresentation(int prec, bool abbrev) const {
string utm;
UTMUPSString(_zone, _northp, _easting, _northing, prec, abbrev, utm);
return utm;
}
string GeoCoords::UTMUPSRepresentation(bool northp, int prec,
bool abbrev) const {
real e, n;
int z;
UTMUPS::Transfer(_zone, _northp, _easting, _northing,
_zone, northp, e, n, z);
string utm;
UTMUPSString(_zone, northp, e, n, prec, abbrev, utm);
return utm;
}
string GeoCoords::AltUTMUPSRepresentation(int prec, bool abbrev) const {
string utm;
UTMUPSString(_alt_zone, _northp, _alt_easting, _alt_northing, prec,
abbrev, utm);
return utm;
}
string GeoCoords::AltUTMUPSRepresentation(bool northp, int prec,
bool abbrev) const {
real e, n;
int z;
UTMUPS::Transfer(_alt_zone, _northp, _alt_easting, _alt_northing,
_alt_zone, northp, e, n, z);
string utm;
UTMUPSString(_alt_zone, northp, e, n, prec, abbrev, utm);
return utm;
}
void GeoCoords::FixHemisphere() {
using std::isnan; // Needed for Centos 7, ubuntu 14
if (_lat == 0 || (_northp && _lat >= 0) || (!_northp && _lat < 0) ||
isnan(_lat))
// Allow either hemisphere for equator
return;
if (_zone != UTMUPS::UPS) {
_northing += (_northp ? 1 : -1) * UTMUPS::UTMShift();
_northp = !_northp;
} else
throw GeographicErr("Hemisphere mixup");
}
} // namespace GeographicLib

172
libs/geographiclib/src/Geocentric.cpp Normal file
View File

@@ -0,0 +1,172 @@
/**
* \file Geocentric.cpp
* \brief Implementation for GeographicLib::Geocentric class
*
* Copyright (c) Charles Karney (2008-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/Geocentric.hpp>
namespace GeographicLib {
using namespace std;
Geocentric::Geocentric(real a, real f)
: _a(a)
, _f(f)
, _e2(_f * (2 - _f))
, _e2m(Math::sq(1 - _f)) // 1 - _e2
, _e2a(fabs(_e2))
, _e4a(Math::sq(_e2))
, _maxrad(2 * _a / numeric_limits<real>::epsilon())
{
if (!(isfinite(_a) && _a > 0))
throw GeographicErr("Equatorial radius is not positive");
if (!(isfinite(_f) && _f < 1))
throw GeographicErr("Polar semi-axis is not positive");
}
const Geocentric& Geocentric::WGS84() {
static const Geocentric wgs84(Constants::WGS84_a(), Constants::WGS84_f());
return wgs84;
}
void Geocentric::IntForward(real lat, real lon, real h,
real& X, real& Y, real& Z,
real M[dim2_]) const {
real sphi, cphi, slam, clam;
Math::sincosd(Math::LatFix(lat), sphi, cphi);
Math::sincosd(lon, slam, clam);
real n = _a/sqrt(1 - _e2 * Math::sq(sphi));
Z = (_e2m * n + h) * sphi;
X = (n + h) * cphi;
Y = X * slam;
X *= clam;
if (M)
Rotation(sphi, cphi, slam, clam, M);
}
void Geocentric::IntReverse(real X, real Y, real Z,
real& lat, real& lon, real& h,
real M[dim2_]) const {
real
R = hypot(X, Y),
slam = R != 0 ? Y / R : 0,
clam = R != 0 ? X / R : 1;
h = hypot(R, Z); // Distance to center of earth
real sphi, cphi;
if (h > _maxrad) {
// We really far away (> 12 million light years); treat the earth as a
// point and h, above, is an acceptable approximation to the height.
// This avoids overflow, e.g., in the computation of disc below. It's
// possible that h has overflowed to inf; but that's OK.
//
// Treat the case X, Y finite, but R overflows to +inf by scaling by 2.
R = hypot(X/2, Y/2);
slam = R != 0 ? (Y/2) / R : 0;
clam = R != 0 ? (X/2) / R : 1;
real H = hypot(Z/2, R);
sphi = (Z/2) / H;
cphi = R / H;
} else if (_e4a == 0) {
// Treat the spherical case. Dealing with underflow in the general case
// with _e2 = 0 is difficult. Origin maps to N pole same as with
// ellipsoid.
real H = hypot(h == 0 ? 1 : Z, R);
sphi = (h == 0 ? 1 : Z) / H;
cphi = R / H;
h -= _a;
} else {
// Treat prolate spheroids by swapping R and Z here and by switching
// the arguments to phi = atan2(...) at the end.
real
p = Math::sq(R / _a),
q = _e2m * Math::sq(Z / _a),
r = (p + q - _e4a) / 6;
if (_f < 0) swap(p, q);
if ( !(_e4a * q == 0 && r <= 0) ) {
real
// Avoid possible division by zero when r = 0 by multiplying
// equations for s and t by r^3 and r, resp.
S = _e4a * p * q / 4, // S = r^3 * s
r2 = Math::sq(r),
r3 = r * r2,
disc = S * (2 * r3 + S);
real u = r;
if (disc >= 0) {
real T3 = S + r3;
// Pick the sign on the sqrt to maximize abs(T3). This minimizes
// loss of precision due to cancellation. The result is unchanged
// because of the way the T is used in definition of u.
T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc); // T3 = (r * t)^3
// N.B. cbrt always returns the real root. cbrt(-8) = -2.
real T = cbrt(T3); // T = r * t
// T can be zero; but then r2 / T -> 0.
u += T + (T != 0 ? r2 / T : 0);
} else {
// T is complex, but the way u is defined the result is real.
real ang = atan2(sqrt(-disc), -(S + r3));
// There are three possible cube roots. We choose the root which
// avoids cancellation. Note that disc < 0 implies that r < 0.
u += 2 * r * cos(ang / 3);
}
real
v = sqrt(Math::sq(u) + _e4a * q), // guaranteed positive
// Avoid loss of accuracy when u < 0. Underflow doesn't occur in
// e4 * q / (v - u) because u ~ e^4 when q is small and u < 0.
uv = u < 0 ? _e4a * q / (v - u) : u + v, // u+v, guaranteed positive
// Need to guard against w going negative due to roundoff in uv - q.
w = fmax(real(0), _e2a * (uv - q) / (2 * v)),
// Rearrange expression for k to avoid loss of accuracy due to
// subtraction. Division by 0 not possible because uv > 0, w >= 0.
k = uv / (sqrt(uv + Math::sq(w)) + w),
k1 = _f >= 0 ? k : k - _e2,
k2 = _f >= 0 ? k + _e2 : k,
d = k1 * R / k2,
H = hypot(Z/k1, R/k2);
sphi = (Z/k1) / H;
cphi = (R/k2) / H;
h = (1 - _e2m/k1) * hypot(d, Z);
} else { // e4 * q == 0 && r <= 0
// This leads to k = 0 (oblate, equatorial plane) and k + e^2 = 0
// (prolate, rotation axis) and the generation of 0/0 in the general
// formulas for phi and h. using the general formula and division by 0
// in formula for h. So handle this case by taking the limits:
// f > 0: z -> 0, k -> e2 * sqrt(q)/sqrt(e4 - p)
// f < 0: R -> 0, k + e2 -> - e2 * sqrt(q)/sqrt(e4 - p)
real
zz = sqrt((_f >= 0 ? _e4a - p : p) / _e2m),
xx = sqrt( _f < 0 ? _e4a - p : p ),
H = hypot(zz, xx);
sphi = zz / H;
cphi = xx / H;
if (Z < 0) sphi = -sphi; // for tiny negative Z (not for prolate)
h = - _a * (_f >= 0 ? _e2m : 1) * H / _e2a;
}
}
lat = Math::atan2d(sphi, cphi);
lon = Math::atan2d(slam, clam);
if (M)
Rotation(sphi, cphi, slam, clam, M);
}
void Geocentric::Rotation(real sphi, real cphi, real slam, real clam,
real M[dim2_]) {
// This rotation matrix is given by the following quaternion operations
// qrot(lam, [0,0,1]) * qrot(phi, [0,-1,0]) * [1,1,1,1]/2
// or
// qrot(pi/2 + lam, [0,0,1]) * qrot(-pi/2 + phi , [-1,0,0])
// where
// qrot(t,v) = [cos(t/2), sin(t/2)*v[1], sin(t/2)*v[2], sin(t/2)*v[3]]
// Local X axis (east) in geocentric coords
M[0] = -slam; M[3] = clam; M[6] = 0;
// Local Y axis (north) in geocentric coords
M[1] = -clam * sphi; M[4] = -slam * sphi; M[7] = cphi;
// Local Z axis (up) in geocentric coords
M[2] = clam * cphi; M[5] = slam * cphi; M[8] = sphi;
}
} // namespace GeographicLib

1904
libs/geographiclib/src/Geodesic.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

1249
libs/geographiclib/src/GeodesicExact.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

326
libs/geographiclib/src/GeodesicLine.cpp Normal file
View File

@@ -0,0 +1,326 @@
/**
* \file GeodesicLine.cpp
* \brief Implementation for GeographicLib::GeodesicLine class
*
* Copyright (c) Charles Karney (2009-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
*
* This is a reformulation of the geodesic problem. The notation is as
* follows:
* - at a general point (no suffix or 1 or 2 as suffix)
* - phi = latitude
* - beta = latitude on auxiliary sphere
* - omega = longitude on auxiliary sphere
* - lambda = longitude
* - alpha = azimuth of great circle
* - sigma = arc length along great circle
* - s = distance
* - tau = scaled distance (= sigma at multiples of pi/2)
* - at northwards equator crossing
* - beta = phi = 0
* - omega = lambda = 0
* - alpha = alpha0
* - sigma = s = 0
* - a 12 suffix means a difference, e.g., s12 = s2 - s1.
* - s and c prefixes mean sin and cos
**********************************************************************/
#include <GeographicLib/GeodesicLine.hpp>
#if defined(_MSC_VER)
// Squelch warnings about mixing enums
# pragma warning (disable: 5054)
#endif
namespace GeographicLib {
using namespace std;
void GeodesicLine::LineInit(const Geodesic& g,
real lat1, real lon1,
real azi1, real salp1, real calp1,
unsigned caps) {
tiny_ = g.tiny_;
_lat1 = Math::LatFix(lat1);
_lon1 = lon1;
_azi1 = azi1;
_salp1 = salp1;
_calp1 = calp1;
_a = g._a;
_f = g._f;
_b = g._b;
_c2 = g._c2;
_f1 = g._f1;
// Always allow latitude and azimuth and unrolling of longitude
_caps = caps | LATITUDE | AZIMUTH | LONG_UNROLL;
real cbet1, sbet1;
Math::sincosd(Math::AngRound(_lat1), sbet1, cbet1); sbet1 *= _f1;
// Ensure cbet1 = +epsilon at poles
Math::norm(sbet1, cbet1); cbet1 = fmax(tiny_, cbet1);
_dn1 = sqrt(1 + g._ep2 * Math::sq(sbet1));
// Evaluate alp0 from sin(alp1) * cos(bet1) = sin(alp0),
_salp0 = _salp1 * cbet1; // alp0 in [0, pi/2 - |bet1|]
// Alt: calp0 = hypot(sbet1, calp1 * cbet1). The following
// is slightly better (consider the case salp1 = 0).
_calp0 = hypot(_calp1, _salp1 * sbet1);
// Evaluate sig with tan(bet1) = tan(sig1) * cos(alp1).
// sig = 0 is nearest northward crossing of equator.
// With bet1 = 0, alp1 = pi/2, we have sig1 = 0 (equatorial line).
// With bet1 = pi/2, alp1 = -pi, sig1 = pi/2
// With bet1 = -pi/2, alp1 = 0 , sig1 = -pi/2
// Evaluate omg1 with tan(omg1) = sin(alp0) * tan(sig1).
// With alp0 in (0, pi/2], quadrants for sig and omg coincide.
// No atan2(0,0) ambiguity at poles since cbet1 = +epsilon.
// With alp0 = 0, omg1 = 0 for alp1 = 0, omg1 = pi for alp1 = pi.
_ssig1 = sbet1; _somg1 = _salp0 * sbet1;
_csig1 = _comg1 = sbet1 != 0 || _calp1 != 0 ? cbet1 * _calp1 : 1;
Math::norm(_ssig1, _csig1); // sig1 in (-pi, pi]
// Math::norm(_somg1, _comg1); -- don't need to normalize!
_k2 = Math::sq(_calp0) * g._ep2;
real eps = _k2 / (2 * (1 + sqrt(1 + _k2)) + _k2);
if (_caps & CAP_C1) {
_aA1m1 = Geodesic::A1m1f(eps);
Geodesic::C1f(eps, _cC1a);
_bB11 = Geodesic::SinCosSeries(true, _ssig1, _csig1, _cC1a, nC1_);
real s = sin(_bB11), c = cos(_bB11);
// tau1 = sig1 + B11
_stau1 = _ssig1 * c + _csig1 * s;
_ctau1 = _csig1 * c - _ssig1 * s;
// Not necessary because C1pa reverts C1a
// _bB11 = -SinCosSeries(true, _stau1, _ctau1, _cC1pa, nC1p_);
}
if (_caps & CAP_C1p)
Geodesic::C1pf(eps, _cC1pa);
if (_caps & CAP_C2) {
_aA2m1 = Geodesic::A2m1f(eps);
Geodesic::C2f(eps, _cC2a);
_bB21 = Geodesic::SinCosSeries(true, _ssig1, _csig1, _cC2a, nC2_);
}
if (_caps & CAP_C3) {
g.C3f(eps, _cC3a);
_aA3c = -_f * _salp0 * g.A3f(eps);
_bB31 = Geodesic::SinCosSeries(true, _ssig1, _csig1, _cC3a, nC3_-1);
}
if (_caps & CAP_C4) {
g.C4f(eps, _cC4a);
// Multiplier = a^2 * e^2 * cos(alpha0) * sin(alpha0)
_aA4 = Math::sq(_a) * _calp0 * _salp0 * g._e2;
_bB41 = Geodesic::SinCosSeries(false, _ssig1, _csig1, _cC4a, nC4_);
}
_a13 = _s13 = Math::NaN();
}
GeodesicLine::GeodesicLine(const Geodesic& g,
real lat1, real lon1, real azi1,
unsigned caps) {
azi1 = Math::AngNormalize(azi1);
real salp1, calp1;
// Guard against underflow in salp0. Also -0 is converted to +0.
Math::sincosd(Math::AngRound(azi1), salp1, calp1);
LineInit(g, lat1, lon1, azi1, salp1, calp1, caps);
}
GeodesicLine::GeodesicLine(const Geodesic& g,
real lat1, real lon1,
real azi1, real salp1, real calp1,
unsigned caps, bool arcmode, real s13_a13) {
LineInit(g, lat1, lon1, azi1, salp1, calp1, caps);
GenSetDistance(arcmode, s13_a13);
}
Math::real GeodesicLine::GenPosition(bool arcmode, real s12_a12,
unsigned outmask,
real& lat2, real& lon2, real& azi2,
real& s12, real& m12,
real& M12, real& M21,
real& S12) const {
outmask &= _caps & OUT_MASK;
if (!( Init() && (arcmode || (_caps & (OUT_MASK & DISTANCE_IN))) ))
// Uninitialized or impossible distance calculation requested
return Math::NaN();
// Avoid warning about uninitialized B12.
real sig12, ssig12, csig12, B12 = 0, AB1 = 0;
if (arcmode) {
// Interpret s12_a12 as spherical arc length
sig12 = s12_a12 * Math::degree();
Math::sincosd(s12_a12, ssig12, csig12);
} else {
// Interpret s12_a12 as distance
real
tau12 = s12_a12 / (_b * (1 + _aA1m1)),
s = sin(tau12),
c = cos(tau12);
// tau2 = tau1 + tau12
B12 = - Geodesic::SinCosSeries(true,
_stau1 * c + _ctau1 * s,
_ctau1 * c - _stau1 * s,
_cC1pa, nC1p_);
sig12 = tau12 - (B12 - _bB11);
ssig12 = sin(sig12); csig12 = cos(sig12);
if (fabs(_f) > 0.01) {
// Reverted distance series is inaccurate for |f| > 1/100, so correct
// sig12 with 1 Newton iteration. The following table shows the
// approximate maximum error for a = WGS_a() and various f relative to
// GeodesicExact.
// erri = the error in the inverse solution (nm)
// errd = the error in the direct solution (series only) (nm)
// errda = the error in the direct solution
// (series + 1 Newton) (nm)
//
// f erri errd errda
// -1/5 12e6 1.2e9 69e6
// -1/10 123e3 12e6 765e3
// -1/20 1110 108e3 7155
// -1/50 18.63 200.9 27.12
// -1/100 18.63 23.78 23.37
// -1/150 18.63 21.05 20.26
// 1/150 22.35 24.73 25.83
// 1/100 22.35 25.03 25.31
// 1/50 29.80 231.9 30.44
// 1/20 5376 146e3 10e3
// 1/10 829e3 22e6 1.5e6
// 1/5 157e6 3.8e9 280e6
real
ssig2 = _ssig1 * csig12 + _csig1 * ssig12,
csig2 = _csig1 * csig12 - _ssig1 * ssig12;
B12 = Geodesic::SinCosSeries(true, ssig2, csig2, _cC1a, nC1_);
real serr = (1 + _aA1m1) * (sig12 + (B12 - _bB11)) - s12_a12 / _b;
sig12 = sig12 - serr / sqrt(1 + _k2 * Math::sq(ssig2));
ssig12 = sin(sig12); csig12 = cos(sig12);
// Update B12 below
}
}
real ssig2, csig2, sbet2, cbet2, salp2, calp2;
// sig2 = sig1 + sig12
ssig2 = _ssig1 * csig12 + _csig1 * ssig12;
csig2 = _csig1 * csig12 - _ssig1 * ssig12;
real dn2 = sqrt(1 + _k2 * Math::sq(ssig2));
if (outmask & (DISTANCE | REDUCEDLENGTH | GEODESICSCALE)) {
if (arcmode || fabs(_f) > 0.01)
B12 = Geodesic::SinCosSeries(true, ssig2, csig2, _cC1a, nC1_);
AB1 = (1 + _aA1m1) * (B12 - _bB11);
}
// sin(bet2) = cos(alp0) * sin(sig2)
sbet2 = _calp0 * ssig2;
// Alt: cbet2 = hypot(csig2, salp0 * ssig2);
cbet2 = hypot(_salp0, _calp0 * csig2);
if (cbet2 == 0)
// I.e., salp0 = 0, csig2 = 0. Break the degeneracy in this case
cbet2 = csig2 = tiny_;
// tan(alp0) = cos(sig2)*tan(alp2)
salp2 = _salp0; calp2 = _calp0 * csig2; // No need to normalize
if (outmask & DISTANCE)
s12 = arcmode ? _b * ((1 + _aA1m1) * sig12 + AB1) : s12_a12;
if (outmask & LONGITUDE) {
// tan(omg2) = sin(alp0) * tan(sig2)
real somg2 = _salp0 * ssig2, comg2 = csig2, // No need to normalize
E = copysign(real(1), _salp0); // east-going?
// omg12 = omg2 - omg1
real omg12 = outmask & LONG_UNROLL
? E * (sig12
- (atan2( ssig2, csig2) - atan2( _ssig1, _csig1))
+ (atan2(E * somg2, comg2) - atan2(E * _somg1, _comg1)))
: atan2(somg2 * _comg1 - comg2 * _somg1,
comg2 * _comg1 + somg2 * _somg1);
real lam12 = omg12 + _aA3c *
( sig12 + (Geodesic::SinCosSeries(true, ssig2, csig2, _cC3a, nC3_-1)
- _bB31));
real lon12 = lam12 / Math::degree();
lon2 = outmask & LONG_UNROLL ? _lon1 + lon12 :
Math::AngNormalize(Math::AngNormalize(_lon1) +
Math::AngNormalize(lon12));
}
if (outmask & LATITUDE)
lat2 = Math::atan2d(sbet2, _f1 * cbet2);
if (outmask & AZIMUTH)
azi2 = Math::atan2d(salp2, calp2);
if (outmask & (REDUCEDLENGTH | GEODESICSCALE)) {
real
B22 = Geodesic::SinCosSeries(true, ssig2, csig2, _cC2a, nC2_),
AB2 = (1 + _aA2m1) * (B22 - _bB21),
J12 = (_aA1m1 - _aA2m1) * sig12 + (AB1 - AB2);
if (outmask & REDUCEDLENGTH)
// Add parens around (_csig1 * ssig2) and (_ssig1 * csig2) to ensure
// accurate cancellation in the case of coincident points.
m12 = _b * ((dn2 * (_csig1 * ssig2) - _dn1 * (_ssig1 * csig2))
- _csig1 * csig2 * J12);
if (outmask & GEODESICSCALE) {
real t = _k2 * (ssig2 - _ssig1) * (ssig2 + _ssig1) / (_dn1 + dn2);
M12 = csig12 + (t * ssig2 - csig2 * J12) * _ssig1 / _dn1;
M21 = csig12 - (t * _ssig1 - _csig1 * J12) * ssig2 / dn2;
}
}
if (outmask & AREA) {
real
B42 = Geodesic::SinCosSeries(false, ssig2, csig2, _cC4a, nC4_);
real salp12, calp12;
if (_calp0 == 0 || _salp0 == 0) {
// alp12 = alp2 - alp1, used in atan2 so no need to normalize
salp12 = salp2 * _calp1 - calp2 * _salp1;
calp12 = calp2 * _calp1 + salp2 * _salp1;
// We used to include here some patch up code that purported to deal
// with nearly meridional geodesics properly. However, this turned out
// to be wrong once _salp1 = -0 was allowed (via
// Geodesic::InverseLine). In fact, the calculation of {s,c}alp12
// was already correct (following the IEEE rules for handling signed
// zeros). So the patch up code was unnecessary (as well as
// dangerous).
} else {
// tan(alp) = tan(alp0) * sec(sig)
// tan(alp2-alp1) = (tan(alp2) -tan(alp1)) / (tan(alp2)*tan(alp1)+1)
// = calp0 * salp0 * (csig1-csig2) / (salp0^2 + calp0^2 * csig1*csig2)
// If csig12 > 0, write
// csig1 - csig2 = ssig12 * (csig1 * ssig12 / (1 + csig12) + ssig1)
// else
// csig1 - csig2 = csig1 * (1 - csig12) + ssig12 * ssig1
// No need to normalize
salp12 = _calp0 * _salp0 *
(csig12 <= 0 ? _csig1 * (1 - csig12) + ssig12 * _ssig1 :
ssig12 * (_csig1 * ssig12 / (1 + csig12) + _ssig1));
calp12 = Math::sq(_salp0) + Math::sq(_calp0) * _csig1 * csig2;
}
S12 = _c2 * atan2(salp12, calp12) + _aA4 * (B42 - _bB41);
}
return arcmode ? s12_a12 : sig12 / Math::degree();
}
void GeodesicLine::SetDistance(real s13) {
_s13 = s13;
real t;
// This will set _a13 to NaN if the GeodesicLine doesn't have the
// DISTANCE_IN capability.
_a13 = GenPosition(false, _s13, 0u, t, t, t, t, t, t, t, t);
}
void GeodesicLine::SetArc(real a13) {
_a13 = a13;
// In case the GeodesicLine doesn't have the DISTANCE capability.
_s13 = Math::NaN();
real t;
GenPosition(true, _a13, DISTANCE, t, t, t, _s13, t, t, t, t);
}
void GeodesicLine::GenSetDistance(bool arcmode, real s13_a13) {
arcmode ? SetArc(s13_a13) : SetDistance(s13_a13);
}
} // namespace GeographicLib

297
libs/geographiclib/src/GeodesicLineExact.cpp Normal file
View File

@@ -0,0 +1,297 @@
/**
* \file GeodesicLineExact.cpp
* \brief Implementation for GeographicLib::GeodesicLineExact class
*
* Copyright (c) Charles Karney (2012-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
*
* This is a reformulation of the geodesic problem. The notation is as
* follows:
* - at a general point (no suffix or 1 or 2 as suffix)
* - phi = latitude
* - beta = latitude on auxiliary sphere
* - omega = longitude on auxiliary sphere
* - lambda = longitude
* - alpha = azimuth of great circle
* - sigma = arc length along great circle
* - s = distance
* - tau = scaled distance (= sigma at multiples of pi/2)
* - at northwards equator crossing
* - beta = phi = 0
* - omega = lambda = 0
* - alpha = alpha0
* - sigma = s = 0
* - a 12 suffix means a difference, e.g., s12 = s2 - s1.
* - s and c prefixes mean sin and cos
**********************************************************************/
#include <GeographicLib/GeodesicLineExact.hpp>
#if defined(_MSC_VER)
// Squelch warnings about mixing enums
# pragma warning (disable: 5054)
#endif
namespace GeographicLib {
using namespace std;
void GeodesicLineExact::LineInit(const GeodesicExact& g,
real lat1, real lon1,
real azi1, real salp1, real calp1,
unsigned caps) {
tiny_ = g.tiny_;
_lat1 = Math::LatFix(lat1);
_lon1 = lon1;
_azi1 = azi1;
_salp1 = salp1;
_calp1 = calp1;
_a = g._a;
_f = g._f;
_b = g._b;
_c2 = g._c2;
_f1 = g._f1;
_e2 = g._e2;
_nC4 = g._nC4;
// Always allow latitude and azimuth and unrolling of longitude
_caps = caps | LATITUDE | AZIMUTH | LONG_UNROLL;
real cbet1, sbet1;
Math::sincosd(Math::AngRound(_lat1), sbet1, cbet1); sbet1 *= _f1;
// Ensure cbet1 = +epsilon at poles
Math::norm(sbet1, cbet1); cbet1 = fmax(tiny_, cbet1);
_dn1 = (_f >= 0 ? sqrt(1 + g._ep2 * Math::sq(sbet1)) :
sqrt(1 - _e2 * Math::sq(cbet1)) / _f1);
// Evaluate alp0 from sin(alp1) * cos(bet1) = sin(alp0),
_salp0 = _salp1 * cbet1; // alp0 in [0, pi/2 - |bet1|]
// Alt: calp0 = hypot(sbet1, calp1 * cbet1). The following
// is slightly better (consider the case salp1 = 0).
_calp0 = hypot(_calp1, _salp1 * sbet1);
// Evaluate sig with tan(bet1) = tan(sig1) * cos(alp1).
// sig = 0 is nearest northward crossing of equator.
// With bet1 = 0, alp1 = pi/2, we have sig1 = 0 (equatorial line).
// With bet1 = pi/2, alp1 = -pi, sig1 = pi/2
// With bet1 = -pi/2, alp1 = 0 , sig1 = -pi/2
// Evaluate omg1 with tan(omg1) = sin(alp0) * tan(sig1).
// With alp0 in (0, pi/2], quadrants for sig and omg coincide.
// No atan2(0,0) ambiguity at poles since cbet1 = +epsilon.
// With alp0 = 0, omg1 = 0 for alp1 = 0, omg1 = pi for alp1 = pi.
_ssig1 = sbet1; _somg1 = _salp0 * sbet1;
_csig1 = _comg1 = sbet1 != 0 || _calp1 != 0 ? cbet1 * _calp1 : 1;
// Without normalization we have schi1 = somg1.
_cchi1 = _f1 * _dn1 * _comg1;
Math::norm(_ssig1, _csig1); // sig1 in (-pi, pi]
// Math::norm(_somg1, _comg1); -- don't need to normalize!
// Math::norm(_schi1, _cchi1); -- don't need to normalize!
_k2 = Math::sq(_calp0) * g._ep2;
_eE.Reset(-_k2, -g._ep2, 1 + _k2, 1 + g._ep2);
if (_caps & CAP_E) {
_eE0 = _eE.E() / (Math::pi() / 2);
_eE1 = _eE.deltaE(_ssig1, _csig1, _dn1);
real s = sin(_eE1), c = cos(_eE1);
// tau1 = sig1 + B11
_stau1 = _ssig1 * c + _csig1 * s;
_ctau1 = _csig1 * c - _ssig1 * s;
// Not necessary because Einv inverts E
// _eE1 = -_eE.deltaEinv(_stau1, _ctau1);
}
if (_caps & CAP_D) {
_dD0 = _eE.D() / (Math::pi() / 2);
_dD1 = _eE.deltaD(_ssig1, _csig1, _dn1);
}
if (_caps & CAP_H) {
_hH0 = _eE.H() / (Math::pi() / 2);
_hH1 = _eE.deltaH(_ssig1, _csig1, _dn1);
}
if (_caps & CAP_C4) {
// Multiplier = a^2 * e^2 * cos(alpha0) * sin(alpha0)
_aA4 = Math::sq(_a) * _calp0 * _salp0 * _e2;
if (_aA4 == 0)
_bB41 = 0;
else {
GeodesicExact::I4Integrand i4(g._ep2, _k2);
_cC4a.resize(_nC4);
g._fft.transform(i4, _cC4a.data());
_bB41 = DST::integral(_ssig1, _csig1, _cC4a.data(), _nC4);
}
}
_a13 = _s13 = Math::NaN();
}
GeodesicLineExact::GeodesicLineExact(const GeodesicExact& g,
real lat1, real lon1, real azi1,
unsigned caps) {
azi1 = Math::AngNormalize(azi1);
real salp1, calp1;
// Guard against underflow in salp0. Also -0 is converted to +0.
Math::sincosd(Math::AngRound(azi1), salp1, calp1);
LineInit(g, lat1, lon1, azi1, salp1, calp1, caps);
}
GeodesicLineExact::GeodesicLineExact(const GeodesicExact& g,
real lat1, real lon1,
real azi1, real salp1, real calp1,
unsigned caps,
bool arcmode, real s13_a13) {
LineInit(g, lat1, lon1, azi1, salp1, calp1, caps);
GenSetDistance(arcmode, s13_a13);
}
Math::real GeodesicLineExact::GenPosition(bool arcmode, real s12_a12,
unsigned outmask,
real& lat2, real& lon2, real& azi2,
real& s12, real& m12,
real& M12, real& M21,
real& S12) const {
outmask &= _caps & OUT_MASK;
if (!( Init() && (arcmode || (_caps & (OUT_MASK & DISTANCE_IN))) ))
// Uninitialized or impossible distance calculation requested
return Math::NaN();
// Avoid warning about uninitialized B12.
real sig12, ssig12, csig12, E2 = 0, AB1 = 0;
if (arcmode) {
// Interpret s12_a12 as spherical arc length
sig12 = s12_a12 * Math::degree();
Math::sincosd(s12_a12, ssig12, csig12);
} else {
// Interpret s12_a12 as distance
real
tau12 = s12_a12 / (_b * _eE0),
s = sin(tau12),
c = cos(tau12);
// tau2 = tau1 + tau12
E2 = - _eE.deltaEinv(_stau1 * c + _ctau1 * s, _ctau1 * c - _stau1 * s);
sig12 = tau12 - (E2 - _eE1);
ssig12 = sin(sig12);
csig12 = cos(sig12);
}
real ssig2, csig2, sbet2, cbet2, salp2, calp2;
// sig2 = sig1 + sig12
ssig2 = _ssig1 * csig12 + _csig1 * ssig12;
csig2 = _csig1 * csig12 - _ssig1 * ssig12;
real dn2 = _eE.Delta(ssig2, csig2);
if (outmask & (DISTANCE | REDUCEDLENGTH | GEODESICSCALE)) {
if (arcmode) {
E2 = _eE.deltaE(ssig2, csig2, dn2);
}
AB1 = _eE0 * (E2 - _eE1);
}
// sin(bet2) = cos(alp0) * sin(sig2)
sbet2 = _calp0 * ssig2;
// Alt: cbet2 = hypot(csig2, salp0 * ssig2);
cbet2 = hypot(_salp0, _calp0 * csig2);
if (cbet2 == 0)
// I.e., salp0 = 0, csig2 = 0. Break the degeneracy in this case
cbet2 = csig2 = tiny_;
// tan(alp0) = cos(sig2)*tan(alp2)
salp2 = _salp0; calp2 = _calp0 * csig2; // No need to normalize
if (outmask & DISTANCE)
s12 = arcmode ? _b * (_eE0 * sig12 + AB1) : s12_a12;
if (outmask & LONGITUDE) {
real somg2 = _salp0 * ssig2, comg2 = csig2, // No need to normalize
E = copysign(real(1), _salp0); // east-going?
// Without normalization we have schi2 = somg2.
real cchi2 = _f1 * dn2 * comg2;
real chi12 = outmask & LONG_UNROLL
? E * (sig12
- (atan2( ssig2, csig2) - atan2( _ssig1, _csig1))
+ (atan2(E * somg2, cchi2) - atan2(E * _somg1, _cchi1)))
: atan2(somg2 * _cchi1 - cchi2 * _somg1,
cchi2 * _cchi1 + somg2 * _somg1);
real lam12 = chi12 -
_e2/_f1 * _salp0 * _hH0 *
(sig12 + (_eE.deltaH(ssig2, csig2, dn2) - _hH1));
real lon12 = lam12 / Math::degree();
lon2 = outmask & LONG_UNROLL ? _lon1 + lon12 :
Math::AngNormalize(Math::AngNormalize(_lon1) +
Math::AngNormalize(lon12));
}
if (outmask & LATITUDE)
lat2 = Math::atan2d(sbet2, _f1 * cbet2);
if (outmask & AZIMUTH)
azi2 = Math::atan2d(salp2, calp2);
if (outmask & (REDUCEDLENGTH | GEODESICSCALE)) {
real J12 = _k2 * _dD0 * (sig12 + (_eE.deltaD(ssig2, csig2, dn2) - _dD1));
if (outmask & REDUCEDLENGTH)
// Add parens around (_csig1 * ssig2) and (_ssig1 * csig2) to ensure
// accurate cancellation in the case of coincident points.
m12 = _b * ((dn2 * (_csig1 * ssig2) - _dn1 * (_ssig1 * csig2))
- _csig1 * csig2 * J12);
if (outmask & GEODESICSCALE) {
real t = _k2 * (ssig2 - _ssig1) * (ssig2 + _ssig1) / (_dn1 + dn2);
M12 = csig12 + (t * ssig2 - csig2 * J12) * _ssig1 / _dn1;
M21 = csig12 - (t * _ssig1 - _csig1 * J12) * ssig2 / dn2;
}
}
if (outmask & AREA) {
real B42 = _aA4 == 0 ? 0 :
DST::integral(ssig2, csig2, _cC4a.data(), _nC4);
real salp12, calp12;
if (_calp0 == 0 || _salp0 == 0) {
// alp12 = alp2 - alp1, used in atan2 so no need to normalize
salp12 = salp2 * _calp1 - calp2 * _salp1;
calp12 = calp2 * _calp1 + salp2 * _salp1;
// We used to include here some patch up code that purported to deal
// with nearly meridional geodesics properly. However, this turned out
// to be wrong once _salp1 = -0 was allowed (via
// GeodesicExact::InverseLine). In fact, the calculation of {s,c}alp12
// was already correct (following the IEEE rules for handling signed
// zeros). So the patch up code was unnecessary (as well as
// dangerous).
} else {
// tan(alp) = tan(alp0) * sec(sig)
// tan(alp2-alp1) = (tan(alp2) -tan(alp1)) / (tan(alp2)*tan(alp1)+1)
// = calp0 * salp0 * (csig1-csig2) / (salp0^2 + calp0^2 * csig1*csig2)
// If csig12 > 0, write
// csig1 - csig2 = ssig12 * (csig1 * ssig12 / (1 + csig12) + ssig1)
// else
// csig1 - csig2 = csig1 * (1 - csig12) + ssig12 * ssig1
// No need to normalize
salp12 = _calp0 * _salp0 *
(csig12 <= 0 ? _csig1 * (1 - csig12) + ssig12 * _ssig1 :
ssig12 * (_csig1 * ssig12 / (1 + csig12) + _ssig1));
calp12 = Math::sq(_salp0) + Math::sq(_calp0) * _csig1 * csig2;
}
S12 = _c2 * atan2(salp12, calp12) + _aA4 * (B42 - _bB41);
}
return arcmode ? s12_a12 : sig12 / Math::degree();
}
void GeodesicLineExact::SetDistance(real s13) {
_s13 = s13;
real t;
// This will set _a13 to NaN if the GeodesicLineExact doesn't have the
// DISTANCE_IN capability.
_a13 = GenPosition(false, _s13, 0u, t, t, t, t, t, t, t, t);
}
void GeodesicLineExact::SetArc(real a13) {
_a13 = a13;
// In case the GeodesicLineExact doesn't have the DISTANCE capability.
_s13 = Math::NaN();
real t;
GenPosition(true, _a13, DISTANCE, t, t, t, _s13, t, t, t, t);
}
void GeodesicLineExact::GenSetDistance(bool arcmode, real s13_a13) {
arcmode ? SetArc(s13_a13) : SetDistance(s13_a13);
}
} // namespace GeographicLib

110
libs/geographiclib/src/Geohash.cpp Normal file
View File

@@ -0,0 +1,110 @@
/**
* \file Geohash.cpp
* \brief Implementation for GeographicLib::Geohash class
*
* Copyright (c) Charles Karney (2012-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/Geohash.hpp>
#include <GeographicLib/Utility.hpp>
#if defined(_MSC_VER)
// Squelch warnings about enum-float expressions
# pragma warning (disable: 5055)
#endif
namespace GeographicLib {
using namespace std;
const char* const Geohash::lcdigits_ = "0123456789bcdefghjkmnpqrstuvwxyz";
const char* const Geohash::ucdigits_ = "0123456789BCDEFGHJKMNPQRSTUVWXYZ";
void Geohash::Forward(real lat, real lon, int len, string& geohash) {
using std::isnan; // Needed for Centos 7, ubuntu 14
static const real shift = ldexp(real(1), 45);
static const real loneps = Math::hd / shift;
static const real lateps = Math::qd / shift;
if (fabs(lat) > Math::qd)
throw GeographicErr("Latitude " + Utility::str(lat)
+ "d not in [-" + to_string(Math::qd)
+ "d, " + to_string(Math::qd) + "d]");
if (isnan(lat) || isnan(lon)) {
geohash = "invalid";
return;
}
if (lat == Math::qd) lat -= lateps / 2;
lon = Math::AngNormalize(lon);
if (lon == Math::hd) lon = -Math::hd; // lon now in [-180,180)
// lon/loneps in [-2^45,2^45); lon/loneps + shift in [0,2^46)
// similarly for lat
len = max(0, min(int(maxlen_), len));
unsigned long long
ulon = (unsigned long long)(floor(lon/loneps) + shift),
ulat = (unsigned long long)(floor(lat/lateps) + shift);
char geohash1[maxlen_];
unsigned byte = 0;
for (unsigned i = 0; i < 5 * unsigned(len);) {
if ((i & 1) == 0) {
byte = (byte << 1) + unsigned((ulon & mask_) != 0);
ulon <<= 1;
} else {
byte = (byte << 1) + unsigned((ulat & mask_) != 0);
ulat <<= 1;
}
++i;
if (i % 5 == 0) {
geohash1[(i/5)-1] = lcdigits_[byte];
byte = 0;
}
}
geohash.resize(len);
copy(geohash1, geohash1 + len, geohash.begin());
}
void Geohash::Reverse(const string& geohash, real& lat, real& lon,
int& len, bool centerp) {
static const real shift = ldexp(real(1), 45);
static const real loneps = Math::hd / shift;
static const real lateps = Math::qd / shift;
int len1 = min(int(maxlen_), int(geohash.length()));
if (len1 >= 3 &&
((toupper(geohash[0]) == 'I' &&
toupper(geohash[1]) == 'N' &&
toupper(geohash[2]) == 'V') ||
// Check A first because it is not in a standard geohash
(toupper(geohash[1]) == 'A' &&
toupper(geohash[0]) == 'N' &&
toupper(geohash[2]) == 'N'))) {
lat = lon = Math::NaN();
return;
}
unsigned long long ulon = 0, ulat = 0;
for (unsigned k = 0, j = 0; k < unsigned(len1); ++k) {
int byte = Utility::lookup(ucdigits_, geohash[k]);
if (byte < 0)
throw GeographicErr("Illegal character in geohash " + geohash);
for (unsigned m = 16; m; m >>= 1) {
if (j == 0)
ulon = (ulon << 1) + unsigned((byte & m) != 0);
else
ulat = (ulat << 1) + unsigned((byte & m) != 0);
j ^= 1;
}
}
ulon <<= 1; ulat <<= 1;
if (centerp) {
ulon += 1;
ulat += 1;
}
int s = 5 * (maxlen_ - len1);
ulon <<= (s / 2);
ulat <<= s - (s / 2);
lon = ulon * loneps - Math::hd;
lat = ulat * lateps - Math::qd;
len = len1;
}
} // namespace GeographicLib

510
libs/geographiclib/src/Geoid.cpp Normal file
View File

@@ -0,0 +1,510 @@
/**
* \file Geoid.cpp
* \brief Implementation for GeographicLib::Geoid class
*
* Copyright (c) Charles Karney (2009-2020) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/Geoid.hpp>
// For getenv
#include <cstdlib>
#include <GeographicLib/Utility.hpp>
#if !defined(GEOGRAPHICLIB_DATA)
# if defined(_WIN32)
# define GEOGRAPHICLIB_DATA "C:/ProgramData/GeographicLib"
# else
# define GEOGRAPHICLIB_DATA "/usr/local/share/GeographicLib"
# endif
#endif
#if !defined(GEOGRAPHICLIB_GEOID_DEFAULT_NAME)
# define GEOGRAPHICLIB_GEOID_DEFAULT_NAME "egm96-5"
#endif
#if defined(_MSC_VER)
// Squelch warnings about unsafe use of getenv and enum-float expressions
# pragma warning (disable: 4996 5055)
#endif
namespace GeographicLib {
using namespace std;
// This is the transfer matrix for a 3rd order fit with a 12-point stencil
// with weights
//
// \x -1 0 1 2
// y
// -1 . 1 1 .
// 0 1 2 2 1
// 1 1 2 2 1
// 2 . 1 1 .
//
// A algorithm for n-dimensional polynomial fits is described in
// F. H. Lesh,
// Multi-dimensional least-squares polynomial curve fitting,
// CACM 2, 29-30 (1959).
// https://doi.org/10.1145/368424.368443
//
// Here's the Maxima code to generate this matrix:
//
// /* The stencil and the weights */
// xarr:[
// 0, 1,
// -1, 0, 1, 2,
// -1, 0, 1, 2,
// 0, 1]$
// yarr:[
// -1,-1,
// 0, 0, 0, 0,
// 1, 1, 1, 1,
// 2, 2]$
// warr:[
// 1, 1,
// 1, 2, 2, 1,
// 1, 2, 2, 1,
// 1, 1]$
//
// /* [x exponent, y exponent] for cubic fit */
// pows:[
// [0,0],
// [1,0],[0,1],
// [2,0],[1,1],[0,2],
// [3,0],[2,1],[1,2],[0,3]]$
//
// basisvec(x,y,pows):=map(lambda([ex],(if ex[1]=0 then 1 else x^ex[1])*
// (if ex[2]=0 then 1 else y^ex[2])),pows)$
// addterm(x,y,f,w,pows):=block([a,b,bb:basisvec(x,y,pows)],
// a:w*(transpose(bb).bb),
// b:(w*f) * bb,
// [a,b])$
//
// c3row(k):=block([a,b,c,pows:pows,n],
// n:length(pows),
// a:zeromatrix(n,n),
// b:copylist(part(a,1)),
// c:[a,b],
// for i:1 thru length(xarr) do
// c:c+addterm(xarr[i],yarr[i],if i=k then 1 else 0,warr[i],pows),
// a:c[1],b:c[2],
// part(transpose( a^^-1 . transpose(b)),1))$
// c3:[]$
// for k:1 thru length(warr) do c3:endcons(c3row(k),c3)$
// c3:apply(matrix,c3)$
// c0:part(ratsimp(
// genmatrix(yc,1,length(warr)).abs(c3).genmatrix(yd,length(pows),1)),2)$
// c3:c0*c3$
const int Geoid::c0_ = 240; // Common denominator
const int Geoid::c3_[stencilsize_ * nterms_] = {
9, -18, -88, 0, 96, 90, 0, 0, -60, -20,
-9, 18, 8, 0, -96, 30, 0, 0, 60, -20,
9, -88, -18, 90, 96, 0, -20, -60, 0, 0,
186, -42, -42, -150, -96, -150, 60, 60, 60, 60,
54, 162, -78, 30, -24, -90, -60, 60, -60, 60,
-9, -32, 18, 30, 24, 0, 20, -60, 0, 0,
-9, 8, 18, 30, -96, 0, -20, 60, 0, 0,
54, -78, 162, -90, -24, 30, 60, -60, 60, -60,
-54, 78, 78, 90, 144, 90, -60, -60, -60, -60,
9, -8, -18, -30, -24, 0, 20, 60, 0, 0,
-9, 18, -32, 0, 24, 30, 0, 0, -60, 20,
9, -18, -8, 0, -24, -30, 0, 0, 60, 20,
};
// Like c3, but with the coeffs of x, x^2, and x^3 constrained to be zero.
// Use this at the N pole so that the height in independent of the longitude
// there.
//
// Here's the Maxima code to generate this matrix (continued from above).
//
// /* figure which terms to exclude so that fit is indep of x at y=0 */
// mask:part(zeromatrix(1,length(pows)),1)+1$
// for i:1 thru length(pows) do
// if pows[i][1]>0 and pows[i][2]=0 then mask[i]:0$
//
// /* Same as c3row but with masked pows. */
// c3nrow(k):=block([a,b,c,powsa:[],n,d,e],
// for i:1 thru length(mask) do if mask[i]>0 then
// powsa:endcons(pows[i],powsa),
// n:length(powsa),
// a:zeromatrix(n,n),
// b:copylist(part(a,1)),
// c:[a,b],
// for i:1 thru length(xarr) do
// c:c+addterm(xarr[i],yarr[i],if i=k then 1 else 0,warr[i],powsa),
// a:c[1],b:c[2],
// d:part(transpose( a^^-1 . transpose(b)),1),
// e:[],
// for i:1 thru length(mask) do
// if mask[i]>0 then (e:endcons(first(d),e),d:rest(d)) else e:endcons(0,e),
// e)$
// c3n:[]$
// for k:1 thru length(warr) do c3n:endcons(c3nrow(k),c3n)$
// c3n:apply(matrix,c3n)$
// c0n:part(ratsimp(
// genmatrix(yc,1,length(warr)).abs(c3n).genmatrix(yd,length(pows),1)),2)$
// c3n:c0n*c3n$
const int Geoid::c0n_ = 372; // Common denominator
const int Geoid::c3n_[stencilsize_ * nterms_] = {
0, 0, -131, 0, 138, 144, 0, 0, -102, -31,
0, 0, 7, 0, -138, 42, 0, 0, 102, -31,
62, 0, -31, 0, 0, -62, 0, 0, 0, 31,
124, 0, -62, 0, 0, -124, 0, 0, 0, 62,
124, 0, -62, 0, 0, -124, 0, 0, 0, 62,
62, 0, -31, 0, 0, -62, 0, 0, 0, 31,
0, 0, 45, 0, -183, -9, 0, 93, 18, 0,
0, 0, 216, 0, 33, 87, 0, -93, 12, -93,
0, 0, 156, 0, 153, 99, 0, -93, -12, -93,
0, 0, -45, 0, -3, 9, 0, 93, -18, 0,
0, 0, -55, 0, 48, 42, 0, 0, -84, 31,
0, 0, -7, 0, -48, -42, 0, 0, 84, 31,
};
// Like c3n, but y -> 1-y so that h is independent of x at y = 1. Use this
// at the S pole so that the height in independent of the longitude there.
//
// Here's the Maxima code to generate this matrix (continued from above).
//
// /* Transform c3n to c3s by transforming y -> 1-y */
// vv:[
// v[11],v[12],
// v[7],v[8],v[9],v[10],
// v[3],v[4],v[5],v[6],
// v[1],v[2]]$
// poly:expand(vv.(c3n/c0n).transpose(basisvec(x,1-y,pows)))$
// c3sf[i,j]:=coeff(coeff(coeff(poly,v[i]),x,pows[j][1]),y,pows[j][2])$
// c3s:genmatrix(c3sf,length(vv),length(pows))$
// c0s:part(ratsimp(
// genmatrix(yc,1,length(warr)).abs(c3s).genmatrix(yd,length(pows),1)),2)$
// c3s:c0s*c3s$
const int Geoid::c0s_ = 372; // Common denominator
const int Geoid::c3s_[stencilsize_ * nterms_] = {
18, -36, -122, 0, 120, 135, 0, 0, -84, -31,
-18, 36, -2, 0, -120, 51, 0, 0, 84, -31,
36, -165, -27, 93, 147, -9, 0, -93, 18, 0,
210, 45, -111, -93, -57, -192, 0, 93, 12, 93,
162, 141, -75, -93, -129, -180, 0, 93, -12, 93,
-36, -21, 27, 93, 39, 9, 0, -93, -18, 0,
0, 0, 62, 0, 0, 31, 0, 0, 0, -31,
0, 0, 124, 0, 0, 62, 0, 0, 0, -62,
0, 0, 124, 0, 0, 62, 0, 0, 0, -62,
0, 0, 62, 0, 0, 31, 0, 0, 0, -31,
-18, 36, -64, 0, 66, 51, 0, 0, -102, 31,
18, -36, 2, 0, -66, -51, 0, 0, 102, 31,
};
Geoid::Geoid(const std::string& name, const std::string& path, bool cubic,
bool threadsafe)
: _name(name)
, _dir(path)
, _cubic(cubic)
, _a( Constants::WGS84_a() )
, _e2( (2 - Constants::WGS84_f()) * Constants::WGS84_f() )
, _degree( Math::degree() )
, _eps( sqrt(numeric_limits<real>::epsilon()) )
, _threadsafe(false) // Set after cache is read
{
static_assert(sizeof(pixel_t) == pixel_size_, "pixel_t has the wrong size");
if (_dir.empty())
_dir = DefaultGeoidPath();
_filename = _dir + "/" + _name + (pixel_size_ != 4 ? ".pgm" : ".pgm4");
_file.open(_filename.c_str(), ios::binary);
if (!(_file.good()))
throw GeographicErr("File not readable " + _filename);
string s;
if (!(getline(_file, s) && s == "P5"))
throw GeographicErr("File not in PGM format " + _filename);
_offset = numeric_limits<real>::max();
_scale = 0;
_maxerror = _rmserror = -1;
_description = "NONE";
_datetime = "UNKNOWN";
while (getline(_file, s)) {
if (s.empty())
continue;
if (s[0] == '#') {
istringstream is(s);
string commentid, key;
if (!(is >> commentid >> key) || commentid != "#")
continue;
if (key == "Description" || key == "DateTime") {
string::size_type p =
s.find_first_not_of(" \t", unsigned(is.tellg()));
if (p != string::npos)
(key == "Description" ? _description : _datetime) = s.substr(p);
} else if (key == "Offset") {
if (!(is >> _offset))
throw GeographicErr("Error reading offset " + _filename);
} else if (key == "Scale") {
if (!(is >> _scale))
throw GeographicErr("Error reading scale " + _filename);
} else if (key == (_cubic ? "MaxCubicError" : "MaxBilinearError")) {
// It's not an error if the error can't be read
is >> _maxerror;
} else if (key == (_cubic ? "RMSCubicError" : "RMSBilinearError")) {
// It's not an error if the error can't be read
is >> _rmserror;
}
} else {
istringstream is(s);
if (!(is >> _width >> _height))
throw GeographicErr("Error reading raster size " + _filename);
break;
}
}
{
unsigned maxval;
if (!(_file >> maxval))
throw GeographicErr("Error reading maxval " + _filename);
if (maxval != pixel_max_)
throw GeographicErr("Incorrect value of maxval " + _filename);
// Add 1 for whitespace after maxval
_datastart = (unsigned long long)(_file.tellg()) + 1ULL;
_swidth = (unsigned long long)(_width);
}
if (_offset == numeric_limits<real>::max())
throw GeographicErr("Offset not set " + _filename);
if (_scale == 0)
throw GeographicErr("Scale not set " + _filename);
if (_scale < 0)
throw GeographicErr("Scale must be positive " + _filename);
if (_height < 2 || _width < 2)
// Coarsest grid spacing is 180deg.
throw GeographicErr("Raster size too small " + _filename);
if (_width & 1)
// This is so that longitude grids can be extended thru the poles.
throw GeographicErr("Raster width is odd " + _filename);
if (!(_height & 1))
// This is so that latitude grid includes the equator.
throw GeographicErr("Raster height is even " + _filename);
_file.seekg(0, ios::end);
if (!_file.good() ||
_datastart + pixel_size_ * _swidth * (unsigned long long)(_height) !=
(unsigned long long)(_file.tellg()))
// Possibly this test should be "<" because the file contains, e.g., a
// second image. However, for now we are more strict.
throw GeographicErr("File has the wrong length " + _filename);
_rlonres = _width / real(Math::td);
_rlatres = (_height - 1) / real(Math::hd);
_cache = false;
_ix = _width;
_iy = _height;
// Ensure that file errors throw exceptions
_file.exceptions(ifstream::eofbit | ifstream::failbit | ifstream::badbit);
if (threadsafe) {
CacheAll();
_file.close();
_threadsafe = true;
}
}
Math::real Geoid::height(real lat, real lon) const {
using std::isnan; // Needed for Centos 7, ubuntu 14
lat = Math::LatFix(lat);
if (isnan(lat) || isnan(lon)) {
return Math::NaN();
}
lon = Math::AngNormalize(lon);
real
fx = lon * _rlonres,
fy = -lat * _rlatres;
int
ix = int(floor(fx)),
iy = min((_height - 1)/2 - 1, int(floor(fy)));
fx -= ix;
fy -= iy;
iy += (_height - 1)/2;
ix += ix < 0 ? _width : (ix >= _width ? -_width : 0);
real v00 = 0, v01 = 0, v10 = 0, v11 = 0;
real t[nterms_];
if (_threadsafe || !(ix == _ix && iy == _iy)) {
if (!_cubic) {
v00 = rawval(ix , iy );
v01 = rawval(ix + 1, iy );
v10 = rawval(ix , iy + 1);
v11 = rawval(ix + 1, iy + 1);
} else {
real v[stencilsize_];
int k = 0;
v[k++] = rawval(ix , iy - 1);
v[k++] = rawval(ix + 1, iy - 1);
v[k++] = rawval(ix - 1, iy );
v[k++] = rawval(ix , iy );
v[k++] = rawval(ix + 1, iy );
v[k++] = rawval(ix + 2, iy );
v[k++] = rawval(ix - 1, iy + 1);
v[k++] = rawval(ix , iy + 1);
v[k++] = rawval(ix + 1, iy + 1);
v[k++] = rawval(ix + 2, iy + 1);
v[k++] = rawval(ix , iy + 2);
v[k++] = rawval(ix + 1, iy + 2);
const int* c3x = iy == 0 ? c3n_ : (iy == _height - 2 ? c3s_ : c3_);
int c0x = iy == 0 ? c0n_ : (iy == _height - 2 ? c0s_ : c0_);
for (unsigned i = 0; i < nterms_; ++i) {
t[i] = 0;
for (unsigned j = 0; j < stencilsize_; ++j)
t[i] += v[j] * c3x[nterms_ * j + i];
t[i] /= c0x;
}
}
} else { // same cell; used cached coefficients
if (!_cubic) {
v00 = _v00;
v01 = _v01;
v10 = _v10;
v11 = _v11;
} else
copy(_t, _t + nterms_, t);
}
if (!_cubic) {
real
a = (1 - fx) * v00 + fx * v01,
b = (1 - fx) * v10 + fx * v11,
c = (1 - fy) * a + fy * b,
h = _offset + _scale * c;
if (!_threadsafe) {
_ix = ix;
_iy = iy;
_v00 = v00;
_v01 = v01;
_v10 = v10;
_v11 = v11;
}
return h;
} else {
real h = t[0] + fx * (t[1] + fx * (t[3] + fx * t[6])) +
fy * (t[2] + fx * (t[4] + fx * t[7]) +
fy * (t[5] + fx * t[8] + fy * t[9]));
h = _offset + _scale * h;
if (!_threadsafe) {
_ix = ix;
_iy = iy;
copy(t, t + nterms_, _t);
}
return h;
}
}
void Geoid::CacheClear() const {
if (!_threadsafe) {
_cache = false;
try {
_data.clear();
// Use swap to release memory back to system
vector< vector<pixel_t> >().swap(_data);
}
catch (const exception&) {
}
}
}
void Geoid::CacheArea(real south, real west, real north, real east) const {
if (_threadsafe)
throw GeographicErr("Attempt to change cache of threadsafe Geoid");
if (south > north) {
CacheClear();
return;
}
south = Math::LatFix(south);
north = Math::LatFix(north);
west = Math::AngNormalize(west); // west in [-180, 180)
east = Math::AngNormalize(east);
if (east <= west)
east += Math::td; // east - west in (0, 360]
int
iw = int(floor(west * _rlonres)),
ie = int(floor(east * _rlonres)),
in = int(floor(-north * _rlatres)) + (_height - 1)/2,
is = int(floor(-south * _rlatres)) + (_height - 1)/2;
in = max(0, min(_height - 2, in));
is = max(0, min(_height - 2, is));
is += 1;
ie += 1;
if (_cubic) {
in -= 1;
is += 1;
iw -= 1;
ie += 1;
}
if (ie - iw >= _width - 1) {
// Include entire longitude range
iw = 0;
ie = _width - 1;
} else {
ie += iw < 0 ? _width : (iw >= _width ? -_width : 0);
iw += iw < 0 ? _width : (iw >= _width ? -_width : 0);
}
int oysize = int(_data.size());
_xsize = ie - iw + 1;
_ysize = is - in + 1;
_xoffset = iw;
_yoffset = in;
try {
_data.resize(_ysize, vector<pixel_t>(_xsize));
for (int iy = min(oysize, _ysize); iy--;)
_data[iy].resize(_xsize);
}
catch (const bad_alloc&) {
CacheClear();
throw GeographicErr("Insufficient memory for caching " + _filename);
}
try {
for (int iy = in; iy <= is; ++iy) {
int iy1 = iy, iw1 = iw;
if (iy < 0 || iy >= _height) {
// Allow points "beyond" the poles to support interpolation
iy1 = iy1 < 0 ? -iy1 : 2 * (_height - 1) - iy1;
iw1 += _width/2;
if (iw1 >= _width)
iw1 -= _width;
}
int xs1 = min(_width - iw1, _xsize);
filepos(iw1, iy1);
Utility::readarray<pixel_t, pixel_t, true>
(_file, &(_data[iy - in][0]), xs1);
if (xs1 < _xsize) {
// Wrap around longitude = 0
filepos(0, iy1);
Utility::readarray<pixel_t, pixel_t, true>
(_file, &(_data[iy - in][xs1]), _xsize - xs1);
}
}
_cache = true;
}
catch (const exception& e) {
CacheClear();
throw GeographicErr(string("Error filling cache ") + e.what());
}
}
string Geoid::DefaultGeoidPath() {
string path;
char* geoidpath = getenv("GEOGRAPHICLIB_GEOID_PATH");
if (geoidpath)
path = string(geoidpath);
if (!path.empty())
return path;
char* datapath = getenv("GEOGRAPHICLIB_DATA");
if (datapath)
path = string(datapath);
return (!path.empty() ? path : string(GEOGRAPHICLIB_DATA)) + "/geoids";
}
string Geoid::DefaultGeoidName() {
string name;
char* geoidname = getenv("GEOGRAPHICLIB_GEOID_NAME");
if (geoidname)
name = string(geoidname);
return !name.empty() ? name : string(GEOGRAPHICLIB_GEOID_DEFAULT_NAME);
}
} // namespace GeographicLib

143
libs/geographiclib/src/Georef.cpp Normal file
View File

@@ -0,0 +1,143 @@
/**
* \file Georef.cpp
* \brief Implementation for GeographicLib::Georef class
*
* Copyright (c) Charles Karney (2015-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/Georef.hpp>
#include <GeographicLib/Utility.hpp>
#if defined(_MSC_VER)
// Squelch warnings about enum-float expressions
# pragma warning (disable: 5055)
#endif
namespace GeographicLib {
using namespace std;
const char* const Georef::digits_ = "0123456789";
const char* const Georef::lontile_ = "ABCDEFGHJKLMNPQRSTUVWXYZ";
const char* const Georef::lattile_ = "ABCDEFGHJKLM";
const char* const Georef::degrees_ = "ABCDEFGHJKLMNPQ";
void Georef::Forward(real lat, real lon, int prec, string& georef) {
using std::isnan; // Needed for Centos 7, ubuntu 14
if (fabs(lat) > Math::qd)
throw GeographicErr("Latitude " + Utility::str(lat)
+ "d not in [-" + to_string(Math::qd)
+ "d, " + to_string(Math::qd) + "d]");
if (isnan(lat) || isnan(lon)) {
georef = "INVALID";
return;
}
lon = Math::AngNormalize(lon); // lon in [-180,180)
if (lat == Math::qd) lat *= (1 - numeric_limits<real>::epsilon() / 2);
prec = max(-1, min(int(maxprec_), prec));
if (prec == 1) ++prec; // Disallow prec = 1
// The C++ standard mandates 64 bits for long long. But
// check, to make sure.
static_assert(numeric_limits<long long>::digits >= 45,
"long long not wide enough to store 21600e9");
const long long m = 60000000000LL;
long long
x = (long long)(floor(lon * real(m))) - lonorig_ * m,
y = (long long)(floor(lat * real(m))) - latorig_ * m;
int ilon = int(x / m); int ilat = int(y / m);
char georef1[maxlen_];
georef1[0] = lontile_[ilon / tile_];
georef1[1] = lattile_[ilat / tile_];
if (prec >= 0) {
georef1[2] = degrees_[ilon % tile_];
georef1[3] = degrees_[ilat % tile_];
if (prec > 0) {
x -= m * ilon; y -= m * ilat;
long long d = (long long)pow(real(base_), maxprec_ - prec);
x /= d; y /= d;
for (int c = prec; c--;) {
georef1[baselen_ + c ] = digits_[x % base_]; x /= base_;
georef1[baselen_ + c + prec] = digits_[y % base_]; y /= base_;
}
}
}
georef.resize(baselen_ + 2 * prec);
copy(georef1, georef1 + baselen_ + 2 * prec, georef.begin());
}
void Georef::Reverse(const string& georef, real& lat, real& lon,
int& prec, bool centerp) {
int len = int(georef.length());
if (len >= 3 &&
toupper(georef[0]) == 'I' &&
toupper(georef[1]) == 'N' &&
toupper(georef[2]) == 'V') {
lat = lon = Math::NaN();
return;
}
if (len < baselen_ - 2)
throw GeographicErr("Georef must start with at least 2 letters "
+ georef);
int prec1 = (2 + len - baselen_) / 2 - 1;
int k;
k = Utility::lookup(lontile_, georef[0]);
if (k < 0)
throw GeographicErr("Bad longitude tile letter in georef " + georef);
real lon1 = k + lonorig_ / tile_;
k = Utility::lookup(lattile_, georef[1]);
if (k < 0)
throw GeographicErr("Bad latitude tile letter in georef " + georef);
real lat1 = k + latorig_ / tile_;
real unit = 1;
if (len > 2) {
unit *= tile_;
k = Utility::lookup(degrees_, georef[2]);
if (k < 0)
throw GeographicErr("Bad longitude degree letter in georef " + georef);
lon1 = lon1 * tile_ + k;
if (len < 4)
throw GeographicErr("Missing latitude degree letter in georef "
+ georef);
k = Utility::lookup(degrees_, georef[3]);
if (k < 0)
throw GeographicErr("Bad latitude degree letter in georef " + georef);
lat1 = lat1 * tile_ + k;
if (prec1 > 0) {
if (georef.find_first_not_of(digits_, baselen_) != string::npos)
throw GeographicErr("Non digits in trailing portion of georef "
+ georef.substr(baselen_));
if (len % 2)
throw GeographicErr("Georef must end with an even number of digits "
+ georef.substr(baselen_));
if (prec1 == 1)
throw GeographicErr("Georef needs at least 4 digits for minutes "
+ georef.substr(baselen_));
if (prec1 > maxprec_)
throw GeographicErr("More than " + Utility::str(2*maxprec_)
+ " digits in georef "
+ georef.substr(baselen_));
for (int i = 0; i < prec1; ++i) {
int m = i ? base_ : 6;
unit *= m;
int
x = Utility::lookup(digits_, georef[baselen_ + i]),
y = Utility::lookup(digits_, georef[baselen_ + i + prec1]);
if (!(i || (x < m && y < m)))
throw GeographicErr("Minutes terms in georef must be less than 60 "
+ georef.substr(baselen_));
lon1 = m * lon1 + x;
lat1 = m * lat1 + y;
}
}
}
if (centerp) {
unit *= 2; lat1 = 2 * lat1 + 1; lon1 = 2 * lon1 + 1;
}
lat = (tile_ * lat1) / unit;
lon = (tile_ * lon1) / unit;
prec = prec1;
}
} // namespace GeographicLib

83
libs/geographiclib/src/Gnomonic.cpp Normal file
View File

@@ -0,0 +1,83 @@
/**
* \file Gnomonic.cpp
* \brief Implementation for GeographicLib::Gnomonic class
*
* Copyright (c) Charles Karney (2010-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/Gnomonic.hpp>
#if defined(_MSC_VER)
// Squelch warnings about potentially uninitialized local variables and
// constant conditional expressions
# pragma warning (disable: 4701 4127)
#endif
namespace GeographicLib {
using namespace std;
Gnomonic::Gnomonic(const Geodesic& earth)
: eps0_(numeric_limits<real>::epsilon())
, eps_(real(0.01) * sqrt(eps0_))
, _earth(earth)
, _a(_earth.EquatorialRadius())
, _f(_earth.Flattening())
{}
void Gnomonic::Forward(real lat0, real lon0, real lat, real lon,
real& x, real& y, real& azi, real& rk) const {
real azi0, m, M, t;
_earth.GenInverse(lat0, lon0, lat, lon,
Geodesic::AZIMUTH | Geodesic::REDUCEDLENGTH |
Geodesic::GEODESICSCALE,
t, azi0, azi, m, M, t, t);
rk = M;
if (M <= 0)
x = y = Math::NaN();
else {
real rho = m/M;
Math::sincosd(azi0, x, y);
x *= rho; y *= rho;
}
}
void Gnomonic::Reverse(real lat0, real lon0, real x, real y,
real& lat, real& lon, real& azi, real& rk) const {
real
azi0 = Math::atan2d(x, y),
rho = hypot(x, y),
s = _a * atan(rho/_a);
bool little = rho <= _a;
if (!little)
rho = 1/rho;
GeodesicLine line(_earth.Line(lat0, lon0, azi0,
Geodesic::LATITUDE | Geodesic::LONGITUDE |
Geodesic::AZIMUTH | Geodesic::DISTANCE_IN |
Geodesic::REDUCEDLENGTH |
Geodesic::GEODESICSCALE));
int count = numit_, trip = 0;
real lat1, lon1, azi1, M;
while (count-- || GEOGRAPHICLIB_PANIC) {
real m, t;
line.Position(s, lat1, lon1, azi1, m, M, t);
if (trip)
break;
// If little, solve rho(s) = rho with drho(s)/ds = 1/M^2
// else solve 1/rho(s) = 1/rho with d(1/rho(s))/ds = -1/m^2
real ds = little ? (m - rho * M) * M : (rho * m - M) * m;
s -= ds;
// Reversed test to allow escape with NaNs
if (!(fabs(ds) >= eps_ * _a))
++trip;
}
if (trip) {
lat = lat1; lon = lon1; azi = azi1; rk = M;
} else
lat = lon = azi = rk = Math::NaN();
return;
}
} // namespace GeographicLib

159
libs/geographiclib/src/GravityCircle.cpp Normal file
View File

@@ -0,0 +1,159 @@
/**
* \file GravityCircle.cpp
* \brief Implementation for GeographicLib::GravityCircle class
*
* Copyright (c) Charles Karney (2011-2020) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/GravityCircle.hpp>
#include <fstream>
#include <sstream>
#include <GeographicLib/Geocentric.hpp>
namespace GeographicLib {
using namespace std;
GravityCircle::GravityCircle(mask caps, real a, real f, real lat, real h,
real Z, real P, real cphi, real sphi,
real amodel, real GMmodel,
real dzonal0, real corrmult,
real gamma0, real gamma, real frot,
const CircularEngine& gravitational,
const CircularEngine& disturbing,
const CircularEngine& correction)
: _caps(caps)
, _a(a)
, _f(f)
, _lat(Math::LatFix(lat))
, _h(h)
, _zZ(Z)
, _pPx(P)
, _invR(1 / hypot(_pPx, _zZ))
, _cpsi(_pPx * _invR)
, _spsi(_zZ * _invR)
, _cphi(cphi)
, _sphi(sphi)
, _amodel(amodel)
, _gGMmodel(GMmodel)
, _dzonal0(dzonal0)
, _corrmult(corrmult)
, _gamma0(gamma0)
, _gamma(gamma)
, _frot(frot)
, _gravitational(gravitational)
, _disturbing(disturbing)
, _correction(correction)
{}
Math::real GravityCircle::Gravity(real lon,
real& gx, real& gy, real& gz) const {
real slam, clam, M[Geocentric::dim2_];
Math::sincosd(lon, slam, clam);
real Wres = W(slam, clam, gx, gy, gz);
Geocentric::Rotation(_sphi, _cphi, slam, clam, M);
Geocentric::Unrotate(M, gx, gy, gz, gx, gy, gz);
return Wres;
}
Math::real GravityCircle::Disturbance(real lon, real& deltax, real& deltay,
real& deltaz) const {
real slam, clam, M[Geocentric::dim2_];
Math::sincosd(lon, slam, clam);
real Tres = InternalT(slam, clam, deltax, deltay, deltaz, true, true);
Geocentric::Rotation(_sphi, _cphi, slam, clam, M);
Geocentric::Unrotate(M, deltax, deltay, deltaz, deltax, deltay, deltaz);
return Tres;
}
Math::real GravityCircle::GeoidHeight(real lon) const {
if ((_caps & GEOID_HEIGHT) != GEOID_HEIGHT)
return Math::NaN();
real slam, clam, dummy;
Math::sincosd(lon, slam, clam);
real T = InternalT(slam, clam, dummy, dummy, dummy, false, false);
real correction = _corrmult * _correction(slam, clam);
return T/_gamma0 + correction;
}
void GravityCircle::SphericalAnomaly(real lon,
real& Dg01, real& xi, real& eta) const {
if ((_caps & SPHERICAL_ANOMALY) != SPHERICAL_ANOMALY) {
Dg01 = xi = eta = Math::NaN();
return;
}
real slam, clam;
Math::sincosd(lon, slam, clam);
real
deltax, deltay, deltaz,
T = InternalT(slam, clam, deltax, deltay, deltaz, true, false);
// Rotate cartesian into spherical coordinates
real MC[Geocentric::dim2_];
Geocentric::Rotation(_spsi, _cpsi, slam, clam, MC);
Geocentric::Unrotate(MC, deltax, deltay, deltaz, deltax, deltay, deltaz);
// H+M, Eq 2-151c
Dg01 = - deltaz - 2 * T * _invR;
xi = -(deltay/_gamma) / Math::degree();
eta = -(deltax/_gamma) / Math::degree();
}
Math::real GravityCircle::W(real slam, real clam,
real& gX, real& gY, real& gZ) const {
real Wres = V(slam, clam, gX, gY, gZ) + _frot * _pPx / 2;
gX += _frot * clam;
gY += _frot * slam;
return Wres;
}
Math::real GravityCircle::V(real slam, real clam,
real& GX, real& GY, real& GZ) const {
if ((_caps & GRAVITY) != GRAVITY) {
GX = GY = GZ = Math::NaN();
return Math::NaN();
}
real
Vres = _gravitational(slam, clam, GX, GY, GZ),
f = _gGMmodel / _amodel;
Vres *= f;
GX *= f;
GY *= f;
GZ *= f;
return Vres;
}
Math::real GravityCircle::InternalT(real slam, real clam,
real& deltaX, real& deltaY, real& deltaZ,
bool gradp, bool correct) const {
if (gradp) {
if ((_caps & DISTURBANCE) != DISTURBANCE) {
deltaX = deltaY = deltaZ = Math::NaN();
return Math::NaN();
}
} else {
if ((_caps & DISTURBING_POTENTIAL) != DISTURBING_POTENTIAL)
return Math::NaN();
}
if (_dzonal0 == 0)
correct = false;
real T = (gradp
? _disturbing(slam, clam, deltaX, deltaY, deltaZ)
: _disturbing(slam, clam));
T = (T / _amodel - (correct ? _dzonal0 : 0) * _invR) * _gGMmodel;
if (gradp) {
real f = _gGMmodel / _amodel;
deltaX *= f;
deltaY *= f;
deltaZ *= f;
if (correct) {
real r3 = _gGMmodel * _dzonal0 * _invR * _invR * _invR;
deltaX += _pPx * clam * r3;
deltaY += _pPx * slam * r3;
deltaZ += _zZ * r3;
}
}
return T;
}
} // namespace GeographicLib

377
libs/geographiclib/src/GravityModel.cpp Normal file
View File

@@ -0,0 +1,377 @@
/**
* \file GravityModel.cpp
* \brief Implementation for GeographicLib::GravityModel class
*
* Copyright (c) Charles Karney (2011-2020) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/GravityModel.hpp>
#include <fstream>
#include <limits>
#include <GeographicLib/SphericalEngine.hpp>
#include <GeographicLib/GravityCircle.hpp>
#include <GeographicLib/Utility.hpp>
#if !defined(GEOGRAPHICLIB_DATA)
# if defined(_WIN32)
# define GEOGRAPHICLIB_DATA "C:/ProgramData/GeographicLib"
# else
# define GEOGRAPHICLIB_DATA "/usr/local/share/GeographicLib"
# endif
#endif
#if !defined(GEOGRAPHICLIB_GRAVITY_DEFAULT_NAME)
# define GEOGRAPHICLIB_GRAVITY_DEFAULT_NAME "egm96"
#endif
#if defined(_MSC_VER)
// Squelch warnings about unsafe use of getenv
# pragma warning (disable: 4996)
#endif
namespace GeographicLib {
using namespace std;
GravityModel::GravityModel(const std::string& name, const std::string& path,
int Nmax, int Mmax)
: _name(name)
, _dir(path)
, _description("NONE")
, _date("UNKNOWN")
, _amodel(Math::NaN())
, _gGMmodel(Math::NaN())
, _zeta0(0)
, _corrmult(1)
, _nmx(-1)
, _mmx(-1)
, _norm(SphericalHarmonic::FULL)
{
if (_dir.empty())
_dir = DefaultGravityPath();
bool truncate = Nmax >= 0 || Mmax >= 0;
if (truncate) {
if (Nmax >= 0 && Mmax < 0) Mmax = Nmax;
if (Nmax < 0) Nmax = numeric_limits<int>::max();
if (Mmax < 0) Mmax = numeric_limits<int>::max();
}
ReadMetadata(_name);
{
string coeff = _filename + ".cof";
ifstream coeffstr(coeff.c_str(), ios::binary);
if (!coeffstr.good())
throw GeographicErr("Error opening " + coeff);
char id[idlength_ + 1];
coeffstr.read(id, idlength_);
if (!coeffstr.good())
throw GeographicErr("No header in " + coeff);
id[idlength_] = '\0';
if (_id != string(id))
throw GeographicErr("ID mismatch: " + _id + " vs " + id);
int N, M;
if (truncate) { N = Nmax; M = Mmax; }
SphericalEngine::coeff::readcoeffs(coeffstr, N, M, _cCx, _sSx, truncate);
if (!(N >= 0 && M >= 0))
throw GeographicErr("Degree and order must be at least 0");
if (_cCx[0] != 0)
throw GeographicErr("The degree 0 term should be zero");
_cCx[0] = 1; // Include the 1/r term in the sum
_gravitational = SphericalHarmonic(_cCx, _sSx, N, N, M, _amodel, _norm);
if (truncate) { N = Nmax; M = Mmax; }
SphericalEngine::coeff::readcoeffs(coeffstr, N, M, _cCC, _cCS, truncate);
if (N < 0) {
N = M = 0;
_cCC.resize(1, real(0));
}
_cCC[0] += _zeta0 / _corrmult;
_correction = SphericalHarmonic(_cCC, _cCS, N, N, M, real(1), _norm);
int pos = int(coeffstr.tellg());
coeffstr.seekg(0, ios::end);
if (pos != coeffstr.tellg())
throw GeographicErr("Extra data in " + coeff);
}
int nmx = _gravitational.Coefficients().nmx();
_nmx = max(nmx, _correction.Coefficients().nmx());
_mmx = max(_gravitational.Coefficients().mmx(),
_correction.Coefficients().mmx());
// Adjust the normalization of the normal potential to match the model.
real mult = _earth._gGM / _gGMmodel;
real amult = Math::sq(_earth._a / _amodel);
// The 0th term in _zonal should be is 1 + _dzonal0. Instead set it to 1
// to give exact cancellation with the (0,0) term in the model and account
// for _dzonal0 separately.
_zonal.clear(); _zonal.push_back(1);
_dzonal0 = (_earth.MassConstant() - _gGMmodel) / _gGMmodel;
for (int n = 2; n <= nmx; n += 2) {
// Only include as many normal zonal terms as matter. Figuring the limit
// in this way works because the coefficients of the normal potential
// (which is smooth) decay much more rapidly that the corresponding
// coefficient of the model potential (which is bumpy). Typically this
// goes out to n = 18.
mult *= amult;
real
r = _cCx[n], // the model term
s = - mult * _earth.Jn(n) / sqrt(real(2 * n + 1)), // the normal term
t = r - s; // the difference
if (t == r) // the normal term is negligible
break;
_zonal.push_back(0); // index = n - 1; the odd terms are 0
_zonal.push_back(s);
}
int nmx1 = int(_zonal.size()) - 1;
_disturbing = SphericalHarmonic1(_cCx, _sSx,
_gravitational.Coefficients().N(),
nmx, _gravitational.Coefficients().mmx(),
_zonal,
_zonal, // This is not accessed!
nmx1, nmx1, 0,
_amodel,
SphericalHarmonic1::normalization(_norm));
}
void GravityModel::ReadMetadata(const string& name) {
const char* spaces = " \t\n\v\f\r";
_filename = _dir + "/" + name + ".egm";
ifstream metastr(_filename.c_str());
if (!metastr.good())
throw GeographicErr("Cannot open " + _filename);
string line;
getline(metastr, line);
if (!(line.size() >= 6 && line.substr(0,5) == "EGMF-"))
throw GeographicErr(_filename + " does not contain EGMF-n signature");
string::size_type n = line.find_first_of(spaces, 5);
if (n != string::npos)
n -= 5;
string version(line, 5, n);
if (version != "1")
throw GeographicErr("Unknown version in " + _filename + ": " + version);
string key, val;
real a = Math::NaN(), GM = a, omega = a, f = a, J2 = a;
while (getline(metastr, line)) {
if (!Utility::ParseLine(line, key, val))
continue;
// Process key words
if (key == "Name")
_name = val;
else if (key == "Description")
_description = val;
else if (key == "ReleaseDate")
_date = val;
else if (key == "ModelRadius")
_amodel = Utility::val<real>(val);
else if (key == "ModelMass")
_gGMmodel = Utility::val<real>(val);
else if (key == "AngularVelocity")
omega = Utility::val<real>(val);
else if (key == "ReferenceRadius")
a = Utility::val<real>(val);
else if (key == "ReferenceMass")
GM = Utility::val<real>(val);
else if (key == "Flattening")
f = Utility::fract<real>(val);
else if (key == "DynamicalFormFactor")
J2 = Utility::fract<real>(val);
else if (key == "HeightOffset")
_zeta0 = Utility::fract<real>(val);
else if (key == "CorrectionMultiplier")
_corrmult = Utility::fract<real>(val);
else if (key == "Normalization") {
if (val == "FULL" || val == "Full" || val == "full")
_norm = SphericalHarmonic::FULL;
else if (val == "SCHMIDT" || val == "Schmidt" || val == "schmidt")
_norm = SphericalHarmonic::SCHMIDT;
else
throw GeographicErr("Unknown normalization " + val);
} else if (key == "ByteOrder") {
if (val == "Big" || val == "big")
throw GeographicErr("Only little-endian ordering is supported");
else if (!(val == "Little" || val == "little"))
throw GeographicErr("Unknown byte ordering " + val);
} else if (key == "ID")
_id = val;
// else unrecognized keywords are skipped
}
// Check values
if (!(isfinite(_amodel) && _amodel > 0))
throw GeographicErr("Model radius must be positive");
if (!(isfinite(_gGMmodel) && _gGMmodel > 0))
throw GeographicErr("Model mass constant must be positive");
if (!(isfinite(_corrmult) && _corrmult > 0))
throw GeographicErr("Correction multiplier must be positive");
if (!(isfinite(_zeta0)))
throw GeographicErr("Height offset must be finite");
if (int(_id.size()) != idlength_)
throw GeographicErr("Invalid ID");
if (isfinite(f) && isfinite(J2))
throw GeographicErr("Cannot specify both f and J2");
_earth = NormalGravity(a, GM, omega,
isfinite(f) ? f : J2, isfinite(f));
}
Math::real GravityModel::InternalT(real X, real Y, real Z,
real& deltaX, real& deltaY, real& deltaZ,
bool gradp, bool correct) const {
// If correct, then produce the correct T = W - U. Otherwise, neglect the
// n = 0 term (which is proportial to the difference in the model and
// reference values of GM).
if (_dzonal0 == 0)
// No need to do the correction
correct = false;
real T, invR = correct ? 1 / hypot(hypot(X, Y), Z) : 1;
if (gradp) {
// initial values to suppress warnings
deltaX = deltaY = deltaZ = 0;
T = _disturbing(-1, X, Y, Z, deltaX, deltaY, deltaZ);
real f = _gGMmodel / _amodel;
deltaX *= f;
deltaY *= f;
deltaZ *= f;
if (correct) {
invR = _gGMmodel * _dzonal0 * invR * invR * invR;
deltaX += X * invR;
deltaY += Y * invR;
deltaZ += Z * invR;
}
} else
T = _disturbing(-1, X, Y, Z);
T = (T / _amodel - (correct ? _dzonal0 : 0) * invR) * _gGMmodel;
return T;
}
Math::real GravityModel::V(real X, real Y, real Z,
real& GX, real& GY, real& GZ) const {
real
Vres = _gravitational(X, Y, Z, GX, GY, GZ),
f = _gGMmodel / _amodel;
Vres *= f;
GX *= f;
GY *= f;
GZ *= f;
return Vres;
}
Math::real GravityModel::W(real X, real Y, real Z,
real& gX, real& gY, real& gZ) const {
real fX, fY,
Wres = V(X, Y, Z, gX, gY, gZ) + _earth.Phi(X, Y, fX, fY);
gX += fX;
gY += fY;
return Wres;
}
void GravityModel::SphericalAnomaly(real lat, real lon, real h,
real& Dg01, real& xi, real& eta) const {
real X, Y, Z, M[Geocentric::dim2_];
_earth.Earth().IntForward(lat, lon, h, X, Y, Z, M);
real
deltax, deltay, deltaz,
T = InternalT(X, Y, Z, deltax, deltay, deltaz, true, false),
clam = M[3], slam = -M[0],
P = hypot(X, Y),
R = hypot(P, Z),
// psi is geocentric latitude
cpsi = R != 0 ? P / R : M[7],
spsi = R != 0 ? Z / R : M[8];
// Rotate cartesian into spherical coordinates
real MC[Geocentric::dim2_];
Geocentric::Rotation(spsi, cpsi, slam, clam, MC);
Geocentric::Unrotate(MC, deltax, deltay, deltaz, deltax, deltay, deltaz);
// H+M, Eq 2-151c
Dg01 = - deltaz - 2 * T / R;
real gammaX, gammaY, gammaZ;
_earth.U(X, Y, Z, gammaX, gammaY, gammaZ);
real gamma = hypot( hypot(gammaX, gammaY), gammaZ);
xi = -(deltay/gamma) / Math::degree();
eta = -(deltax/gamma) / Math::degree();
}
Math::real GravityModel::GeoidHeight(real lat, real lon) const
{
real X, Y, Z;
_earth.Earth().IntForward(lat, lon, 0, X, Y, Z, NULL);
real
gamma0 = _earth.SurfaceGravity(lat),
dummy,
T = InternalT(X, Y, Z, dummy, dummy, dummy, false, false),
invR = 1 / hypot(hypot(X, Y), Z),
correction = _corrmult * _correction(invR * X, invR * Y, invR * Z);
// _zeta0 has been included in _correction
return T/gamma0 + correction;
}
Math::real GravityModel::Gravity(real lat, real lon, real h,
real& gx, real& gy, real& gz) const {
real X, Y, Z, M[Geocentric::dim2_];
_earth.Earth().IntForward(lat, lon, h, X, Y, Z, M);
real Wres = W(X, Y, Z, gx, gy, gz);
Geocentric::Unrotate(M, gx, gy, gz, gx, gy, gz);
return Wres;
}
Math::real GravityModel::Disturbance(real lat, real lon, real h,
real& deltax, real& deltay,
real& deltaz) const {
real X, Y, Z, M[Geocentric::dim2_];
_earth.Earth().IntForward(lat, lon, h, X, Y, Z, M);
real Tres = InternalT(X, Y, Z, deltax, deltay, deltaz, true, true);
Geocentric::Unrotate(M, deltax, deltay, deltaz, deltax, deltay, deltaz);
return Tres;
}
GravityCircle GravityModel::Circle(real lat, real h, unsigned caps) const {
if (h != 0)
// Disallow invoking GeoidHeight unless h is zero.
caps &= ~(CAP_GAMMA0 | CAP_C);
real X, Y, Z, M[Geocentric::dim2_];
_earth.Earth().IntForward(lat, 0, h, X, Y, Z, M);
// Y = 0, cphi = M[7], sphi = M[8];
real
invR = 1 / hypot(X, Z),
gamma0 = (caps & CAP_GAMMA0 ?_earth.SurfaceGravity(lat)
: Math::NaN()),
fx, fy, fz, gamma;
if (caps & CAP_GAMMA) {
_earth.U(X, Y, Z, fx, fy, fz); // fy = 0
gamma = hypot(fx, fz);
} else
gamma = Math::NaN();
_earth.Phi(X, Y, fx, fy);
return GravityCircle(GravityCircle::mask(caps),
_earth._a, _earth._f, lat, h, Z, X, M[7], M[8],
_amodel, _gGMmodel, _dzonal0, _corrmult,
gamma0, gamma, fx,
caps & CAP_G ?
_gravitational.Circle(X, Z, true) :
CircularEngine(),
// N.B. If CAP_DELTA is set then CAP_T should be too.
caps & CAP_T ?
_disturbing.Circle(-1, X, Z, (caps&CAP_DELTA) != 0) :
CircularEngine(),
caps & CAP_C ?
_correction.Circle(invR * X, invR * Z, false) :
CircularEngine());
}
string GravityModel::DefaultGravityPath() {
string path;
char* gravitypath = getenv("GEOGRAPHICLIB_GRAVITY_PATH");
if (gravitypath)
path = string(gravitypath);
if (!path.empty())
return path;
char* datapath = getenv("GEOGRAPHICLIB_DATA");
if (datapath)
path = string(datapath);
return (!path.empty() ? path : string(GEOGRAPHICLIB_DATA)) + "/gravity";
}
string GravityModel::DefaultGravityName() {
string name;
char* gravityname = getenv("GEOGRAPHICLIB_GRAVITY_NAME");
if (gravityname)
name = string(gravityname);
return !name.empty() ? name : string(GEOGRAPHICLIB_GRAVITY_DEFAULT_NAME);
}
} // namespace GeographicLib

471
libs/geographiclib/src/LambertConformalConic.cpp Normal file
View File

@@ -0,0 +1,471 @@
/**
* \file LambertConformalConic.cpp
* \brief Implementation for GeographicLib::LambertConformalConic class
*
* Copyright (c) Charles Karney (2010-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/LambertConformalConic.hpp>
#if defined(_MSC_VER)
// Squelch warnings about enum-float expressions
# pragma warning (disable: 5055)
#endif
namespace GeographicLib {
using namespace std;
LambertConformalConic::LambertConformalConic(real a, real f,
real stdlat, real k0)
: eps_(numeric_limits<real>::epsilon())
, epsx_(Math::sq(eps_))
, ahypover_(Math::digits() * log(real(numeric_limits<real>::radix)) + 2)
, _a(a)
, _f(f)
, _fm(1 - _f)
, _e2(_f * (2 - _f))
, _es((_f < 0 ? -1 : 1) * sqrt(fabs(_e2)))
{
if (!(isfinite(_a) && _a > 0))
throw GeographicErr("Equatorial radius is not positive");
if (!(isfinite(_f) && _f < 1))
throw GeographicErr("Polar semi-axis is not positive");
if (!(isfinite(k0) && k0 > 0))
throw GeographicErr("Scale is not positive");
if (!(fabs(stdlat) <= Math::qd))
throw GeographicErr("Standard latitude not in [-" + to_string(Math::qd)
+ "d, " + to_string(Math::qd) + "d]");
real sphi, cphi;
Math::sincosd(stdlat, sphi, cphi);
Init(sphi, cphi, sphi, cphi, k0);
}
LambertConformalConic::LambertConformalConic(real a, real f,
real stdlat1, real stdlat2,
real k1)
: eps_(numeric_limits<real>::epsilon())
, epsx_(Math::sq(eps_))
, ahypover_(Math::digits() * log(real(numeric_limits<real>::radix)) + 2)
, _a(a)
, _f(f)
, _fm(1 - _f)
, _e2(_f * (2 - _f))
, _es((_f < 0 ? -1 : 1) * sqrt(fabs(_e2)))
{
if (!(isfinite(_a) && _a > 0))
throw GeographicErr("Equatorial radius is not positive");
if (!(isfinite(_f) && _f < 1))
throw GeographicErr("Polar semi-axis is not positive");
if (!(isfinite(k1) && k1 > 0))
throw GeographicErr("Scale is not positive");
if (!(fabs(stdlat1) <= Math::qd))
throw GeographicErr("Standard latitude 1 not in [-"
+ to_string(Math::qd) + "d, "
+ to_string(Math::qd) + "d]");
if (!(fabs(stdlat2) <= Math::qd))
throw GeographicErr("Standard latitude 2 not in [-"
+ to_string(Math::qd) + "d, "
+ to_string(Math::qd) + "d]");
real sphi1, cphi1, sphi2, cphi2;
Math::sincosd(stdlat1, sphi1, cphi1);
Math::sincosd(stdlat2, sphi2, cphi2);
Init(sphi1, cphi1, sphi2, cphi2, k1);
}
LambertConformalConic::LambertConformalConic(real a, real f,
real sinlat1, real coslat1,
real sinlat2, real coslat2,
real k1)
: eps_(numeric_limits<real>::epsilon())
, epsx_(Math::sq(eps_))
, ahypover_(Math::digits() * log(real(numeric_limits<real>::radix)) + 2)
, _a(a)
, _f(f)
, _fm(1 - _f)
, _e2(_f * (2 - _f))
, _es((_f < 0 ? -1 : 1) * sqrt(fabs(_e2)))
{
if (!(isfinite(_a) && _a > 0))
throw GeographicErr("Equatorial radius is not positive");
if (!(isfinite(_f) && _f < 1))
throw GeographicErr("Polar semi-axis is not positive");
if (!(isfinite(k1) && k1 > 0))
throw GeographicErr("Scale is not positive");
if (signbit(coslat1))
throw GeographicErr("Standard latitude 1 not in [-"
+ to_string(Math::qd) + "d, "
+ to_string(Math::qd) + "d]");
if (signbit(coslat2))
throw GeographicErr("Standard latitude 2 not in [-"
+ to_string(Math::qd) + "d, "
+ to_string(Math::qd) + "d]");
if (!(fabs(sinlat1) <= 1 && coslat1 <= 1) || (coslat1 == 0 && sinlat1 == 0))
throw GeographicErr("Bad sine/cosine of standard latitude 1");
if (!(fabs(sinlat2) <= 1 && coslat2 <= 1) || (coslat2 == 0 && sinlat2 == 0))
throw GeographicErr("Bad sine/cosine of standard latitude 2");
if (coslat1 == 0 || coslat2 == 0)
if (!(coslat1 == coslat2 && sinlat1 == sinlat2))
throw GeographicErr
("Standard latitudes must be equal is either is a pole");
Init(sinlat1, coslat1, sinlat2, coslat2, k1);
}
void LambertConformalConic::Init(real sphi1, real cphi1,
real sphi2, real cphi2, real k1) {
{
real r;
r = hypot(sphi1, cphi1);
sphi1 /= r; cphi1 /= r;
r = hypot(sphi2, cphi2);
sphi2 /= r; cphi2 /= r;
}
bool polar = (cphi1 == 0);
cphi1 = fmax(epsx_, cphi1); // Avoid singularities at poles
cphi2 = fmax(epsx_, cphi2);
// Determine hemisphere of tangent latitude
_sign = sphi1 + sphi2 >= 0 ? 1 : -1;
// Internally work with tangent latitude positive
sphi1 *= _sign; sphi2 *= _sign;
if (sphi1 > sphi2) {
swap(sphi1, sphi2); swap(cphi1, cphi2); // Make phi1 < phi2
}
real
tphi1 = sphi1/cphi1, tphi2 = sphi2/cphi2, tphi0;
//
// Snyder: 15-8: n = (log(m1) - log(m2))/(log(t1)-log(t2))
//
// m = cos(bet) = 1/sec(bet) = 1/sqrt(1+tan(bet)^2)
// bet = parametric lat, tan(bet) = (1-f)*tan(phi)
//
// t = tan(pi/4-chi/2) = 1/(sec(chi) + tan(chi)) = sec(chi) - tan(chi)
// log(t) = -asinh(tan(chi)) = -psi
// chi = conformal lat
// tan(chi) = tan(phi)*cosh(xi) - sinh(xi)*sec(phi)
// xi = eatanhe(sin(phi)), eatanhe(x) = e * atanh(e*x)
//
// n = (log(sec(bet2))-log(sec(bet1)))/(asinh(tan(chi2))-asinh(tan(chi1)))
//
// Let log(sec(bet)) = b(tphi), asinh(tan(chi)) = c(tphi)
// Then n = Db(tphi2, tphi1)/Dc(tphi2, tphi1)
// In limit tphi2 -> tphi1, n -> sphi1
//
real
tbet1 = _fm * tphi1, scbet1 = hyp(tbet1),
tbet2 = _fm * tphi2, scbet2 = hyp(tbet2);
real
scphi1 = 1/cphi1,
xi1 = Math::eatanhe(sphi1, _es), shxi1 = sinh(xi1), chxi1 = hyp(shxi1),
tchi1 = chxi1 * tphi1 - shxi1 * scphi1, scchi1 = hyp(tchi1),
scphi2 = 1/cphi2,
xi2 = Math::eatanhe(sphi2, _es), shxi2 = sinh(xi2), chxi2 = hyp(shxi2),
tchi2 = chxi2 * tphi2 - shxi2 * scphi2, scchi2 = hyp(tchi2),
psi1 = asinh(tchi1);
if (tphi2 - tphi1 != 0) {
// Db(tphi2, tphi1)
real num = Dlog1p(Math::sq(tbet2)/(1 + scbet2),
Math::sq(tbet1)/(1 + scbet1))
* Dhyp(tbet2, tbet1, scbet2, scbet1) * _fm;
// Dc(tphi2, tphi1)
real den = Dasinh(tphi2, tphi1, scphi2, scphi1)
- Deatanhe(sphi2, sphi1) * Dsn(tphi2, tphi1, sphi2, sphi1);
_n = num/den;
if (_n < 1/real(4))
_nc = sqrt((1 - _n) * (1 + _n));
else {
// Compute nc = cos(phi0) = sqrt((1 - n) * (1 + n)), evaluating 1 - n
// carefully. First write
//
// Dc(tphi2, tphi1) * (tphi2 - tphi1)
// = log(tchi2 + scchi2) - log(tchi1 + scchi1)
//
// then den * (1 - n) =
// (log((tchi2 + scchi2)/(2*scbet2)) -
// log((tchi1 + scchi1)/(2*scbet1))) / (tphi2 - tphi1)
// = Dlog1p(a2, a1) * (tchi2+scchi2 + tchi1+scchi1)/(4*scbet1*scbet2)
// * fm * Q
//
// where
// a1 = ( (tchi1 - scbet1) + (scchi1 - scbet1) ) / (2 * scbet1)
// Q = ((scbet2 + scbet1)/fm)/((scchi2 + scchi1)/D(tchi2, tchi1))
// - (tbet2 + tbet1)/(scbet2 + scbet1)
real t;
{
real
// s1 = (scbet1 - scchi1) * (scbet1 + scchi1)
s1 = (tphi1 * (2 * shxi1 * chxi1 * scphi1 - _e2 * tphi1) -
Math::sq(shxi1) * (1 + 2 * Math::sq(tphi1))),
s2 = (tphi2 * (2 * shxi2 * chxi2 * scphi2 - _e2 * tphi2) -
Math::sq(shxi2) * (1 + 2 * Math::sq(tphi2))),
// t1 = scbet1 - tchi1
t1 = tchi1 < 0 ? scbet1 - tchi1 : (s1 + 1)/(scbet1 + tchi1),
t2 = tchi2 < 0 ? scbet2 - tchi2 : (s2 + 1)/(scbet2 + tchi2),
a2 = -(s2 / (scbet2 + scchi2) + t2) / (2 * scbet2),
a1 = -(s1 / (scbet1 + scchi1) + t1) / (2 * scbet1);
t = Dlog1p(a2, a1) / den;
}
// multiply by (tchi2 + scchi2 + tchi1 + scchi1)/(4*scbet1*scbet2) * fm
t *= ( ( (tchi2 >= 0 ? scchi2 + tchi2 : 1/(scchi2 - tchi2)) +
(tchi1 >= 0 ? scchi1 + tchi1 : 1/(scchi1 - tchi1)) ) /
(4 * scbet1 * scbet2) ) * _fm;
// Rewrite
// Q = (1 - (tbet2 + tbet1)/(scbet2 + scbet1)) -
// (1 - ((scbet2 + scbet1)/fm)/((scchi2 + scchi1)/D(tchi2, tchi1)))
// = tbm - tam
// where
real tbm = ( ((tbet1 > 0 ? 1/(scbet1+tbet1) : scbet1 - tbet1) +
(tbet2 > 0 ? 1/(scbet2+tbet2) : scbet2 - tbet2)) /
(scbet1+scbet2) );
// tam = (1 - ((scbet2+scbet1)/fm)/((scchi2+scchi1)/D(tchi2, tchi1)))
//
// Let
// (scbet2 + scbet1)/fm = scphi2 + scphi1 + dbet
// (scchi2 + scchi1)/D(tchi2, tchi1) = scphi2 + scphi1 + dchi
// then
// tam = D(tchi2, tchi1) * (dchi - dbet) / (scchi1 + scchi2)
real
// D(tchi2, tchi1)
dtchi = den / Dasinh(tchi2, tchi1, scchi2, scchi1),
// (scbet2 + scbet1)/fm - (scphi2 + scphi1)
dbet = (_e2/_fm) * ( 1 / (scbet2 + _fm * scphi2) +
1 / (scbet1 + _fm * scphi1) );
// dchi = (scchi2 + scchi1)/D(tchi2, tchi1) - (scphi2 + scphi1)
// Let
// tzet = chxiZ * tphi - shxiZ * scphi
// tchi = tzet + nu
// scchi = sczet + mu
// where
// xiZ = eatanhe(1), shxiZ = sinh(xiZ), chxiZ = cosh(xiZ)
// nu = scphi * (shxiZ - shxi) - tphi * (chxiZ - chxi)
// mu = - scphi * (chxiZ - chxi) + tphi * (shxiZ - shxi)
// then
// dchi = ((mu2 + mu1) - D(nu2, nu1) * (scphi2 + scphi1)) /
// D(tchi2, tchi1)
real
xiZ = Math::eatanhe(real(1), _es),
shxiZ = sinh(xiZ), chxiZ = hyp(shxiZ),
// These are differences not divided differences
// dxiZ1 = xiZ - xi1; dshxiZ1 = shxiZ - shxi; dchxiZ1 = chxiZ - chxi
dxiZ1 = Deatanhe(real(1), sphi1)/(scphi1*(tphi1+scphi1)),
dxiZ2 = Deatanhe(real(1), sphi2)/(scphi2*(tphi2+scphi2)),
dshxiZ1 = Dsinh(xiZ, xi1, shxiZ, shxi1, chxiZ, chxi1) * dxiZ1,
dshxiZ2 = Dsinh(xiZ, xi2, shxiZ, shxi2, chxiZ, chxi2) * dxiZ2,
dchxiZ1 = Dhyp(shxiZ, shxi1, chxiZ, chxi1) * dshxiZ1,
dchxiZ2 = Dhyp(shxiZ, shxi2, chxiZ, chxi2) * dshxiZ2,
// mu1 + mu2
amu12 = (- scphi1 * dchxiZ1 + tphi1 * dshxiZ1
- scphi2 * dchxiZ2 + tphi2 * dshxiZ2),
// D(xi2, xi1)
dxi = Deatanhe(sphi1, sphi2) * Dsn(tphi2, tphi1, sphi2, sphi1),
// D(nu2, nu1)
dnu12 =
( (_f * 4 * scphi2 * dshxiZ2 > _f * scphi1 * dshxiZ1 ?
// Use divided differences
(dshxiZ1 + dshxiZ2)/2 * Dhyp(tphi1, tphi2, scphi1, scphi2)
- ( (scphi1 + scphi2)/2
* Dsinh(xi1, xi2, shxi1, shxi2, chxi1, chxi2) * dxi ) :
// Use ratio of differences
(scphi2 * dshxiZ2 - scphi1 * dshxiZ1)/(tphi2 - tphi1))
+ ( (tphi1 + tphi2)/2 * Dhyp(shxi1, shxi2, chxi1, chxi2)
* Dsinh(xi1, xi2, shxi1, shxi2, chxi1, chxi2) * dxi )
- (dchxiZ1 + dchxiZ2)/2 ),
// dtchi * dchi
dchia = (amu12 - dnu12 * (scphi2 + scphi1)),
tam = (dchia - dtchi * dbet) / (scchi1 + scchi2);
t *= tbm - tam;
_nc = sqrt(fmax(real(0), t) * (1 + _n));
}
{
real r = hypot(_n, _nc);
_n /= r;
_nc /= r;
}
tphi0 = _n / _nc;
} else {
tphi0 = tphi1;
_nc = 1/hyp(tphi0);
_n = tphi0 * _nc;
if (polar)
_nc = 0;
}
_scbet0 = hyp(_fm * tphi0);
real shxi0 = sinh(Math::eatanhe(_n, _es));
_tchi0 = tphi0 * hyp(shxi0) - shxi0 * hyp(tphi0); _scchi0 = hyp(_tchi0);
_psi0 = asinh(_tchi0);
_lat0 = atan(_sign * tphi0) / Math::degree();
_t0nm1 = expm1(- _n * _psi0); // Snyder's t0^n - 1
// a * k1 * m1/t1^n = a * k1 * m2/t2^n = a * k1 * n * (Snyder's F)
// = a * k1 / (scbet1 * exp(-n * psi1))
_scale = _a * k1 / scbet1 *
// exp(n * psi1) = exp(- (1 - n) * psi1) * exp(psi1)
// with (1-n) = nc^2/(1+n) and exp(-psi1) = scchi1 + tchi1
exp( - (Math::sq(_nc)/(1 + _n)) * psi1 )
* (tchi1 >= 0 ? scchi1 + tchi1 : 1 / (scchi1 - tchi1));
// Scale at phi0 = k0 = k1 * (scbet0*exp(-n*psi0))/(scbet1*exp(-n*psi1))
// = k1 * scbet0/scbet1 * exp(n * (psi1 - psi0))
// psi1 - psi0 = Dasinh(tchi1, tchi0) * (tchi1 - tchi0)
_k0 = k1 * (_scbet0/scbet1) *
exp( - (Math::sq(_nc)/(1 + _n)) *
Dasinh(tchi1, _tchi0, scchi1, _scchi0) * (tchi1 - _tchi0))
* (tchi1 >= 0 ? scchi1 + tchi1 : 1 / (scchi1 - tchi1)) /
(_scchi0 + _tchi0);
_nrho0 = polar ? 0 : _a * _k0 / _scbet0;
{
// Figure _drhomax using code at beginning of Forward with lat = -90
real
sphi = -1, cphi = epsx_,
tphi = sphi/cphi,
scphi = 1/cphi, shxi = sinh(Math::eatanhe(sphi, _es)),
tchi = hyp(shxi) * tphi - shxi * scphi, scchi = hyp(tchi),
psi = asinh(tchi),
dpsi = Dasinh(tchi, _tchi0, scchi, _scchi0) * (tchi - _tchi0);
_drhomax = - _scale * (2 * _nc < 1 && dpsi != 0 ?
(exp(Math::sq(_nc)/(1 + _n) * psi ) *
(tchi > 0 ? 1/(scchi + tchi) : (scchi - tchi))
- (_t0nm1 + 1))/(-_n) :
Dexp(-_n * psi, -_n * _psi0) * dpsi);
}
}
const LambertConformalConic& LambertConformalConic::Mercator() {
static const LambertConformalConic mercator(Constants::WGS84_a(),
Constants::WGS84_f(),
real(0), real(1));
return mercator;
}
void LambertConformalConic::Forward(real lon0, real lat, real lon,
real& x, real& y,
real& gamma, real& k) const {
lon = Math::AngDiff(lon0, lon);
// From Snyder, we have
//
// theta = n * lambda
// x = rho * sin(theta)
// = (nrho0 + n * drho) * sin(theta)/n
// y = rho0 - rho * cos(theta)
// = nrho0 * (1-cos(theta))/n - drho * cos(theta)
//
// where nrho0 = n * rho0, drho = rho - rho0
// and drho is evaluated with divided differences
real sphi, cphi;
Math::sincosd(Math::LatFix(lat) * _sign, sphi, cphi);
cphi = fmax(epsx_, cphi);
real
lam = lon * Math::degree(),
tphi = sphi/cphi, scbet = hyp(_fm * tphi),
scphi = 1/cphi, shxi = sinh(Math::eatanhe(sphi, _es)),
tchi = hyp(shxi) * tphi - shxi * scphi, scchi = hyp(tchi),
psi = asinh(tchi),
theta = _n * lam, stheta = sin(theta), ctheta = cos(theta),
dpsi = Dasinh(tchi, _tchi0, scchi, _scchi0) * (tchi - _tchi0),
drho = - _scale * (2 * _nc < 1 && dpsi != 0 ?
(exp(Math::sq(_nc)/(1 + _n) * psi ) *
(tchi > 0 ? 1/(scchi + tchi) : (scchi - tchi))
- (_t0nm1 + 1))/(-_n) :
Dexp(-_n * psi, -_n * _psi0) * dpsi);
x = (_nrho0 + _n * drho) * (_n != 0 ? stheta / _n : lam);
y = _nrho0 *
(_n != 0 ?
(ctheta < 0 ? 1 - ctheta : Math::sq(stheta)/(1 + ctheta)) / _n : 0)
- drho * ctheta;
k = _k0 * (scbet/_scbet0) /
(exp( - (Math::sq(_nc)/(1 + _n)) * dpsi )
* (tchi >= 0 ? scchi + tchi : 1 / (scchi - tchi)) / (_scchi0 + _tchi0));
y *= _sign;
gamma = _sign * theta / Math::degree();
}
void LambertConformalConic::Reverse(real lon0, real x, real y,
real& lat, real& lon,
real& gamma, real& k) const {
// From Snyder, we have
//
// x = rho * sin(theta)
// rho0 - y = rho * cos(theta)
//
// rho = hypot(x, rho0 - y)
// drho = (n*x^2 - 2*y*nrho0 + n*y^2)/(hypot(n*x, nrho0-n*y) + nrho0)
// theta = atan2(n*x, nrho0-n*y)
//
// From drho, obtain t^n-1
// psi = -log(t), so
// dpsi = - Dlog1p(t^n-1, t0^n-1) * drho / scale
y *= _sign;
real
// Guard against 0 * inf in computation of ny
nx = _n * x, ny = _n != 0 ? _n * y : 0, y1 = _nrho0 - ny,
den = hypot(nx, y1) + _nrho0, // 0 implies origin with polar aspect
// isfinite test is to avoid inf/inf
drho = ((den != 0 && isfinite(den))
? (x*nx + y * (ny - 2*_nrho0)) / den
: den);
drho = fmin(drho, _drhomax);
if (_n == 0)
drho = fmax(drho, -_drhomax);
real
tnm1 = _t0nm1 + _n * drho/_scale,
dpsi = (den == 0 ? 0 :
(tnm1 + 1 != 0 ? - Dlog1p(tnm1, _t0nm1) * drho / _scale :
ahypover_));
real tchi;
if (2 * _n <= 1) {
// tchi = sinh(psi)
real
psi = _psi0 + dpsi, tchia = sinh(psi), scchi = hyp(tchia),
dtchi = Dsinh(psi, _psi0, tchia, _tchi0, scchi, _scchi0) * dpsi;
tchi = _tchi0 + dtchi; // Update tchi using divided difference
} else {
// tchi = sinh(-1/n * log(tn))
// = sinh((1-1/n) * log(tn) - log(tn))
// = + sinh((1-1/n) * log(tn)) * cosh(log(tn))
// - cosh((1-1/n) * log(tn)) * sinh(log(tn))
// (1-1/n) = - nc^2/(n*(1+n))
// cosh(log(tn)) = (tn + 1/tn)/2; sinh(log(tn)) = (tn - 1/tn)/2
real
tn = tnm1 + 1 == 0 ? epsx_ : tnm1 + 1,
sh = sinh( -Math::sq(_nc)/(_n * (1 + _n)) *
(2 * tn > 1 ? log1p(tnm1) : log(tn)) );
tchi = sh * (tn + 1/tn)/2 - hyp(sh) * (tnm1 * (tn + 1)/tn)/2;
}
// log(t) = -asinh(tan(chi)) = -psi
gamma = atan2(nx, y1);
real
tphi = Math::tauf(tchi, _es),
scbet = hyp(_fm * tphi), scchi = hyp(tchi),
lam = _n != 0 ? gamma / _n : x / y1;
lat = Math::atand(_sign * tphi);
lon = lam / Math::degree();
lon = Math::AngNormalize(lon + Math::AngNormalize(lon0));
k = _k0 * (scbet/_scbet0) /
(exp(_nc != 0 ? - (Math::sq(_nc)/(1 + _n)) * dpsi : 0)
* (tchi >= 0 ? scchi + tchi : 1 / (scchi - tchi)) / (_scchi0 + _tchi0));
gamma /= _sign * Math::degree();
}
void LambertConformalConic::SetScale(real lat, real k) {
if (!(isfinite(k) && k > 0))
throw GeographicErr("Scale is not positive");
if (!(fabs(lat) <= Math::qd))
throw GeographicErr("Latitude for SetScale not in [-"
+ to_string(Math::qd) + "d, "
+ to_string(Math::qd) + "d]");
if (fabs(lat) == Math::qd && !(_nc == 0 && lat * _n > 0))
throw GeographicErr("Incompatible polar latitude in SetScale");
real x, y, gamma, kold;
Forward(0, lat, 0, x, y, gamma, kold);
k /= kold;
_scale *= k;
_k0 *= k;
}
} // namespace GeographicLib

62
libs/geographiclib/src/LocalCartesian.cpp Normal file
View File

@@ -0,0 +1,62 @@
/**
* \file LocalCartesian.cpp
* \brief Implementation for GeographicLib::LocalCartesian class
*
* Copyright (c) Charles Karney (2008-2015) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/LocalCartesian.hpp>
namespace GeographicLib {
using namespace std;
void LocalCartesian::Reset(real lat0, real lon0, real h0) {
_lat0 = Math::LatFix(lat0);
_lon0 = Math::AngNormalize(lon0);
_h0 = h0;
_earth.Forward(_lat0, _lon0, _h0, _x0, _y0, _z0);
real sphi, cphi, slam, clam;
Math::sincosd(_lat0, sphi, cphi);
Math::sincosd(_lon0, slam, clam);
Geocentric::Rotation(sphi, cphi, slam, clam, _r);
}
void LocalCartesian::MatrixMultiply(real M[dim2_]) const {
// M = r' . M
real t[dim2_];
copy(M, M + dim2_, t);
for (size_t i = 0; i < dim2_; ++i) {
size_t row = i / dim_, col = i % dim_;
M[i] = _r[row] * t[col] + _r[row+3] * t[col+3] + _r[row+6] * t[col+6];
}
}
void LocalCartesian::IntForward(real lat, real lon, real h,
real& x, real& y, real& z,
real M[dim2_]) const {
real xc, yc, zc;
_earth.IntForward(lat, lon, h, xc, yc, zc, M);
xc -= _x0; yc -= _y0; zc -= _z0;
x = _r[0] * xc + _r[3] * yc + _r[6] * zc;
y = _r[1] * xc + _r[4] * yc + _r[7] * zc;
z = _r[2] * xc + _r[5] * yc + _r[8] * zc;
if (M)
MatrixMultiply(M);
}
void LocalCartesian::IntReverse(real x, real y, real z,
real& lat, real& lon, real& h,
real M[dim2_]) const {
real
xc = _x0 + _r[0] * x + _r[1] * y + _r[2] * z,
yc = _y0 + _r[3] * x + _r[4] * y + _r[5] * z,
zc = _z0 + _r[6] * x + _r[7] * y + _r[8] * z;
_earth.IntReverse(xc, yc, zc, lat, lon, h, M);
if (M)
MatrixMultiply(M);
}
} // namespace GeographicLib

515
libs/geographiclib/src/MGRS.cpp Normal file
View File

@@ -0,0 +1,515 @@
/**
* \file MGRS.cpp
* \brief Implementation for GeographicLib::MGRS class
*
* Copyright (c) Charles Karney (2008-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/MGRS.hpp>
#include <GeographicLib/Utility.hpp>
#if defined(_MSC_VER)
// Squelch warnings about enum-float expressions and mixing enums
# pragma warning (disable: 5055 5054)
#endif
namespace GeographicLib {
using namespace std;
const char* const MGRS::hemispheres_ = "SN";
const char* const MGRS::utmcols_[] = { "ABCDEFGH", "JKLMNPQR", "STUVWXYZ" };
const char* const MGRS::utmrow_ = "ABCDEFGHJKLMNPQRSTUV";
const char* const MGRS::upscols_[] =
{ "JKLPQRSTUXYZ", "ABCFGHJKLPQR", "RSTUXYZ", "ABCFGHJ" };
const char* const MGRS::upsrows_[] =
{ "ABCDEFGHJKLMNPQRSTUVWXYZ", "ABCDEFGHJKLMNP" };
const char* const MGRS::latband_ = "CDEFGHJKLMNPQRSTUVWX";
const char* const MGRS::upsband_ = "ABYZ";
const char* const MGRS::digits_ = "0123456789";
const char* const MGRS::alpha_ = // Omit I+O
"ABCDEFGHJKLMNPQRSTUVWXYZabcdefghjklmnpqrstuvwxyz";
const int MGRS::mineasting_[] =
{ minupsSind_, minupsNind_, minutmcol_, minutmcol_ };
const int MGRS::maxeasting_[] =
{ maxupsSind_, maxupsNind_, maxutmcol_, maxutmcol_ };
const int MGRS::minnorthing_[] =
{ minupsSind_, minupsNind_,
minutmSrow_, minutmSrow_ - (maxutmSrow_ - minutmNrow_) };
const int MGRS::maxnorthing_[] =
{ maxupsSind_, maxupsNind_,
maxutmNrow_ + (maxutmSrow_ - minutmNrow_), maxutmNrow_ };
void MGRS::Forward(int zone, bool northp, real x, real y, real lat,
int prec, std::string& mgrs) {
using std::isnan; // Needed for Centos 7, ubuntu 14
// The smallest angle s.t., 90 - angeps() < 90 (approx 50e-12 arcsec)
// 7 = ceil(log_2(90))
static const real angeps = ldexp(real(1), -(Math::digits() - 7));
if (zone == UTMUPS::INVALID ||
isnan(x) || isnan(y) || isnan(lat)) {
mgrs = "INVALID";
return;
}
bool utmp = zone != 0;
CheckCoords(utmp, northp, x, y);
if (!(zone >= UTMUPS::MINZONE && zone <= UTMUPS::MAXZONE))
throw GeographicErr("Zone " + Utility::str(zone) + " not in [0,60]");
if (!(prec >= -1 && prec <= maxprec_))
throw GeographicErr("MGRS precision " + Utility::str(prec)
+ " not in [-1, "
+ Utility::str(int(maxprec_)) + "]");
// Fixed char array for accumulating string. Allow space for zone, 3 block
// letters, easting + northing. Don't need to allow for terminating null.
char mgrs1[2 + 3 + 2 * maxprec_];
int
zone1 = zone - 1,
z = utmp ? 2 : 0,
mlen = z + 3 + 2 * prec;
if (utmp) {
mgrs1[0] = digits_[ zone / base_ ];
mgrs1[1] = digits_[ zone % base_ ];
// This isn't necessary...! Keep y non-neg
// if (!northp) y -= maxutmSrow_ * tile_;
}
// The C++ standard mandates 64 bits for long long. But
// check, to make sure.
static_assert(numeric_limits<long long>::digits >= 44,
"long long not wide enough to store 10e12");
// Guard against floor(x * mult_) being computed incorrectly on some
// platforms. The problem occurs when x * mult_ is held in extended
// precision and floor is inlined. This causes tests GeoConvert1[678] to
// fail. Problem reported and diagnosed by Thorkil Naur with g++ 10.2.0
// under Cygwin.
GEOGRAPHICLIB_VOLATILE real xx = x * mult_;
GEOGRAPHICLIB_VOLATILE real yy = y * mult_;
long long
ix = (long long)(floor(xx)),
iy = (long long)(floor(yy)),
m = (long long)(mult_) * (long long)(tile_);
int xh = int(ix / m), yh = int(iy / m);
if (utmp) {
int
// Correct fuzziness in latitude near equator
iband = fabs(lat) < angeps ? (northp ? 0 : -1) : LatitudeBand(lat),
icol = xh - minutmcol_,
irow = UTMRow(iband, icol, yh % utmrowperiod_);
if (irow != yh - (northp ? minutmNrow_ : maxutmSrow_))
throw GeographicErr("Latitude " + Utility::str(lat)
+ " is inconsistent with UTM coordinates");
mgrs1[z++] = latband_[10 + iband];
mgrs1[z++] = utmcols_[zone1 % 3][icol];
mgrs1[z++] = utmrow_[(yh + (zone1 & 1 ? utmevenrowshift_ : 0))
% utmrowperiod_];
} else {
bool eastp = xh >= upseasting_;
int iband = (northp ? 2 : 0) + (eastp ? 1 : 0);
mgrs1[z++] = upsband_[iband];
mgrs1[z++] = upscols_[iband][xh - (eastp ? upseasting_ :
(northp ? minupsNind_ :
minupsSind_))];
mgrs1[z++] = upsrows_[northp][yh - (northp ? minupsNind_ : minupsSind_)];
}
if (prec > 0) {
ix -= m * xh; iy -= m * yh;
long long d = (long long)(pow(real(base_), maxprec_ - prec));
ix /= d; iy /= d;
for (int c = prec; c--;) {
mgrs1[z + c ] = digits_[ix % base_]; ix /= base_;
mgrs1[z + c + prec] = digits_[iy % base_]; iy /= base_;
}
}
mgrs.resize(mlen);
copy(mgrs1, mgrs1 + mlen, mgrs.begin());
}
void MGRS::Forward(int zone, bool northp, real x, real y,
int prec, std::string& mgrs) {
real lat, lon;
if (zone > 0) {
// Does a rough estimate for latitude determine the latitude band?
real ys = northp ? y : y - utmNshift_;
// A cheap calculation of the latitude which results in an "allowed"
// latitude band would be
// lat = ApproxLatitudeBand(ys) * 8 + 4;
//
// Here we do a more careful job using the band letter corresponding to
// the actual latitude.
ys /= tile_;
if (fabs(ys) < 1)
lat = real(0.9) * ys; // accurate enough estimate near equator
else {
real
// The poleward bound is a fit from above of lat(x,y)
// for x = 500km and y = [0km, 950km]
latp = real(0.901) * ys + (ys > 0 ? 1 : -1) * real(0.135),
// The equatorward bound is a fit from below of lat(x,y)
// for x = 900km and y = [0km, 950km]
late = real(0.902) * ys * (1 - real(1.85e-6) * ys * ys);
if (LatitudeBand(latp) == LatitudeBand(late))
lat = latp;
else
// bounds straddle a band boundary so need to compute lat accurately
UTMUPS::Reverse(zone, northp, x, y, lat, lon);
}
} else
// Latitude isn't needed for UPS specs or for INVALID
lat = 0;
Forward(zone, northp, x, y, lat, prec, mgrs);
}
void MGRS::Reverse(const string& mgrs,
int& zone, bool& northp, real& x, real& y,
int& prec, bool centerp) {
int
p = 0,
len = int(mgrs.length());
if (len >= 3 &&
toupper(mgrs[0]) == 'I' &&
toupper(mgrs[1]) == 'N' &&
toupper(mgrs[2]) == 'V') {
zone = UTMUPS::INVALID;
northp = false;
x = y = Math::NaN();
prec = -2;
return;
}
int zone1 = 0;
while (p < len) {
int i = Utility::lookup(digits_, mgrs[p]);
if (i < 0)
break;
zone1 = 10 * zone1 + i;
++p;
}
if (p > 0 && !(zone1 >= UTMUPS::MINUTMZONE && zone1 <= UTMUPS::MAXUTMZONE))
throw GeographicErr("Zone " + Utility::str(zone1) + " not in [1,60]");
if (p > 2)
throw GeographicErr("More than 2 digits at start of MGRS "
+ mgrs.substr(0, p));
if (len - p < 1)
throw GeographicErr("MGRS string too short " + mgrs);
bool utmp = zone1 != UTMUPS::UPS;
int zonem1 = zone1 - 1;
const char* band = utmp ? latband_ : upsband_;
int iband = Utility::lookup(band, mgrs[p++]);
if (iband < 0)
throw GeographicErr("Band letter " + Utility::str(mgrs[p-1]) + " not in "
+ (utmp ? "UTM" : "UPS") + " set " + band);
bool northp1 = iband >= (utmp ? 10 : 2);
if (p == len) { // Grid zone only (ignore centerp)
// Approx length of a degree of meridian arc in units of tile.
real deg = real(utmNshift_) / (Math::qd * tile_);
zone = zone1;
northp = northp1;
if (utmp) {
// Pick central meridian except for 31V
x = ((zone == 31 && iband == 17) ? 4 : 5) * tile_;
// Pick center of 8deg latitude bands
y = floor(8 * (iband - real(9.5)) * deg + real(0.5)) * tile_
+ (northp ? 0 : utmNshift_);
} else {
// Pick point at lat 86N or 86S
x = ((iband & 1 ? 1 : -1) * floor(4 * deg + real(0.5))
+ upseasting_) * tile_;
// Pick point at lon 90E or 90W.
y = upseasting_ * tile_;
}
prec = -1;
return;
} else if (len - p < 2)
throw GeographicErr("Missing row letter in " + mgrs);
const char* col = utmp ? utmcols_[zonem1 % 3] : upscols_[iband];
const char* row = utmp ? utmrow_ : upsrows_[northp1];
int icol = Utility::lookup(col, mgrs[p++]);
if (icol < 0)
throw GeographicErr("Column letter " + Utility::str(mgrs[p-1])
+ " not in "
+ (utmp ? "zone " + mgrs.substr(0, p-2) :
"UPS band " + Utility::str(mgrs[p-2]))
+ " set " + col );
int irow = Utility::lookup(row, mgrs[p++]);
if (irow < 0)
throw GeographicErr("Row letter " + Utility::str(mgrs[p-1]) + " not in "
+ (utmp ? "UTM" :
"UPS " + Utility::str(hemispheres_[northp1]))
+ " set " + row);
if (utmp) {
if (zonem1 & 1)
irow = (irow + utmrowperiod_ - utmevenrowshift_) % utmrowperiod_;
iband -= 10;
irow = UTMRow(iband, icol, irow);
if (irow == maxutmSrow_)
throw GeographicErr("Block " + mgrs.substr(p-2, 2)
+ " not in zone/band " + mgrs.substr(0, p-2));
irow = northp1 ? irow : irow + 100;
icol = icol + minutmcol_;
} else {
bool eastp = iband & 1;
icol += eastp ? upseasting_ : (northp1 ? minupsNind_ : minupsSind_);
irow += northp1 ? minupsNind_ : minupsSind_;
}
int prec1 = (len - p)/2;
real
unit = 1,
x1 = icol,
y1 = irow;
for (int i = 0; i < prec1; ++i) {
unit *= base_;
int
ix = Utility::lookup(digits_, mgrs[p + i]),
iy = Utility::lookup(digits_, mgrs[p + i + prec1]);
if (ix < 0 || iy < 0)
throw GeographicErr("Encountered a non-digit in " + mgrs.substr(p));
x1 = base_ * x1 + ix;
y1 = base_ * y1 + iy;
}
if ((len - p) % 2) {
if (Utility::lookup(digits_, mgrs[len - 1]) < 0)
throw GeographicErr("Encountered a non-digit in " + mgrs.substr(p));
else
throw GeographicErr("Not an even number of digits in "
+ mgrs.substr(p));
}
if (prec1 > maxprec_)
throw GeographicErr("More than " + Utility::str(2*maxprec_)
+ " digits in " + mgrs.substr(p));
if (centerp) {
unit *= 2; x1 = 2 * x1 + 1; y1 = 2 * y1 + 1;
}
zone = zone1;
northp = northp1;
x = (tile_ * x1) / unit;
y = (tile_ * y1) / unit;
prec = prec1;
}
void MGRS::CheckCoords(bool utmp, bool& northp, real& x, real& y) {
// Limits are all multiples of 100km and are all closed on the lower end
// and open on the upper end -- and this is reflected in the error
// messages. However if a coordinate lies on the excluded upper end (e.g.,
// after rounding), it is shifted down by eps. This also folds UTM
// northings to the correct N/S hemisphere.
// The smallest length s.t., 1.0e7 - eps() < 1.0e7 (approx 1.9 nm)
// 25 = ceil(log_2(2e7)) -- use half circumference here because
// northing 195e5 is a legal in the "southern" hemisphere.
static const real eps = ldexp(real(1), -(Math::digits() - 25));
int
ix = int(floor(x / tile_)),
iy = int(floor(y / tile_)),
ind = (utmp ? 2 : 0) + (northp ? 1 : 0);
if (! (ix >= mineasting_[ind] && ix < maxeasting_[ind]) ) {
if (ix == maxeasting_[ind] && x == maxeasting_[ind] * tile_)
x -= eps;
else
throw GeographicErr("Easting " + Utility::str(int(floor(x/1000)))
+ "km not in MGRS/"
+ (utmp ? "UTM" : "UPS") + " range for "
+ (northp ? "N" : "S" ) + " hemisphere ["
+ Utility::str(mineasting_[ind]*tile_/1000)
+ "km, "
+ Utility::str(maxeasting_[ind]*tile_/1000)
+ "km)");
}
if (! (iy >= minnorthing_[ind] && iy < maxnorthing_[ind]) ) {
if (iy == maxnorthing_[ind] && y == maxnorthing_[ind] * tile_)
y -= eps;
else
throw GeographicErr("Northing " + Utility::str(int(floor(y/1000)))
+ "km not in MGRS/"
+ (utmp ? "UTM" : "UPS") + " range for "
+ (northp ? "N" : "S" ) + " hemisphere ["
+ Utility::str(minnorthing_[ind]*tile_/1000)
+ "km, "
+ Utility::str(maxnorthing_[ind]*tile_/1000)
+ "km)");
}
// Correct the UTM northing and hemisphere if necessary
if (utmp) {
if (northp && iy < minutmNrow_) {
northp = false;
y += utmNshift_;
} else if (!northp && iy >= maxutmSrow_) {
if (y == maxutmSrow_ * tile_)
// If on equator retain S hemisphere
y -= eps;
else {
northp = true;
y -= utmNshift_;
}
}
}
}
int MGRS::UTMRow(int iband, int icol, int irow) {
// Input is iband = band index in [-10, 10) (as returned by LatitudeBand),
// icol = column index in [0,8) with origin of easting = 100km, and irow =
// periodic row index in [0,20) with origin = equator. Output is true row
// index in [-90, 95). Returns maxutmSrow_ = 100, if irow and iband are
// incompatible.
// Estimate center row number for latitude band
// 90 deg = 100 tiles; 1 band = 8 deg = 100*8/90 tiles
real c = 100 * (8 * iband + 4) / real(Math::qd);
bool northp = iband >= 0;
// These are safe bounds on the rows
// iband minrow maxrow
// -10 -90 -81
// -9 -80 -72
// -8 -71 -63
// -7 -63 -54
// -6 -54 -45
// -5 -45 -36
// -4 -36 -27
// -3 -27 -18
// -2 -18 -9
// -1 -9 -1
// 0 0 8
// 1 8 17
// 2 17 26
// 3 26 35
// 4 35 44
// 5 44 53
// 6 53 62
// 7 62 70
// 8 71 79
// 9 80 94
int
minrow = iband > -10 ?
int(floor(c - real(4.3) - real(0.1) * northp)) : -90,
maxrow = iband < 9 ?
int(floor(c + real(4.4) - real(0.1) * northp)) : 94,
baserow = (minrow + maxrow) / 2 - utmrowperiod_ / 2;
// Offset irow by the multiple of utmrowperiod_ which brings it as close as
// possible to the center of the latitude band, (minrow + maxrow) / 2.
// (Add maxutmSrow_ = 5 * utmrowperiod_ to ensure operand is positive.)
irow = (irow - baserow + maxutmSrow_) % utmrowperiod_ + baserow;
if (!( irow >= minrow && irow <= maxrow )) {
// Outside the safe bounds, so need to check...
// Northing = 71e5 and 80e5 intersect band boundaries
// y = 71e5 in scol = 2 (x = [3e5,4e5] and x = [6e5,7e5])
// y = 80e5 in scol = 1 (x = [2e5,3e5] and x = [7e5,8e5])
// This holds for all the ellipsoids given in NGA.SIG.0012_2.0.0_UTMUPS.
// The following deals with these special cases.
int
// Fold [-10,-1] -> [9,0]
sband = iband >= 0 ? iband : -iband - 1,
// Fold [-90,-1] -> [89,0]
srow = irow >= 0 ? irow : -irow - 1,
// Fold [4,7] -> [3,0]
scol = icol < 4 ? icol : -icol + 7;
// For example, the safe rows for band 8 are 71 - 79. However row 70 is
// allowed if scol = [2,3] and row 80 is allowed if scol = [0,1].
if ( ! ( (srow == 70 && sband == 8 && scol >= 2) ||
(srow == 71 && sband == 7 && scol <= 2) ||
(srow == 79 && sband == 9 && scol >= 1) ||
(srow == 80 && sband == 8 && scol <= 1) ) )
irow = maxutmSrow_;
}
return irow;
}
void MGRS::Decode(const string& mgrs,
string& gridzone, string& block,
string& easting, string& northing) {
string::size_type n = mgrs.length();
if (n >= 3 &&
toupper(mgrs[0]) == 'I' &&
toupper(mgrs[1]) == 'N' &&
toupper(mgrs[2]) == 'V') {
gridzone = mgrs.substr(0, 3);
block = easting = northing = "";
return;
}
string::size_type p0 = mgrs.find_first_not_of(digits_);
if (p0 == string::npos)
throw GeographicErr("MGRS::Decode: ref does not contain alpha chars");
if (!(p0 <= 2))
throw GeographicErr("MGRS::Decode: ref does not start with 0-2 digits");
string::size_type p1 = mgrs.find_first_of(alpha_, p0);
if (p1 != p0)
throw GeographicErr("MGRS::Decode: ref contains non alphanumeric chars");
p1 = min(mgrs.find_first_not_of(alpha_, p0), n);
if (!(p1 == p0 + 1 || p1 == p0 + 3))
throw GeographicErr("MGRS::Decode: ref must contain 1 or 3 alpha chars");
if (p1 == p0 + 1 && p1 < n)
throw GeographicErr("MGRS::Decode: ref contains junk after 1 alpha char");
if (p1 < n && (mgrs.find_first_of(digits_, p1) != p1 ||
mgrs.find_first_not_of(digits_, p1) != string::npos))
throw GeographicErr("MGRS::Decode: ref contains junk at end");
if ((n - p1) & 1u)
throw GeographicErr("MGRS::Decode: ref must end with even no of digits");
// Here [0, p0) = initial digits; [p0, p1) = alpha; [p1, n) = end digits
gridzone = mgrs.substr(0, p0+1);
block = mgrs.substr(p0+1, p1 - (p0 + 1));
easting = mgrs.substr(p1, (n - p1) / 2);
northing = mgrs.substr(p1 + (n - p1) / 2);
}
void MGRS::Check() {
real lat, lon, x, y, t = tile_; int zone; bool northp;
UTMUPS::Reverse(31, true , 1*t, 0*t, lat, lon);
if (!( lon < 0 ))
throw GeographicErr("MGRS::Check: equator coverage failure");
UTMUPS::Reverse(31, true , 1*t, 95*t, lat, lon);
if (!( lat > 84 ))
throw GeographicErr("MGRS::Check: UTM doesn't reach latitude = 84");
UTMUPS::Reverse(31, false, 1*t, 10*t, lat, lon);
if (!( lat < -80 ))
throw GeographicErr("MGRS::Check: UTM doesn't reach latitude = -80");
UTMUPS::Forward(56, 3, zone, northp, x, y, 32);
if (!( x > 1*t ))
throw GeographicErr("MGRS::Check: Norway exception creates a gap");
UTMUPS::Forward(72, 21, zone, northp, x, y, 35);
if (!( x > 1*t ))
throw GeographicErr("MGRS::Check: Svalbard exception creates a gap");
UTMUPS::Reverse(0, true , 20*t, 13*t, lat, lon);
if (!( lat < 84 ))
throw
GeographicErr("MGRS::Check: North UPS doesn't reach latitude = 84");
UTMUPS::Reverse(0, false, 20*t, 8*t, lat, lon);
if (!( lat > -80 ))
throw
GeographicErr("MGRS::Check: South UPS doesn't reach latitude = -80");
// Entries are [band, x, y] either side of the band boundaries. Units for
// x, y are t = 100km.
const short tab[] = {
0, 5, 0, 0, 9, 0, // south edge of band 0
0, 5, 8, 0, 9, 8, // north edge of band 0
1, 5, 9, 1, 9, 9, // south edge of band 1
1, 5, 17, 1, 9, 17, // north edge of band 1
2, 5, 18, 2, 9, 18, // etc.
2, 5, 26, 2, 9, 26,
3, 5, 27, 3, 9, 27,
3, 5, 35, 3, 9, 35,
4, 5, 36, 4, 9, 36,
4, 5, 44, 4, 9, 44,
5, 5, 45, 5, 9, 45,
5, 5, 53, 5, 9, 53,
6, 5, 54, 6, 9, 54,
6, 5, 62, 6, 9, 62,
7, 5, 63, 7, 9, 63,
7, 5, 70, 7, 7, 70, 7, 7, 71, 7, 9, 71, // y = 71t crosses boundary
8, 5, 71, 8, 6, 71, 8, 6, 72, 8, 9, 72, // between bands 7 and 8.
8, 5, 79, 8, 8, 79, 8, 8, 80, 8, 9, 80, // y = 80t crosses boundary
9, 5, 80, 9, 7, 80, 9, 7, 81, 9, 9, 81, // between bands 8 and 9.
9, 5, 95, 9, 9, 95, // north edge of band 9
};
const int bandchecks = sizeof(tab) / (3 * sizeof(short));
for (int i = 0; i < bandchecks; ++i) {
UTMUPS::Reverse(38, true, tab[3*i+1]*t, tab[3*i+2]*t, lat, lon);
if (!( LatitudeBand(lat) == tab[3*i+0] ))
throw GeographicErr("MGRS::Check: Band error, b = " +
Utility::str(tab[3*i+0]) + ", x = " +
Utility::str(tab[3*i+1]) + "00km, y = " +
Utility::str(tab[3*i+2]) + "00km");
}
}
} // namespace GeographicLib

67
libs/geographiclib/src/MagneticCircle.cpp Normal file
View File

@@ -0,0 +1,67 @@
/**
* \file MagneticCircle.cpp
* \brief Implementation for GeographicLib::MagneticCircle class
*
* Copyright (c) Charles Karney (2011-2021) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/MagneticCircle.hpp>
#include <fstream>
#include <sstream>
#include <GeographicLib/Geocentric.hpp>
namespace GeographicLib {
using namespace std;
void MagneticCircle::FieldGeocentric(real slam, real clam,
real& BX, real& BY, real& BZ,
real& BXt, real& BYt, real& BZt) const {
real BXc = 0, BYc = 0, BZc = 0;
_circ0(slam, clam, BX, BY, BZ);
_circ1(slam, clam, BXt, BYt, BZt);
if (_constterm)
_circ2(slam, clam, BXc, BYc, BZc);
if (_interpolate) {
BXt = (BXt - BX) / _dt0;
BYt = (BYt - BY) / _dt0;
BZt = (BZt - BZ) / _dt0;
}
BX += _t1 * BXt + BXc;
BY += _t1 * BYt + BYc;
BZ += _t1 * BZt + BZc;
BXt *= - _a;
BYt *= - _a;
BZt *= - _a;
BX *= - _a;
BY *= - _a;
BZ *= - _a;
}
void MagneticCircle::FieldGeocentric(real lon,
real& BX, real& BY, real& BZ,
real& BXt, real& BYt, real& BZt) const {
real slam, clam;
Math::sincosd(lon, slam, clam);
FieldGeocentric(slam, clam, BX, BY, BZ, BXt, BYt, BZt);
}
void MagneticCircle::Field(real lon, bool diffp,
real& Bx, real& By, real& Bz,
real& Bxt, real& Byt, real& Bzt) const {
real slam, clam;
Math::sincosd(lon, slam, clam);
real M[Geocentric::dim2_];
Geocentric::Rotation(_sphi, _cphi, slam, clam, M);
real BX, BY, BZ, BXt, BYt, BZt; // Components in geocentric basis
FieldGeocentric(slam, clam, BX, BY, BZ, BXt, BYt, BZt);
if (diffp)
Geocentric::Unrotate(M, BXt, BYt, BZt, Bxt, Byt, Bzt);
Geocentric::Unrotate(M, BX, BY, BZ, Bx, By, Bz);
}
} // namespace GeographicLib

290
libs/geographiclib/src/MagneticModel.cpp Normal file
View File

@@ -0,0 +1,290 @@
/**
* \file MagneticModel.cpp
* \brief Implementation for GeographicLib::MagneticModel class
*
* Copyright (c) Charles Karney (2011-2021) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/MagneticModel.hpp>
#include <fstream>
#include <GeographicLib/SphericalEngine.hpp>
#include <GeographicLib/MagneticCircle.hpp>
#include <GeographicLib/Utility.hpp>
#if !defined(GEOGRAPHICLIB_DATA)
# if defined(_WIN32)
# define GEOGRAPHICLIB_DATA "C:/ProgramData/GeographicLib"
# else
# define GEOGRAPHICLIB_DATA "/usr/local/share/GeographicLib"
# endif
#endif
#if !defined(GEOGRAPHICLIB_MAGNETIC_DEFAULT_NAME)
# define GEOGRAPHICLIB_MAGNETIC_DEFAULT_NAME "wmm2020"
#endif
#if defined(_MSC_VER)
// Squelch warnings about unsafe use of getenv
# pragma warning (disable: 4996)
#endif
namespace GeographicLib {
using namespace std;
MagneticModel::MagneticModel(const std::string& name, const std::string& path,
const Geocentric& earth, int Nmax, int Mmax)
: _name(name)
, _dir(path)
, _description("NONE")
, _date("UNKNOWN")
, _t0(Math::NaN())
, _dt0(1)
, _tmin(Math::NaN())
, _tmax(Math::NaN())
, _a(Math::NaN())
, _hmin(Math::NaN())
, _hmax(Math::NaN())
, _nNmodels(1)
, _nNconstants(0)
, _nmx(-1)
, _mmx(-1)
, _norm(SphericalHarmonic::SCHMIDT)
, _earth(earth)
{
if (_dir.empty())
_dir = DefaultMagneticPath();
bool truncate = Nmax >= 0 || Mmax >= 0;
if (truncate) {
if (Nmax >= 0 && Mmax < 0) Mmax = Nmax;
if (Nmax < 0) Nmax = numeric_limits<int>::max();
if (Mmax < 0) Mmax = numeric_limits<int>::max();
}
ReadMetadata(_name);
_gG.resize(_nNmodels + 1 + _nNconstants);
_hH.resize(_nNmodels + 1 + _nNconstants);
{
string coeff = _filename + ".cof";
ifstream coeffstr(coeff.c_str(), ios::binary);
if (!coeffstr.good())
throw GeographicErr("Error opening " + coeff);
char id[idlength_ + 1];
coeffstr.read(id, idlength_);
if (!coeffstr.good())
throw GeographicErr("No header in " + coeff);
id[idlength_] = '\0';
if (_id != string(id))
throw GeographicErr("ID mismatch: " + _id + " vs " + id);
for (int i = 0; i < _nNmodels + 1 + _nNconstants; ++i) {
int N, M;
if (truncate) { N = Nmax; M = Mmax; }
SphericalEngine::coeff::readcoeffs(coeffstr, N, M, _gG[i], _hH[i],
truncate);
if (!(M < 0 || _gG[i][0] == 0))
throw GeographicErr("A degree 0 term is not permitted");
_harm.push_back(SphericalHarmonic(_gG[i], _hH[i], N, N, M, _a, _norm));
_nmx = max(_nmx, _harm.back().Coefficients().nmx());
_mmx = max(_mmx, _harm.back().Coefficients().mmx());
}
int pos = int(coeffstr.tellg());
coeffstr.seekg(0, ios::end);
if (pos != coeffstr.tellg())
throw GeographicErr("Extra data in " + coeff);
}
}
void MagneticModel::ReadMetadata(const string& name) {
const char* spaces = " \t\n\v\f\r";
_filename = _dir + "/" + name + ".wmm";
ifstream metastr(_filename.c_str());
if (!metastr.good())
throw GeographicErr("Cannot open " + _filename);
string line;
getline(metastr, line);
if (!(line.size() >= 6 && line.substr(0,5) == "WMMF-"))
throw GeographicErr(_filename + " does not contain WMMF-n signature");
string::size_type n = line.find_first_of(spaces, 5);
if (n != string::npos)
n -= 5;
string version(line, 5, n);
if (!(version == "1" || version == "2"))
throw GeographicErr("Unknown version in " + _filename + ": " + version);
string key, val;
while (getline(metastr, line)) {
if (!Utility::ParseLine(line, key, val))
continue;
// Process key words
if (key == "Name")
_name = val;
else if (key == "Description")
_description = val;
else if (key == "ReleaseDate")
_date = val;
else if (key == "Radius")
_a = Utility::val<real>(val);
else if (key == "Type") {
if (!(val == "Linear" || val == "linear"))
throw GeographicErr("Only linear models are supported");
} else if (key == "Epoch")
_t0 = Utility::val<real>(val);
else if (key == "DeltaEpoch")
_dt0 = Utility::val<real>(val);
else if (key == "NumModels")
_nNmodels = Utility::val<int>(val);
else if (key == "NumConstants")
_nNconstants = Utility::val<int>(val);
else if (key == "MinTime")
_tmin = Utility::val<real>(val);
else if (key == "MaxTime")
_tmax = Utility::val<real>(val);
else if (key == "MinHeight")
_hmin = Utility::val<real>(val);
else if (key == "MaxHeight")
_hmax = Utility::val<real>(val);
else if (key == "Normalization") {
if (val == "FULL" || val == "Full" || val == "full")
_norm = SphericalHarmonic::FULL;
else if (val == "SCHMIDT" || val == "Schmidt" || val == "schmidt")
_norm = SphericalHarmonic::SCHMIDT;
else
throw GeographicErr("Unknown normalization " + val);
} else if (key == "ByteOrder") {
if (val == "Big" || val == "big")
throw GeographicErr("Only little-endian ordering is supported");
else if (!(val == "Little" || val == "little"))
throw GeographicErr("Unknown byte ordering " + val);
} else if (key == "ID")
_id = val;
// else unrecognized keywords are skipped
}
// Check values
if (!(isfinite(_a) && _a > 0))
throw GeographicErr("Reference radius must be positive");
if (!(_t0 > 0))
throw GeographicErr("Epoch time not defined");
if (_tmin >= _tmax)
throw GeographicErr("Min time exceeds max time");
if (_hmin >= _hmax)
throw GeographicErr("Min height exceeds max height");
if (int(_id.size()) != idlength_)
throw GeographicErr("Invalid ID");
if (_nNmodels < 1)
throw GeographicErr("NumModels must be positive");
if (!(_nNconstants == 0 || _nNconstants == 1))
throw GeographicErr("NumConstants must be 0 or 1");
if (!(_dt0 > 0)) {
if (_nNmodels > 1)
throw GeographicErr("DeltaEpoch must be positive");
else
_dt0 = 1;
}
}
void MagneticModel::FieldGeocentric(real t, real X, real Y, real Z,
real& BX, real& BY, real& BZ,
real& BXt, real& BYt, real& BZt) const {
t -= _t0;
int n = max(min(int(floor(t / _dt0)), _nNmodels - 1), 0);
bool interpolate = n + 1 < _nNmodels;
t -= n * _dt0;
// Components in geocentric basis
// initial values to suppress warning
real BXc = 0, BYc = 0, BZc = 0;
_harm[n](X, Y, Z, BX, BY, BZ);
_harm[n + 1](X, Y, Z, BXt, BYt, BZt);
if (_nNconstants)
_harm[_nNmodels + 1](X, Y, Z, BXc, BYc, BZc);
if (interpolate) {
// Convert to a time derivative
BXt = (BXt - BX) / _dt0;
BYt = (BYt - BY) / _dt0;
BZt = (BZt - BZ) / _dt0;
}
BX += t * BXt + BXc;
BY += t * BYt + BYc;
BZ += t * BZt + BZc;
BXt = BXt * - _a;
BYt = BYt * - _a;
BZt = BZt * - _a;
BX *= - _a;
BY *= - _a;
BZ *= - _a;
}
void MagneticModel::Field(real t, real lat, real lon, real h, bool diffp,
real& Bx, real& By, real& Bz,
real& Bxt, real& Byt, real& Bzt) const {
real X, Y, Z;
real M[Geocentric::dim2_];
_earth.IntForward(lat, lon, h, X, Y, Z, M);
// Components in geocentric basis
// initial values to suppress warning
real BX = 0, BY = 0, BZ = 0, BXt = 0, BYt = 0, BZt = 0;
FieldGeocentric(t, X, Y, Z, BX, BY, BZ, BXt, BYt, BZt);
if (diffp)
Geocentric::Unrotate(M, BXt, BYt, BZt, Bxt, Byt, Bzt);
Geocentric::Unrotate(M, BX, BY, BZ, Bx, By, Bz);
}
MagneticCircle MagneticModel::Circle(real t, real lat, real h) const {
real t1 = t - _t0;
int n = max(min(int(floor(t1 / _dt0)), _nNmodels - 1), 0);
bool interpolate = n + 1 < _nNmodels;
t1 -= n * _dt0;
real X, Y, Z, M[Geocentric::dim2_];
_earth.IntForward(lat, 0, h, X, Y, Z, M);
// Y = 0, cphi = M[7], sphi = M[8];
return (_nNconstants == 0 ?
MagneticCircle(_a, _earth._f, lat, h, t,
M[7], M[8], t1, _dt0, interpolate,
_harm[n].Circle(X, Z, true),
_harm[n + 1].Circle(X, Z, true)) :
MagneticCircle(_a, _earth._f, lat, h, t,
M[7], M[8], t1, _dt0, interpolate,
_harm[n].Circle(X, Z, true),
_harm[n + 1].Circle(X, Z, true),
_harm[_nNmodels + 1].Circle(X, Z, true)));
}
void MagneticModel::FieldComponents(real Bx, real By, real Bz,
real Bxt, real Byt, real Bzt,
real& H, real& F, real& D, real& I,
real& Ht, real& Ft,
real& Dt, real& It) {
H = hypot(Bx, By);
Ht = H != 0 ? (Bx * Bxt + By * Byt) / H : hypot(Bxt, Byt);
D = H != 0 ? Math::atan2d(Bx, By) : Math::atan2d(Bxt, Byt);
Dt = (H != 0 ? (By * Bxt - Bx * Byt) / Math::sq(H) : 0) / Math::degree();
F = hypot(H, Bz);
Ft = F != 0 ? (H * Ht + Bz * Bzt) / F : hypot(Ht, Bzt);
I = F != 0 ? Math::atan2d(-Bz, H) : Math::atan2d(-Bzt, Ht);
It = (F != 0 ? (Bz * Ht - H * Bzt) / Math::sq(F) : 0) / Math::degree();
}
string MagneticModel::DefaultMagneticPath() {
string path;
char* magneticpath = getenv("GEOGRAPHICLIB_MAGNETIC_PATH");
if (magneticpath)
path = string(magneticpath);
if (!path.empty())
return path;
char* datapath = getenv("GEOGRAPHICLIB_DATA");
if (datapath)
path = string(datapath);
return (!path.empty() ? path : string(GEOGRAPHICLIB_DATA)) + "/magnetic";
}
string MagneticModel::DefaultMagneticName() {
string name;
char* magneticname = getenv("GEOGRAPHICLIB_MAGNETIC_NAME");
if (magneticname)
name = string(magneticname);
return !name.empty() ? name : string(GEOGRAPHICLIB_MAGNETIC_DEFAULT_NAME);
}
} // namespace GeographicLib

108
libs/geographiclib/src/Makefile.am Normal file
View File

@@ -0,0 +1,108 @@
#
# Makefile.am
#
# Copyright (C) 2009, Francesco P. Lovergine <frankie@debian.org>
AM_CPPFLAGS = -I$(top_builddir)/include -I$(top_srcdir)/include -Wall -Wextra
lib_LTLIBRARIES = libGeographicLib.la
libGeographicLib_la_LDFLAGS = \
-version-info $(LT_CURRENT):$(LT_REVISION):$(LT_AGE)
libGeographicLib_la_SOURCES = Accumulator.cpp \
AlbersEqualArea.cpp \
AzimuthalEquidistant.cpp \
CassiniSoldner.cpp \
CircularEngine.cpp \
DMS.cpp \
DST.cpp \
Ellipsoid.cpp \
EllipticFunction.cpp \
GARS.cpp \
GeoCoords.cpp \
Geocentric.cpp \
Geodesic.cpp \
GeodesicExact.cpp \
GeodesicLine.cpp \
GeodesicLineExact.cpp \
Geohash.cpp \
Geoid.cpp \
Georef.cpp \
Gnomonic.cpp \
GravityCircle.cpp \
GravityModel.cpp \
LambertConformalConic.cpp \
LocalCartesian.cpp \
MGRS.cpp \
MagneticCircle.cpp \
MagneticModel.cpp \
Math.cpp \
NormalGravity.cpp \
OSGB.cpp \
PolarStereographic.cpp \
PolygonArea.cpp \
Rhumb.cpp \
SphericalEngine.cpp \
TransverseMercator.cpp \
TransverseMercatorExact.cpp \
UTMUPS.cpp \
Utility.cpp \
kissfft.hh \
../include/GeographicLib/Accumulator.hpp \
../include/GeographicLib/AlbersEqualArea.hpp \
../include/GeographicLib/AzimuthalEquidistant.hpp \
../include/GeographicLib/CassiniSoldner.hpp \
../include/GeographicLib/CircularEngine.hpp \
../include/GeographicLib/Constants.hpp \
../include/GeographicLib/DMS.hpp \
../include/GeographicLib/Ellipsoid.hpp \
../include/GeographicLib/EllipticFunction.hpp \
../include/GeographicLib/GARS.hpp \
../include/GeographicLib/GeoCoords.hpp \
../include/GeographicLib/Geocentric.hpp \
../include/GeographicLib/Geodesic.hpp \
../include/GeographicLib/GeodesicExact.hpp \
../include/GeographicLib/GeodesicLine.hpp \
../include/GeographicLib/GeodesicLineExact.hpp \
../include/GeographicLib/Geohash.hpp \
../include/GeographicLib/Geoid.hpp \
../include/GeographicLib/Georef.hpp \
../include/GeographicLib/Gnomonic.hpp \
../include/GeographicLib/GravityCircle.hpp \
../include/GeographicLib/GravityModel.hpp \
../include/GeographicLib/LambertConformalConic.hpp \
../include/GeographicLib/LocalCartesian.hpp \
../include/GeographicLib/MGRS.hpp \
../include/GeographicLib/MagneticCircle.hpp \
../include/GeographicLib/MagneticModel.hpp \
../include/GeographicLib/Math.hpp \
../include/GeographicLib/NearestNeighbor.hpp \
../include/GeographicLib/NormalGravity.hpp \
../include/GeographicLib/OSGB.hpp \
../include/GeographicLib/PolarStereographic.hpp \
../include/GeographicLib/PolygonArea.hpp \
../include/GeographicLib/Rhumb.hpp \
../include/GeographicLib/SphericalEngine.hpp \
../include/GeographicLib/SphericalHarmonic.hpp \
../include/GeographicLib/SphericalHarmonic1.hpp \
../include/GeographicLib/SphericalHarmonic2.hpp \
../include/GeographicLib/TransverseMercator.hpp \
../include/GeographicLib/TransverseMercatorExact.hpp \
../include/GeographicLib/UTMUPS.hpp \
../include/GeographicLib/Utility.hpp \
../include/GeographicLib/Config.h
../include/GeographicLib/Config.h: ../include/GeographicLib/Config-ac.h
( egrep '\bVERSION\b|\bGEOGRAPHICLIB_|\bHAVE_LONG_DOUBLE\b' $< | \
sed -e 's/ VERSION / GEOGRAPHICLIB_VERSION_STRING /' \
-e 's/ HAVE_LONG_DOUBLE / GEOGRAPHICLIB_HAVE_LONG_DOUBLE /'; \
grep WORDS_BIGENDIAN $< | tail -1 | \
sed -e 's/ WORDS_BIGENDIAN / GEOGRAPHICLIB_WORDS_BIGENDIAN /' ) > $@
$(libGeographicLib_la_OBJECTS): ../include/GeographicLib/Config.h
geographiclib_data=$(datadir)/GeographicLib
DEFS=-DGEOGRAPHICLIB_DATA=\"$(geographiclib_data)\" @DEFS@
EXTRA_DIST = CMakeLists.txt kissfft.hh

313
libs/geographiclib/src/Math.cpp Normal file
View File

@@ -0,0 +1,313 @@
/**
* \file Math.cpp
* \brief Implementation for GeographicLib::Math class
*
* Copyright (c) Charles Karney (2015-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/Math.hpp>
#if defined(_MSC_VER)
// Squelch warnings about constant conditional and enum-float expressions
# pragma warning (disable: 4127 5055)
#endif
namespace GeographicLib {
using namespace std;
void Math::dummy() {
static_assert(GEOGRAPHICLIB_PRECISION >= 1 && GEOGRAPHICLIB_PRECISION <= 5,
"Bad value of precision");
}
int Math::digits() {
#if GEOGRAPHICLIB_PRECISION != 5
return numeric_limits<real>::digits;
#else
return numeric_limits<real>::digits();
#endif
}
int Math::set_digits(int ndigits) {
#if GEOGRAPHICLIB_PRECISION != 5
(void)ndigits;
#else
mpfr::mpreal::set_default_prec(ndigits >= 2 ? ndigits : 2);
#endif
return digits();
}
int Math::digits10() {
#if GEOGRAPHICLIB_PRECISION != 5
return numeric_limits<real>::digits10;
#else
return numeric_limits<real>::digits10();
#endif
}
int Math::extra_digits() {
return
digits10() > numeric_limits<double>::digits10 ?
digits10() - numeric_limits<double>::digits10 : 0;
}
template<typename T> T Math::sum(T u, T v, T& t) {
GEOGRAPHICLIB_VOLATILE T s = u + v;
GEOGRAPHICLIB_VOLATILE T up = s - v;
GEOGRAPHICLIB_VOLATILE T vpp = s - up;
up -= u;
vpp -= v;
// if s = 0, then t = 0 and give t the same sign as s
// mpreal needs T(0) here
t = s != 0 ? T(0) - (up + vpp) : s;
// u + v = s + t
// = round(u + v) + t
return s;
}
template<typename T> T Math::AngNormalize(T x) {
T y = remainder(x, T(td));
#if GEOGRAPHICLIB_PRECISION == 4
// boost-quadmath doesn't set the sign of 0 correctly, see
// https://github.com/boostorg/multiprecision/issues/426
// Fixed by https://github.com/boostorg/multiprecision/pull/428
if (y == 0) y = copysign(y, x);
#endif
return fabs(y) == T(hd) ? copysign(T(hd), x) : y;
}
template<typename T> T Math::AngDiff(T x, T y, T& e) {
// Use remainder instead of AngNormalize, since we treat boundary cases
// later taking account of the error
T d = sum(remainder(-x, T(td)), remainder( y, T(td)), e);
// This second sum can only change d if abs(d) < 128, so don't need to
// apply remainder yet again.
d = sum(remainder(d, T(td)), e, e);
// Fix the sign if d = -180, 0, 180.
if (d == 0 || fabs(d) == hd)
// If e == 0, take sign from y - x
// else (e != 0, implies d = +/-180), d and e must have opposite signs
d = copysign(d, e == 0 ? y - x : -e);
return d;
}
template<typename T> T Math::AngRound(T x) {
static const T z = T(1)/T(16);
GEOGRAPHICLIB_VOLATILE T y = fabs(x);
GEOGRAPHICLIB_VOLATILE T w = z - y;
// The compiler mustn't "simplify" z - (z - y) to y
y = w > 0 ? z - w : y;
return copysign(y, x);
}
template<typename T> void Math::sincosd(T x, T& sinx, T& cosx) {
// In order to minimize round-off errors, this function exactly reduces
// the argument to the range [-45, 45] before converting it to radians.
T r; int q = 0;
r = remquo(x, T(qd), &q); // now abs(r) <= 45
r *= degree<T>();
// g++ -O turns these two function calls into a call to sincos
T s = sin(r), c = cos(r);
switch (unsigned(q) & 3U) {
case 0U: sinx = s; cosx = c; break;
case 1U: sinx = c; cosx = -s; break;
case 2U: sinx = -s; cosx = -c; break;
default: sinx = -c; cosx = s; break; // case 3U
}
// http://www.open-std.org/jtc1/sc22/wg14/www/docs/n1950.pdf
// mpreal needs T(0) here
cosx += T(0); // special values from F.10.1.12
if (sinx == 0) sinx = copysign(sinx, x); // special values from F.10.1.13
}
template<typename T> void Math::sincosde(T x, T t, T& sinx, T& cosx) {
// In order to minimize round-off errors, this function exactly reduces
// the argument to the range [-45, 45] before converting it to radians.
// This implementation allows x outside [-180, 180], but implementations in
// other languages may not.
T r; int q = 0;
r = AngRound(remquo(x, T(qd), &q) + t); // now abs(r) <= 45
r *= degree<T>();
// g++ -O turns these two function calls into a call to sincos
T s = sin(r), c = cos(r);
switch (unsigned(q) & 3U) {
case 0U: sinx = s; cosx = c; break;
case 1U: sinx = c; cosx = -s; break;
case 2U: sinx = -s; cosx = -c; break;
default: sinx = -c; cosx = s; break; // case 3U
}
// http://www.open-std.org/jtc1/sc22/wg14/www/docs/n1950.pdf
// mpreal needs T(0) here
cosx += T(0); // special values from F.10.1.12
if (sinx == 0) sinx = copysign(sinx, x); // special values from F.10.1.13
}
template<typename T> T Math::sind(T x) {
// See sincosd
T r; int q = 0;
r = remquo(x, T(qd), &q); // now abs(r) <= 45
r *= degree<T>();
unsigned p = unsigned(q);
r = p & 1U ? cos(r) : sin(r);
if (p & 2U) r = -r;
if (r == 0) r = copysign(r, x);
return r;
}
template<typename T> T Math::cosd(T x) {
// See sincosd
T r; int q = 0;
r = remquo(x, T(qd), &q); // now abs(r) <= 45
r *= degree<T>();
unsigned p = unsigned(q + 1);
r = p & 1U ? cos(r) : sin(r);
if (p & 2U) r = -r;
// mpreal needs T(0) here
return T(0) + r;
}
template<typename T> T Math::tand(T x) {
static const T overflow = 1 / sq(numeric_limits<T>::epsilon());
T s, c;
sincosd(x, s, c);
// http://www.open-std.org/jtc1/sc22/wg14/www/docs/n1950.pdf
T r = s / c; // special values from F.10.1.14
// With C++17 this becomes clamp(s / c, -overflow, overflow);
// Use max/min here (instead of fmax/fmin) to preserve NaN
return min(max(r, -overflow), overflow);
}
template<typename T> T Math::atan2d(T y, T x) {
// In order to minimize round-off errors, this function rearranges the
// arguments so that result of atan2 is in the range [-pi/4, pi/4] before
// converting it to degrees and mapping the result to the correct
// quadrant.
int q = 0;
if (fabs(y) > fabs(x)) { swap(x, y); q = 2; }
if (signbit(x)) { x = -x; ++q; }
// here x >= 0 and x >= abs(y), so angle is in [-pi/4, pi/4]
T ang = atan2(y, x) / degree<T>();
switch (q) {
case 1: ang = copysign(T(hd), y) - ang; break;
case 2: ang = qd - ang; break;
case 3: ang = -qd + ang; break;
default: break;
}
return ang;
}
template<typename T> T Math::atand(T x)
{ return atan2d(x, T(1)); }
template<typename T> T Math::eatanhe(T x, T es) {
return es > 0 ? es * atanh(es * x) : -es * atan(es * x);
}
template<typename T> T Math::taupf(T tau, T es) {
// Need this test, otherwise tau = +/-inf gives taup = nan.
if (isfinite(tau)) {
T tau1 = hypot(T(1), tau),
sig = sinh( eatanhe(tau / tau1, es ) );
return hypot(T(1), sig) * tau - sig * tau1;
} else
return tau;
}
template<typename T> T Math::tauf(T taup, T es) {
static const int numit = 5;
// min iterations = 1, max iterations = 2; mean = 1.95
static const T tol = sqrt(numeric_limits<T>::epsilon()) / 10;
static const T taumax = 2 / sqrt(numeric_limits<T>::epsilon());
T e2m = 1 - sq(es),
// To lowest order in e^2, taup = (1 - e^2) * tau = _e2m * tau; so use
// tau = taup/e2m as a starting guess. Only 1 iteration is needed for
// |lat| < 3.35 deg, otherwise 2 iterations are needed. If, instead, tau
// = taup is used the mean number of iterations increases to 1.999 (2
// iterations are needed except near tau = 0).
//
// For large tau, taup = exp(-es*atanh(es)) * tau. Use this as for the
// initial guess for |taup| > 70 (approx |phi| > 89deg). Then for
// sufficiently large tau (such that sqrt(1+tau^2) = |tau|), we can exit
// with the intial guess and avoid overflow problems. This also reduces
// the mean number of iterations slightly from 1.963 to 1.954.
tau = fabs(taup) > 70 ? taup * exp(eatanhe(T(1), es)) : taup/e2m,
stol = tol * fmax(T(1), fabs(taup));
if (!(fabs(tau) < taumax)) return tau; // handles +/-inf and nan
for (int i = 0; i < numit || GEOGRAPHICLIB_PANIC; ++i) {
T taupa = taupf(tau, es),
dtau = (taup - taupa) * (1 + e2m * sq(tau)) /
( e2m * hypot(T(1), tau) * hypot(T(1), taupa) );
tau += dtau;
if (!(fabs(dtau) >= stol))
break;
}
return tau;
}
template<typename T> T Math::NaN() {
#if defined(_MSC_VER)
return numeric_limits<T>::has_quiet_NaN ?
numeric_limits<T>::quiet_NaN() :
(numeric_limits<T>::max)();
#else
return numeric_limits<T>::has_quiet_NaN ?
numeric_limits<T>::quiet_NaN() :
numeric_limits<T>::max();
#endif
}
template<typename T> T Math::infinity() {
#if defined(_MSC_VER)
return numeric_limits<T>::has_infinity ?
numeric_limits<T>::infinity() :
(numeric_limits<T>::max)();
#else
return numeric_limits<T>::has_infinity ?
numeric_limits<T>::infinity() :
numeric_limits<T>::max();
#endif
}
/// \cond SKIP
// Instantiate
#define GEOGRAPHICLIB_MATH_INSTANTIATE(T) \
template T GEOGRAPHICLIB_EXPORT Math::sum <T>(T, T, T&); \
template T GEOGRAPHICLIB_EXPORT Math::AngNormalize <T>(T); \
template T GEOGRAPHICLIB_EXPORT Math::AngDiff <T>(T, T, T&); \
template T GEOGRAPHICLIB_EXPORT Math::AngRound <T>(T); \
template void GEOGRAPHICLIB_EXPORT Math::sincosd <T>(T, T&, T&); \
template void GEOGRAPHICLIB_EXPORT Math::sincosde <T>(T, T, T&, T&); \
template T GEOGRAPHICLIB_EXPORT Math::sind <T>(T); \
template T GEOGRAPHICLIB_EXPORT Math::cosd <T>(T); \
template T GEOGRAPHICLIB_EXPORT Math::tand <T>(T); \
template T GEOGRAPHICLIB_EXPORT Math::atan2d <T>(T, T); \
template T GEOGRAPHICLIB_EXPORT Math::atand <T>(T); \
template T GEOGRAPHICLIB_EXPORT Math::eatanhe <T>(T, T); \
template T GEOGRAPHICLIB_EXPORT Math::taupf <T>(T, T); \
template T GEOGRAPHICLIB_EXPORT Math::tauf <T>(T, T); \
template T GEOGRAPHICLIB_EXPORT Math::NaN <T>(); \
template T GEOGRAPHICLIB_EXPORT Math::infinity <T>();
// Instantiate with the standard floating type
GEOGRAPHICLIB_MATH_INSTANTIATE(float)
GEOGRAPHICLIB_MATH_INSTANTIATE(double)
#if GEOGRAPHICLIB_HAVE_LONG_DOUBLE
// Instantiate if long double is distinct from double
GEOGRAPHICLIB_MATH_INSTANTIATE(long double)
#endif
#if GEOGRAPHICLIB_PRECISION > 3
// Instantiate with the high precision type
GEOGRAPHICLIB_MATH_INSTANTIATE(Math::real)
#endif
#undef GEOGRAPHICLIB_MATH_INSTANTIATE
// Also we need int versions for Utility::nummatch
template int GEOGRAPHICLIB_EXPORT Math::NaN <int>();
template int GEOGRAPHICLIB_EXPORT Math::infinity<int>();
/// \endcond
} // namespace GeographicLib

300
libs/geographiclib/src/NormalGravity.cpp Normal file
View File

@@ -0,0 +1,300 @@
/**
* \file NormalGravity.cpp
* \brief Implementation for GeographicLib::NormalGravity class
*
* Copyright (c) Charles Karney (2011-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/NormalGravity.hpp>
#if defined(_MSC_VER)
// Squelch warnings about constant conditional expressions
# pragma warning (disable: 4127)
#endif
namespace GeographicLib {
using namespace std;
void NormalGravity::Initialize(real a, real GM, real omega, real f_J2,
bool geometricp) {
_a = a;
if (!(isfinite(_a) && _a > 0))
throw GeographicErr("Equatorial radius is not positive");
_gGM = GM;
if (!isfinite(_gGM))
throw GeographicErr("Gravitational constant is not finite");
_omega = omega;
_omega2 = Math::sq(_omega);
_aomega2 = Math::sq(_omega * _a);
if (!(isfinite(_omega2) && isfinite(_aomega2)))
throw GeographicErr("Rotation velocity is not finite");
_f = geometricp ? f_J2 : J2ToFlattening(_a, _gGM, _omega, f_J2);
_b = _a * (1 - _f);
if (!(isfinite(_b) && _b > 0))
throw GeographicErr("Polar semi-axis is not positive");
_jJ2 = geometricp ? FlatteningToJ2(_a, _gGM, _omega, f_J2) : f_J2;
_e2 = _f * (2 - _f);
_ep2 = _e2 / (1 - _e2);
real ex2 = _f < 0 ? -_e2 : _ep2;
_qQ0 = Qf(ex2, _f < 0);
_earth = Geocentric(_a, _f);
_eE = _a * sqrt(fabs(_e2)); // H+M, Eq 2-54
// H+M, Eq 2-61
_uU0 = _gGM * atanzz(ex2, _f < 0) / _b + _aomega2 / 3;
real P = Hf(ex2, _f < 0) / (6 * _qQ0);
// H+M, Eq 2-73
_gammae = _gGM / (_a * _b) - (1 + P) * _a * _omega2;
// H+M, Eq 2-74
_gammap = _gGM / (_a * _a) + 2 * P * _b * _omega2;
// k = gammae * (b * gammap / (a * gammae) - 1)
// = (b * gammap - a * gammae) / a
_k = -_e2 * _gGM / (_a * _b) +
_omega2 * (P * (_a + 2 * _b * (1 - _f)) + _a);
// f* = (gammap - gammae) / gammae
_fstar = (-_f * _gGM / (_a * _b) + _omega2 * (P * (_a + 2 * _b) + _a)) /
_gammae;
}
NormalGravity::NormalGravity(real a, real GM, real omega, real f_J2,
bool geometricp) {
Initialize(a, GM, omega, f_J2, geometricp);
}
const NormalGravity& NormalGravity::WGS84() {
static const NormalGravity wgs84(Constants::WGS84_a(),
Constants::WGS84_GM(),
Constants::WGS84_omega(),
Constants::WGS84_f(), true);
return wgs84;
}
const NormalGravity& NormalGravity::GRS80() {
static const NormalGravity grs80(Constants::GRS80_a(),
Constants::GRS80_GM(),
Constants::GRS80_omega(),
Constants::GRS80_J2(), false);
return grs80;
}
Math::real NormalGravity::atan7series(real x) {
// compute -sum( (-x)^n/(2*n+7), n, 0, inf)
// = -1/7 + x/9 - x^2/11 + x^3/13 ...
// = (atan(sqrt(x))/sqrt(x)-(1-x/3+x^2/5)) / x^3 (x > 0)
// = (atanh(sqrt(-x))/sqrt(-x)-(1-x/3+x^2/5)) / x^3 (x < 0)
// require abs(x) < 1/2, but better to restrict calls to abs(x) < 1/4
static const real lg2eps_ = -log2(numeric_limits<real>::epsilon() / 2);
int e;
frexp(x, &e);
e = max(-e, 1); // Here's where abs(x) < 1/2 is assumed
// x = [0.5,1) * 2^(-e)
// estimate n s.t. x^n/n < 1/7 * epsilon/2
// a stronger condition is x^n < epsilon/2
// taking log2 of both sides, a stronger condition is n*(-e) < -lg2eps;
// or n*e > lg2eps or n > ceiling(lg2eps/e)
int n = x == 0 ? 1 : int(ceil(lg2eps_ / e));
Math::real v = 0;
while (n--) // iterating from n-1 down to 0
v = - x * v - 1/Math::real(2*n + 7);
return v;
}
Math::real NormalGravity::atan5series(real x) {
// Compute Taylor series approximations to
// (atan(z)-(z-z^3/3))/z^5,
// z = sqrt(x)
// require abs(x) < 1/2, but better to restrict calls to abs(x) < 1/4
return 1/real(5) + x * atan7series(x);
}
Math::real NormalGravity::Qf(real x, bool alt) {
// Compute
// Q(z) = (((1 + 3/z^2) * atan(z) - 3/z)/2) / z^3
// = q(z)/z^3 with q(z) defined by H+M, Eq 2-57 with z = E/u
// z = sqrt(x)
real y = alt ? -x / (1 + x) : x;
return !(4 * fabs(y) < 1) ? // Backwards test to allow NaNs through
((1 + 3/y) * atanzz(x, alt) - 3/y) / (2 * y) :
(3 * (3 + y) * atan5series(y) - 1) / 6;
}
Math::real NormalGravity::Hf(real x, bool alt) {
// z = sqrt(x)
// Compute
// H(z) = (3*Q(z)+z*diff(Q(z),z))*(1+z^2)
// = (3 * (1 + 1/z^2) * (1 - atan(z)/z) - 1) / z^2
// = q'(z)/z^2, with q'(z) defined by H+M, Eq 2-67, with z = E/u
real y = alt ? -x / (1 + x) : x;
return !(4 * fabs(y) < 1) ? // Backwards test to allow NaNs through
(3 * (1 + 1/y) * (1 - atanzz(x, alt)) - 1) / y :
1 - 3 * (1 + y) * atan5series(y);
}
Math::real NormalGravity::QH3f(real x, bool alt) {
// z = sqrt(x)
// (Q(z) - H(z)/3) / z^2
// = - (1+z^2)/(3*z) * d(Q(z))/dz - Q(z)
// = ((15+9*z^2)*atan(z)-4*z^3-15*z)/(6*z^7)
// = ((25+15*z^2)*atan7+3)/10
real y = alt ? -x / (1 + x) : x;
return !(4 * fabs(y) < 1) ? // Backwards test to allow NaNs through
((9 + 15/y) * atanzz(x, alt) - 4 - 15/y) / (6 * Math::sq(y)) :
((25 + 15*y) * atan7series(y) + 3)/10;
}
Math::real NormalGravity::Jn(int n) const {
// Note Jn(0) = -1; Jn(2) = _jJ2; Jn(odd) = 0
if (n & 1 || n < 0)
return 0;
n /= 2;
real e2n = 1; // Perhaps this should just be e2n = pow(-_e2, n);
for (int j = n; j--;)
e2n *= -_e2;
return // H+M, Eq 2-92
-3 * e2n * ((1 - n) + 5 * n * _jJ2 / _e2) / ((2 * n + 1) * (2 * n + 3));
}
Math::real NormalGravity::SurfaceGravity(real lat) const {
real sphi = Math::sind(Math::LatFix(lat));
// H+M, Eq 2-78
return (_gammae + _k * Math::sq(sphi)) / sqrt(1 - _e2 * Math::sq(sphi));
}
Math::real NormalGravity::V0(real X, real Y, real Z,
real& GammaX, real& GammaY, real& GammaZ) const
{
// See H+M, Sec 6-2
real
p = hypot(X, Y),
clam = p != 0 ? X/p : 1,
slam = p != 0 ? Y/p : 0,
r = hypot(p, Z);
if (_f < 0) swap(p, Z);
real
Q = Math::sq(r) - Math::sq(_eE),
t2 = Math::sq(2 * _eE * Z),
disc = sqrt(Math::sq(Q) + t2),
// This is H+M, Eq 6-8a, but generalized to deal with Q negative
// accurately.
u = sqrt((Q >= 0 ? (Q + disc) : t2 / (disc - Q)) / 2),
uE = hypot(u, _eE),
// H+M, Eq 6-8b
sbet = u != 0 ? Z * uE : copysign(sqrt(-Q), Z),
cbet = u != 0 ? p * u : p,
s = hypot(cbet, sbet);
sbet = s != 0 ? sbet/s : 1;
cbet = s != 0 ? cbet/s : 0;
real
z = _eE/u,
z2 = Math::sq(z),
den = hypot(u, _eE * sbet);
if (_f < 0) {
swap(sbet, cbet);
swap(u, uE);
}
real
invw = uE / den, // H+M, Eq 2-63
bu = _b / (u != 0 || _f < 0 ? u : _eE),
// Qf(z2->inf, false) = pi/(4*z^3)
q = ((u != 0 || _f < 0 ? Qf(z2, _f < 0) : Math::pi() / 4) / _qQ0) *
bu * Math::sq(bu),
qp = _b * Math::sq(bu) * (u != 0 || _f < 0 ? Hf(z2, _f < 0) : 2) / _qQ0,
ang = (Math::sq(sbet) - 1/real(3)) / 2,
// H+M, Eqs 2-62 + 6-9, but omitting last (rotational) term.
Vres = _gGM * (u != 0 || _f < 0 ?
atanzz(z2, _f < 0) / u :
Math::pi() / (2 * _eE)) + _aomega2 * q * ang,
// H+M, Eq 6-10
gamu = - (_gGM + (_aomega2 * qp * ang)) * invw / Math::sq(uE),
gamb = _aomega2 * q * sbet * cbet * invw / uE,
t = u * invw / uE,
gamp = t * cbet * gamu - invw * sbet * gamb;
// H+M, Eq 6-12
GammaX = gamp * clam;
GammaY = gamp * slam;
GammaZ = invw * sbet * gamu + t * cbet * gamb;
return Vres;
}
Math::real NormalGravity::Phi(real X, real Y, real& fX, real& fY) const {
fX = _omega2 * X;
fY = _omega2 * Y;
// N.B. fZ = 0;
return _omega2 * (Math::sq(X) + Math::sq(Y)) / 2;
}
Math::real NormalGravity::U(real X, real Y, real Z,
real& gammaX, real& gammaY, real& gammaZ) const {
real fX, fY;
real Ures = V0(X, Y, Z, gammaX, gammaY, gammaZ) + Phi(X, Y, fX, fY);
gammaX += fX;
gammaY += fY;
return Ures;
}
Math::real NormalGravity::Gravity(real lat, real h,
real& gammay, real& gammaz) const {
real X, Y, Z;
real M[Geocentric::dim2_];
_earth.IntForward(lat, 0, h, X, Y, Z, M);
real gammaX, gammaY, gammaZ,
Ures = U(X, Y, Z, gammaX, gammaY, gammaZ);
// gammax = M[0] * gammaX + M[3] * gammaY + M[6] * gammaZ;
gammay = M[1] * gammaX + M[4] * gammaY + M[7] * gammaZ;
gammaz = M[2] * gammaX + M[5] * gammaY + M[8] * gammaZ;
return Ures;
}
Math::real NormalGravity::J2ToFlattening(real a, real GM,
real omega, real J2) {
// Solve
// f = e^2 * (1 - K * e/q0) - 3 * J2 = 0
// for e^2 using Newton's method
static const real maxe_ = 1 - numeric_limits<real>::epsilon();
static const real eps2_ = sqrt(numeric_limits<real>::epsilon()) / 100;
real
K = 2 * Math::sq(a * omega) * a / (15 * GM),
J0 = (1 - 4 * K / Math::pi()) / 3;
if (!(GM > 0 && isfinite(K) && K >= 0))
return Math::NaN();
if (!(isfinite(J2) && J2 <= J0)) return Math::NaN();
if (J2 == J0) return 1;
// Solve e2 - f1 * f2 * K / Q0 - 3 * J2 = 0 for J2 close to J0;
// subst e2 = ep2/(1+ep2), f2 = 1/(1+ep2), f1 = 1/sqrt(1+ep2), J2 = J0-dJ2,
// Q0 = pi/(4*z^3) - 2/z^4 + (3*pi)/(4*z^5), z = sqrt(ep2), and balance two
// leading terms to give
real
ep2 = fmax(Math::sq(32 * K / (3 * Math::sq(Math::pi()) * (J0 - J2))),
-maxe_),
e2 = fmin(ep2 / (1 + ep2), maxe_);
for (int j = 0; j < maxit_ || GEOGRAPHICLIB_PANIC; ++j) {
real
e2a = e2, ep2a = ep2,
f2 = 1 - e2, // (1 - f)^2
f1 = sqrt(f2), // (1 - f)
Q0 = Qf(e2 < 0 ? -e2 : ep2, e2 < 0),
h = e2 - f1 * f2 * K / Q0 - 3 * J2,
dh = 1 - 3 * f1 * K * QH3f(e2 < 0 ? -e2 : ep2, e2 < 0) /
(2 * Math::sq(Q0));
e2 = fmin(e2a - h / dh, maxe_);
ep2 = fmax(e2 / (1 - e2), -maxe_);
if (fabs(h) < eps2_ || e2 == e2a || ep2 == ep2a)
break;
}
return e2 / (1 + sqrt(1 - e2));
}
Math::real NormalGravity::FlatteningToJ2(real a, real GM,
real omega, real f) {
real
K = 2 * Math::sq(a * omega) * a / (15 * GM),
f1 = 1 - f,
f2 = Math::sq(f1),
e2 = f * (2 - f);
// H+M, Eq 2-90 + 2-92'
return (e2 - K * f1 * f2 / Qf(f < 0 ? -e2 : e2 / f2, f < 0)) / 3;
}
} // namespace GeographicLib

174
libs/geographiclib/src/OSGB.cpp Normal file
View File

@@ -0,0 +1,174 @@
/**
* \file OSGB.cpp
* \brief Implementation for GeographicLib::OSGB class
*
* Copyright (c) Charles Karney (2010-2020) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/OSGB.hpp>
#include <GeographicLib/Utility.hpp>
#if defined(_MSC_VER)
// Squelch warnings about enum-float expressions
# pragma warning (disable: 5055)
#endif
namespace GeographicLib {
using namespace std;
const char* const OSGB::letters_ = "ABCDEFGHJKLMNOPQRSTUVWXYZ";
const char* const OSGB::digits_ = "0123456789";
const TransverseMercator& OSGB::OSGBTM() {
static const TransverseMercator osgbtm(EquatorialRadius(), Flattening(),
CentralScale());
return osgbtm;
}
Math::real OSGB::computenorthoffset() {
real x, y;
static const real northoffset =
( OSGBTM().Forward(real(0), OriginLatitude(), real(0), x, y),
FalseNorthing() - y );
return northoffset;
}
void OSGB::GridReference(real x, real y, int prec, std::string& gridref) {
using std::isnan; // Needed for Centos 7, ubuntu 14
CheckCoords(x, y);
if (!(prec >= 0 && prec <= maxprec_))
throw GeographicErr("OSGB precision " + Utility::str(prec)
+ " not in [0, "
+ Utility::str(int(maxprec_)) + "]");
if (isnan(x) || isnan(y)) {
gridref = "INVALID";
return;
}
char grid[2 + 2 * maxprec_];
int
xh = int(floor(x / tile_)),
yh = int(floor(y / tile_));
real
xf = x - tile_ * xh,
yf = y - tile_ * yh;
xh += tileoffx_;
yh += tileoffy_;
int z = 0;
grid[z++] = letters_[(tilegrid_ - (yh / tilegrid_) - 1)
* tilegrid_ + (xh / tilegrid_)];
grid[z++] = letters_[(tilegrid_ - (yh % tilegrid_) - 1)
* tilegrid_ + (xh % tilegrid_)];
// Need extra real because, since C++11, pow(float, int) returns double
real mult = real(pow(real(base_), max(tilelevel_ - prec, 0)));
int
ix = int(floor(xf / mult)),
iy = int(floor(yf / mult));
for (int c = min(prec, int(tilelevel_)); c--;) {
grid[z + c] = digits_[ ix % base_ ];
ix /= base_;
grid[z + c + prec] = digits_[ iy % base_ ];
iy /= base_;
}
if (prec > tilelevel_) {
xf -= floor(xf / mult);
yf -= floor(yf / mult);
mult = real(pow(real(base_), prec - tilelevel_));
ix = int(floor(xf * mult));
iy = int(floor(yf * mult));
for (int c = prec - tilelevel_; c--;) {
grid[z + c + tilelevel_] = digits_[ ix % base_ ];
ix /= base_;
grid[z + c + tilelevel_ + prec] = digits_[ iy % base_ ];
iy /= base_;
}
}
int mlen = z + 2 * prec;
gridref.resize(mlen);
copy(grid, grid + mlen, gridref.begin());
}
void OSGB::GridReference(const std::string& gridref,
real& x, real& y, int& prec,
bool centerp) {
int
len = int(gridref.size()),
p = 0;
if (len >= 2 &&
toupper(gridref[0]) == 'I' &&
toupper(gridref[1]) == 'N') {
x = y = Math::NaN();
prec = -2; // For compatibility with MGRS::Reverse.
return;
}
char grid[2 + 2 * maxprec_];
for (int i = 0; i < len; ++i) {
if (!isspace(gridref[i])) {
if (p >= 2 + 2 * maxprec_)
throw GeographicErr("OSGB string " + gridref + " too long");
grid[p++] = gridref[i];
}
}
len = p;
p = 0;
if (len < 2)
throw GeographicErr("OSGB string " + gridref + " too short");
if (len % 2)
throw GeographicErr("OSGB string " + gridref +
" has odd number of characters");
int
xh = 0,
yh = 0;
while (p < 2) {
int i = Utility::lookup(letters_, grid[p++]);
if (i < 0)
throw GeographicErr("Illegal prefix character " + gridref);
yh = yh * tilegrid_ + tilegrid_ - (i / tilegrid_) - 1;
xh = xh * tilegrid_ + (i % tilegrid_);
}
xh -= tileoffx_;
yh -= tileoffy_;
int prec1 = (len - p)/2;
real
unit = tile_,
x1 = unit * xh,
y1 = unit * yh;
for (int i = 0; i < prec1; ++i) {
unit /= base_;
int
ix = Utility::lookup(digits_, grid[p + i]),
iy = Utility::lookup(digits_, grid[p + i + prec1]);
if (ix < 0 || iy < 0)
throw GeographicErr("Encountered a non-digit in " + gridref);
x1 += unit * ix;
y1 += unit * iy;
}
if (centerp) {
x1 += unit/2;
y1 += unit/2;
}
x = x1;
y = y1;
prec = prec1;
}
void OSGB::CheckCoords(real x, real y) {
// Limits are all multiples of 100km and are all closed on the lower end
// and open on the upper end -- and this is reflected in the error
// messages. NaNs are let through.
if (x < minx_ || x >= maxx_)
throw GeographicErr("Easting " + Utility::str(int(floor(x/1000)))
+ "km not in OSGB range ["
+ Utility::str(minx_/1000) + "km, "
+ Utility::str(maxx_/1000) + "km)");
if (y < miny_ || y >= maxy_)
throw GeographicErr("Northing " + Utility::str(int(floor(y/1000)))
+ "km not in OSGB range ["
+ Utility::str(miny_/1000) + "km, "
+ Utility::str(maxy_/1000) + "km)");
}
} // namespace GeographicLib

115
libs/geographiclib/src/PolarStereographic.cpp Normal file
View File

@@ -0,0 +1,115 @@
/**
* \file PolarStereographic.cpp
* \brief Implementation for GeographicLib::PolarStereographic class
*
* Copyright (c) Charles Karney (2008-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/PolarStereographic.hpp>
#if defined(_MSC_VER)
// Squelch warnings about enum-float expressions
# pragma warning (disable: 5055)
#endif
namespace GeographicLib {
using namespace std;
PolarStereographic::PolarStereographic(real a, real f, real k0)
: _a(a)
, _f(f)
, _e2(_f * (2 - _f))
, _es((_f < 0 ? -1 : 1) * sqrt(fabs(_e2)))
, _e2m(1 - _e2)
, _c( (1 - _f) * exp(Math::eatanhe(real(1), _es)) )
, _k0(k0)
{
if (!(isfinite(_a) && _a > 0))
throw GeographicErr("Equatorial radius is not positive");
if (!(isfinite(_f) && _f < 1))
throw GeographicErr("Polar semi-axis is not positive");
if (!(isfinite(_k0) && _k0 > 0))
throw GeographicErr("Scale is not positive");
}
const PolarStereographic& PolarStereographic::UPS() {
static const PolarStereographic ups(Constants::WGS84_a(),
Constants::WGS84_f(),
Constants::UPS_k0());
return ups;
}
// This formulation converts to conformal coordinates by tau = tan(phi) and
// tau' = tan(phi') where phi' is the conformal latitude. The formulas are:
// tau = tan(phi)
// secphi = hypot(1, tau)
// sig = sinh(e * atanh(e * tau / secphi))
// taup = tan(phip) = tau * hypot(1, sig) - sig * hypot(1, tau)
// c = (1 - f) * exp(e * atanh(e))
//
// Forward:
// rho = (2*k0*a/c) / (hypot(1, taup) + taup) (taup >= 0)
// = (2*k0*a/c) * (hypot(1, taup) - taup) (taup < 0)
//
// Reverse:
// taup = ((2*k0*a/c) / rho - rho / (2*k0*a/c))/2
//
// Scale:
// k = (rho/a) * secphi * sqrt((1-e2) + e2 / secphi^2)
//
// In limit rho -> 0, tau -> inf, taup -> inf, secphi -> inf, secphip -> inf
// secphip = taup = exp(-e * atanh(e)) * tau = exp(-e * atanh(e)) * secphi
void PolarStereographic::Forward(bool northp, real lat, real lon,
real& x, real& y,
real& gamma, real& k) const {
lat = Math::LatFix(lat);
lat *= northp ? 1 : -1;
real
tau = Math::tand(lat),
secphi = hypot(real(1), tau),
taup = Math::taupf(tau, _es),
rho = hypot(real(1), taup) + fabs(taup);
rho = taup >= 0 ? (lat != Math::qd ? 1/rho : 0) : rho;
rho *= 2 * _k0 * _a / _c;
k = lat != Math::qd ?
(rho / _a) * secphi * sqrt(_e2m + _e2 / Math::sq(secphi)) : _k0;
Math::sincosd(lon, x, y);
x *= rho;
y *= (northp ? -rho : rho);
gamma = Math::AngNormalize(northp ? lon : -lon);
}
void PolarStereographic::Reverse(bool northp, real x, real y,
real& lat, real& lon,
real& gamma, real& k) const {
real
rho = hypot(x, y),
t = rho != 0 ? rho / (2 * _k0 * _a / _c) :
Math::sq(numeric_limits<real>::epsilon()),
taup = (1 / t - t) / 2,
tau = Math::tauf(taup, _es),
secphi = hypot(real(1), tau);
k = rho != 0 ? (rho / _a) * secphi * sqrt(_e2m + _e2 / Math::sq(secphi)) :
_k0;
lat = (northp ? 1 : -1) * Math::atand(tau);
lon = Math::atan2d(x, northp ? -y : y );
gamma = Math::AngNormalize(northp ? lon : -lon);
}
void PolarStereographic::SetScale(real lat, real k) {
if (!(isfinite(k) && k > 0))
throw GeographicErr("Scale is not positive");
if (!(-Math::qd < lat && lat <= Math::qd))
throw GeographicErr("Latitude must be in (-" + to_string(Math::qd)
+ "d, " + to_string(Math::qd) + "d]");
real x, y, gamma, kold;
_k0 = 1;
Forward(true, lat, 0, x, y, gamma, kold);
_k0 *= k/kold;
}
} // namespace GeographicLib

229
libs/geographiclib/src/PolygonArea.cpp Normal file
View File

@@ -0,0 +1,229 @@
/**
* \file PolygonArea.cpp
* \brief Implementation for GeographicLib::PolygonAreaT class
*
* Copyright (c) Charles Karney (2010-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/PolygonArea.hpp>
#if defined(_MSC_VER)
// Squelch warnings about enum-float expressions
# pragma warning (disable: 5055)
#endif
namespace GeographicLib {
using namespace std;
template<class GeodType>
int PolygonAreaT<GeodType>::transit(real lon1, real lon2) {
// Return 1 or -1 if crossing prime meridian in east or west direction.
// Otherwise return zero. longitude = +/-0 considered to be positive.
// This is (should be?) compatible with transitdirect which computes
// exactly the parity of
// int(floor((lon1 + lon12) / 360)) - int(floor(lon1 / 360)))
real lon12 = Math::AngDiff(lon1, lon2);
lon1 = Math::AngNormalize(lon1);
lon2 = Math::AngNormalize(lon2);
// N.B. lon12 == 0 gives cross = 0
return
// edge case lon1 = 180, lon2 = 360->0, lon12 = 180 to give 1
lon12 > 0 && ((lon1 < 0 && lon2 >= 0) ||
// lon12 > 0 && lon1 > 0 && lon2 == 0 implies lon1 == 180
(lon1 > 0 && lon2 == 0)) ? 1 :
// non edge case lon1 = -180, lon2 = -360->-0, lon12 = -180
(lon12 < 0 && lon1 >= 0 && lon2 < 0 ? -1 : 0);
// This was the old method (treating +/- 0 as negative). However, with the
// new scheme for handling longitude differences this fails on:
// lon1 = -180, lon2 = -360->-0, lon12 = -180 gives 0 not -1.
// return
// lon1 <= 0 && lon2 > 0 && lon12 > 0 ? 1 :
// (lon2 <= 0 && lon1 > 0 && lon12 < 0 ? -1 : 0);
}
// an alternate version of transit to deal with longitudes in the direct
// problem.
template<class GeodType>
int PolygonAreaT<GeodType>::transitdirect(real lon1, real lon2) {
// Compute exactly the parity of
// int(floor(lon2 / 360)) - int(floor(lon1 / 360))
using std::remainder;
// C++ C remainder -> [-360, 360]
// Java % -> (-720, 720) switch to IEEEremainder -> [-360, 360]
// JS % -> (-720, 720)
// Python fmod -> (-720, 720) swith to Math.remainder
// Fortran, Octave skip
// If mod function gives result in [-360, 360]
// [0, 360) -> 0; [-360, 0) or 360 -> 1
// If mod function gives result in (-720, 720)
// [0, 360) or [-inf, -360) -> 0; [-360, 0) or [360, inf) -> 1
lon1 = remainder(lon1, real(2 * Math::td));
lon2 = remainder(lon2, real(2 * Math::td));
return ( (lon2 >= 0 && lon2 < Math::td ? 0 : 1) -
(lon1 >= 0 && lon1 < Math::td ? 0 : 1) );
}
template<class GeodType>
void PolygonAreaT<GeodType>::AddPoint(real lat, real lon) {
if (_num == 0) {
_lat0 = _lat1 = lat;
_lon0 = _lon1 = lon;
} else {
real s12, S12, t;
_earth.GenInverse(_lat1, _lon1, lat, lon, _mask,
s12, t, t, t, t, t, S12);
_perimetersum += s12;
if (!_polyline) {
_areasum += S12;
_crossings += transit(_lon1, lon);
}
_lat1 = lat; _lon1 = lon;
}
++_num;
}
template<class GeodType>
void PolygonAreaT<GeodType>::AddEdge(real azi, real s) {
if (_num) { // Do nothing if _num is zero
real lat, lon, S12, t;
_earth.GenDirect(_lat1, _lon1, azi, false, s, _mask,
lat, lon, t, t, t, t, t, S12);
_perimetersum += s;
if (!_polyline) {
_areasum += S12;
_crossings += transitdirect(_lon1, lon);
}
_lat1 = lat; _lon1 = lon;
++_num;
}
}
template<class GeodType>
unsigned PolygonAreaT<GeodType>::Compute(bool reverse, bool sign,
real& perimeter, real& area) const
{
real s12, S12, t;
if (_num < 2) {
perimeter = 0;
if (!_polyline)
area = 0;
return _num;
}
if (_polyline) {
perimeter = _perimetersum();
return _num;
}
_earth.GenInverse(_lat1, _lon1, _lat0, _lon0, _mask,
s12, t, t, t, t, t, S12);
perimeter = _perimetersum(s12);
Accumulator<> tempsum(_areasum);
tempsum += S12;
int crossings = _crossings + transit(_lon1, _lon0);
AreaReduce(tempsum, crossings, reverse, sign);
area = real(0) + tempsum();
return _num;
}
template<class GeodType>
unsigned PolygonAreaT<GeodType>::TestPoint(real lat, real lon,
bool reverse, bool sign,
real& perimeter, real& area) const
{
if (_num == 0) {
perimeter = 0;
if (!_polyline)
area = 0;
return 1;
}
perimeter = _perimetersum();
real tempsum = _polyline ? 0 : _areasum();
int crossings = _crossings;
unsigned num = _num + 1;
for (int i = 0; i < (_polyline ? 1 : 2); ++i) {
real s12, S12, t;
_earth.GenInverse(i == 0 ? _lat1 : lat, i == 0 ? _lon1 : lon,
i != 0 ? _lat0 : lat, i != 0 ? _lon0 : lon,
_mask, s12, t, t, t, t, t, S12);
perimeter += s12;
if (!_polyline) {
tempsum += S12;
crossings += transit(i == 0 ? _lon1 : lon,
i != 0 ? _lon0 : lon);
}
}
if (_polyline)
return num;
AreaReduce(tempsum, crossings, reverse, sign);
area = real(0) + tempsum;
return num;
}
template<class GeodType>
unsigned PolygonAreaT<GeodType>::TestEdge(real azi, real s,
bool reverse, bool sign,
real& perimeter, real& area) const
{
if (_num == 0) { // we don't have a starting point!
perimeter = Math::NaN();
if (!_polyline)
area = Math::NaN();
return 0;
}
unsigned num = _num + 1;
perimeter = _perimetersum() + s;
if (_polyline)
return num;
real tempsum = _areasum();
int crossings = _crossings;
{
real lat, lon, s12, S12, t;
_earth.GenDirect(_lat1, _lon1, azi, false, s, _mask,
lat, lon, t, t, t, t, t, S12);
tempsum += S12;
crossings += transitdirect(_lon1, lon);
_earth.GenInverse(lat, lon, _lat0, _lon0, _mask,
s12, t, t, t, t, t, S12);
perimeter += s12;
tempsum += S12;
crossings += transit(lon, _lon0);
}
AreaReduce(tempsum, crossings, reverse, sign);
area = real(0) + tempsum;
return num;
}
template<class GeodType>
template<typename T>
void PolygonAreaT<GeodType>::AreaReduce(T& area, int crossings,
bool reverse, bool sign) const {
Remainder(area);
if (crossings & 1) area += (area < 0 ? 1 : -1) * _area0/2;
// area is with the clockwise sense. If !reverse convert to
// counter-clockwise convention.
if (!reverse) area *= -1;
// If sign put area in (-_area0/2, _area0/2], else put area in [0, _area0)
if (sign) {
if (area > _area0/2)
area -= _area0;
else if (area <= -_area0/2)
area += _area0;
} else {
if (area >= _area0)
area -= _area0;
else if (area < 0)
area += _area0;
}
}
template class GEOGRAPHICLIB_EXPORT PolygonAreaT<Geodesic>;
template class GEOGRAPHICLIB_EXPORT PolygonAreaT<GeodesicExact>;
template class GEOGRAPHICLIB_EXPORT PolygonAreaT<Rhumb>;
} // namespace GeographicLib

387
libs/geographiclib/src/Rhumb.cpp Normal file
View File

@@ -0,0 +1,387 @@
/**
* \file Rhumb.cpp
* \brief Implementation for GeographicLib::Rhumb and GeographicLib::RhumbLine
* classes
*
* Copyright (c) Charles Karney (2014-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <algorithm>
#include <GeographicLib/Rhumb.hpp>
#if defined(_MSC_VER)
// Squelch warnings about enum-float expressions
# pragma warning (disable: 5055)
#endif
namespace GeographicLib {
using namespace std;
Rhumb::Rhumb(real a, real f, bool exact)
: _ell(a, f)
, _exact(exact)
, _c2(_ell.Area() / (2 * Math::td))
{
// Generated by Maxima on 2015-05-15 08:24:04-04:00
#if GEOGRAPHICLIB_RHUMBAREA_ORDER == 4
static const real coeff[] = {
// R[0]/n^0, polynomial in n of order 4
691, 7860, -20160, 18900, 0, 56700,
// R[1]/n^1, polynomial in n of order 3
1772, -5340, 6930, -4725, 14175,
// R[2]/n^2, polynomial in n of order 2
-1747, 1590, -630, 4725,
// R[3]/n^3, polynomial in n of order 1
104, -31, 315,
// R[4]/n^4, polynomial in n of order 0
-41, 420,
}; // count = 20
#elif GEOGRAPHICLIB_RHUMBAREA_ORDER == 5
static const real coeff[] = {
// R[0]/n^0, polynomial in n of order 5
-79036, 22803, 259380, -665280, 623700, 0, 1871100,
// R[1]/n^1, polynomial in n of order 4
41662, 58476, -176220, 228690, -155925, 467775,
// R[2]/n^2, polynomial in n of order 3
18118, -57651, 52470, -20790, 155925,
// R[3]/n^3, polynomial in n of order 2
-23011, 17160, -5115, 51975,
// R[4]/n^4, polynomial in n of order 1
5480, -1353, 13860,
// R[5]/n^5, polynomial in n of order 0
-668, 5775,
}; // count = 27
#elif GEOGRAPHICLIB_RHUMBAREA_ORDER == 6
static const real coeff[] = {
// R[0]/n^0, polynomial in n of order 6
128346268, -107884140, 31126095, 354053700, -908107200, 851350500, 0,
2554051500LL,
// R[1]/n^1, polynomial in n of order 5
-114456994, 56868630, 79819740, -240540300, 312161850, -212837625,
638512875,
// R[2]/n^2, polynomial in n of order 4
51304574, 24731070, -78693615, 71621550, -28378350, 212837625,
// R[3]/n^3, polynomial in n of order 3
1554472, -6282003, 4684680, -1396395, 14189175,
// R[4]/n^4, polynomial in n of order 2
-4913956, 3205800, -791505, 8108100,
// R[5]/n^5, polynomial in n of order 1
1092376, -234468, 2027025,
// R[6]/n^6, polynomial in n of order 0
-313076, 2027025,
}; // count = 35
#elif GEOGRAPHICLIB_RHUMBAREA_ORDER == 7
static const real coeff[] = {
// R[0]/n^0, polynomial in n of order 7
-317195588, 385038804, -323652420, 93378285, 1062161100, -2724321600LL,
2554051500LL, 0, 7662154500LL,
// R[1]/n^1, polynomial in n of order 6
258618446, -343370982, 170605890, 239459220, -721620900, 936485550,
-638512875, 1915538625,
// R[2]/n^2, polynomial in n of order 5
-248174686, 153913722, 74193210, -236080845, 214864650, -85135050,
638512875,
// R[3]/n^3, polynomial in n of order 4
114450437, 23317080, -94230045, 70270200, -20945925, 212837625,
// R[4]/n^4, polynomial in n of order 3
15445736, -103193076, 67321800, -16621605, 170270100,
// R[5]/n^5, polynomial in n of order 2
-27766753, 16385640, -3517020, 30405375,
// R[6]/n^6, polynomial in n of order 1
4892722, -939228, 6081075,
// R[7]/n^7, polynomial in n of order 0
-3189007, 14189175,
}; // count = 44
#elif GEOGRAPHICLIB_RHUMBAREA_ORDER == 8
static const real coeff[] = {
// R[0]/n^0, polynomial in n of order 8
71374704821LL, -161769749880LL, 196369790040LL, -165062734200LL,
47622925350LL, 541702161000LL, -1389404016000LL, 1302566265000LL, 0,
3907698795000LL,
// R[1]/n^1, polynomial in n of order 7
-13691187484LL, 65947703730LL, -87559600410LL, 43504501950LL,
61062101100LL, -184013329500LL, 238803815250LL, -162820783125LL,
488462349375LL,
// R[2]/n^2, polynomial in n of order 6
30802104839LL, -63284544930LL, 39247999110LL, 18919268550LL,
-60200615475LL, 54790485750LL, -21709437750LL, 162820783125LL,
// R[3]/n^3, polynomial in n of order 5
-8934064508LL, 5836972287LL, 1189171080, -4805732295LL, 3583780200LL,
-1068242175, 10854718875LL,
// R[4]/n^4, polynomial in n of order 4
50072287748LL, 3938662680LL, -26314234380LL, 17167059000LL,
-4238509275LL, 43418875500LL,
// R[5]/n^5, polynomial in n of order 3
359094172, -9912730821LL, 5849673480LL, -1255576140, 10854718875LL,
// R[6]/n^6, polynomial in n of order 2
-16053944387LL, 8733508770LL, -1676521980, 10854718875LL,
// R[7]/n^7, polynomial in n of order 1
930092876, -162639357, 723647925,
// R[8]/n^8, polynomial in n of order 0
-673429061, 1929727800,
}; // count = 54
#else
#error "Bad value for GEOGRAPHICLIB_RHUMBAREA_ORDER"
#endif
static_assert(sizeof(coeff) / sizeof(real) ==
((maxpow_ + 1) * (maxpow_ + 4))/2,
"Coefficient array size mismatch for Rhumb");
real d = 1;
int o = 0;
for (int l = 0; l <= maxpow_; ++l) {
int m = maxpow_ - l;
// R[0] is just an integration constant so it cancels when evaluating a
// definite integral. So don't bother computing it. It won't be used
// when invoking SinCosSeries.
if (l)
_rR[l] = d * Math::polyval(m, coeff + o, _ell._n) / coeff[o + m + 1];
o += m + 2;
d *= _ell._n;
}
// Post condition: o == sizeof(alpcoeff) / sizeof(real)
}
const Rhumb& Rhumb::WGS84() {
static const Rhumb
wgs84(Constants::WGS84_a(), Constants::WGS84_f(), false);
return wgs84;
}
void Rhumb::GenInverse(real lat1, real lon1, real lat2, real lon2,
unsigned outmask,
real& s12, real& azi12, real& S12) const {
real
lon12 = Math::AngDiff(lon1, lon2),
psi1 = _ell.IsometricLatitude(lat1),
psi2 = _ell.IsometricLatitude(lat2),
psi12 = psi2 - psi1,
h = hypot(lon12, psi12);
if (outmask & AZIMUTH)
azi12 = Math::atan2d(lon12, psi12);
if (outmask & DISTANCE) {
real dmudpsi = DIsometricToRectifying(psi2, psi1);
s12 = h * dmudpsi * _ell.QuarterMeridian() / Math::qd;
}
if (outmask & AREA)
S12 = _c2 * lon12 *
MeanSinXi(psi2 * Math::degree(), psi1 * Math::degree());
}
RhumbLine Rhumb::Line(real lat1, real lon1, real azi12) const
{ return RhumbLine(*this, lat1, lon1, azi12); }
void Rhumb::GenDirect(real lat1, real lon1, real azi12, real s12,
unsigned outmask,
real& lat2, real& lon2, real& S12) const
{ Line(lat1, lon1, azi12).GenPosition(s12, outmask, lat2, lon2, S12); }
Math::real Rhumb::DE(real x, real y) const {
const EllipticFunction& ei = _ell._ell;
real d = x - y;
if (x * y <= 0)
return d != 0 ? (ei.E(x) - ei.E(y)) / d : 1;
// See DLMF: Eqs (19.11.2) and (19.11.4) letting
// theta -> x, phi -> -y, psi -> z
//
// (E(x) - E(y)) / d = E(z)/d - k2 * sin(x) * sin(y) * sin(z)/d
//
// tan(z/2) = (sin(x)*Delta(y) - sin(y)*Delta(x)) / (cos(x) + cos(y))
// = d * Dsin(x,y) * (sin(x) + sin(y))/(cos(x) + cos(y)) /
// (sin(x)*Delta(y) + sin(y)*Delta(x))
// = t = d * Dt
// sin(z) = 2*t/(1+t^2); cos(z) = (1-t^2)/(1+t^2)
// Alt (this only works for |z| <= pi/2 -- however, this conditions holds
// if x*y > 0):
// sin(z) = d * Dsin(x,y) * (sin(x) + sin(y))/
// (sin(x)*cos(y)*Delta(y) + sin(y)*cos(x)*Delta(x))
// cos(z) = sqrt((1-sin(z))*(1+sin(z)))
real sx = sin(x), sy = sin(y), cx = cos(x), cy = cos(y);
real Dt = Dsin(x, y) * (sx + sy) /
((cx + cy) * (sx * ei.Delta(sy, cy) + sy * ei.Delta(sx, cx))),
t = d * Dt, Dsz = 2 * Dt / (1 + t*t),
sz = d * Dsz, cz = (1 - t) * (1 + t) / (1 + t*t);
return ((sz != 0 ? ei.E(sz, cz, ei.Delta(sz, cz)) / sz : 1)
- ei.k2() * sx * sy) * Dsz;
}
Math::real Rhumb::DRectifying(real latx, real laty) const {
real
tbetx = _ell._f1 * Math::tand(latx),
tbety = _ell._f1 * Math::tand(laty);
return (Math::pi()/2) * _ell._b * _ell._f1 * DE(atan(tbetx), atan(tbety))
* Dtan(latx, laty) * Datan(tbetx, tbety) / _ell.QuarterMeridian();
}
Math::real Rhumb::DIsometric(real latx, real laty) const {
real
phix = latx * Math::degree(), tx = Math::tand(latx),
phiy = laty * Math::degree(), ty = Math::tand(laty);
return Dasinh(tx, ty) * Dtan(latx, laty)
- Deatanhe(sin(phix), sin(phiy)) * Dsin(phix, phiy);
}
Math::real Rhumb::SinCosSeries(bool sinp,
real x, real y, const real c[], int n) {
// N.B. n >= 0 and c[] has n+1 elements 0..n, of which c[0] is ignored.
//
// Use Clenshaw summation to evaluate
// m = (g(x) + g(y)) / 2 -- mean value
// s = (g(x) - g(y)) / (x - y) -- average slope
// where
// g(x) = sum(c[j]*SC(2*j*x), j = 1..n)
// SC = sinp ? sin : cos
// CS = sinp ? cos : sin
//
// This function returns only s; m is discarded.
//
// Write
// t = [m; s]
// t = sum(c[j] * f[j](x,y), j = 1..n)
// where
// f[j](x,y) = [ (SC(2*j*x)+SC(2*j*y))/2 ]
// [ (SC(2*j*x)-SC(2*j*y))/d ]
//
// = [ cos(j*d)*SC(j*p) ]
// [ +/-(2/d)*sin(j*d)*CS(j*p) ]
// (+/- = sinp ? + : -) and
// p = x+y, d = x-y
//
// f[j+1](x,y) = A * f[j](x,y) - f[j-1](x,y)
//
// A = [ 2*cos(p)*cos(d) -sin(p)*sin(d)*d]
// [ -4*sin(p)*sin(d)/d 2*cos(p)*cos(d) ]
//
// Let b[n+1] = b[n+2] = [0 0; 0 0]
// b[j] = A * b[j+1] - b[j+2] + c[j] * I for j = n..1
// t = (c[0] * I - b[2]) * f[0](x,y) + b[1] * f[1](x,y)
// c[0] is not accessed for s = t[2]
real p = x + y, d = x - y,
cp = cos(p), cd = cos(d),
sp = sin(p), sd = d != 0 ? sin(d)/d : 1,
m = 2 * cp * cd, s = sp * sd;
// 2x2 matrices stored in row-major order
const real a[4] = {m, -s * d * d, -4 * s, m};
real ba[4] = {0, 0, 0, 0};
real bb[4] = {0, 0, 0, 0};
real* b1 = ba;
real* b2 = bb;
if (n > 0) b1[0] = b1[3] = c[n];
for (int j = n - 1; j > 0; --j) { // j = n-1 .. 1
swap(b1, b2);
// b1 = A * b2 - b1 + c[j] * I
b1[0] = a[0] * b2[0] + a[1] * b2[2] - b1[0] + c[j];
b1[1] = a[0] * b2[1] + a[1] * b2[3] - b1[1];
b1[2] = a[2] * b2[0] + a[3] * b2[2] - b1[2];
b1[3] = a[2] * b2[1] + a[3] * b2[3] - b1[3] + c[j];
}
// Here are the full expressions for m and s
// m = (c[0] - b2[0]) * f01 - b2[1] * f02 + b1[0] * f11 + b1[1] * f12;
// s = - b2[2] * f01 + (c[0] - b2[3]) * f02 + b1[2] * f11 + b1[3] * f12;
if (sinp) {
// real f01 = 0, f02 = 0;
real f11 = cd * sp, f12 = 2 * sd * cp;
// m = b1[0] * f11 + b1[1] * f12;
s = b1[2] * f11 + b1[3] * f12;
} else {
// real f01 = 1, f02 = 0;
real f11 = cd * cp, f12 = - 2 * sd * sp;
// m = c[0] - b2[0] + b1[0] * f11 + b1[1] * f12;
s = - b2[2] + b1[2] * f11 + b1[3] * f12;
}
return s;
}
Math::real Rhumb::DConformalToRectifying(real chix, real chiy) const {
return 1 + SinCosSeries(true, chix, chiy,
_ell.ConformalToRectifyingCoeffs(), tm_maxord);
}
Math::real Rhumb::DRectifyingToConformal(real mux, real muy) const {
return 1 - SinCosSeries(true, mux, muy,
_ell.RectifyingToConformalCoeffs(), tm_maxord);
}
Math::real Rhumb::DIsometricToRectifying(real psix, real psiy) const {
if (_exact) {
real
latx = _ell.InverseIsometricLatitude(psix),
laty = _ell.InverseIsometricLatitude(psiy);
return DRectifying(latx, laty) / DIsometric(latx, laty);
} else {
psix *= Math::degree();
psiy *= Math::degree();
return DConformalToRectifying(gd(psix), gd(psiy)) * Dgd(psix, psiy);
}
}
Math::real Rhumb::DRectifyingToIsometric(real mux, real muy) const {
real
latx = _ell.InverseRectifyingLatitude(mux/Math::degree()),
laty = _ell.InverseRectifyingLatitude(muy/Math::degree());
return _exact ?
DIsometric(latx, laty) / DRectifying(latx, laty) :
Dgdinv(Math::taupf(Math::tand(latx), _ell._es),
Math::taupf(Math::tand(laty), _ell._es)) *
DRectifyingToConformal(mux, muy);
}
Math::real Rhumb::MeanSinXi(real psix, real psiy) const {
return Dlog(cosh(psix), cosh(psiy)) * Dcosh(psix, psiy)
+ SinCosSeries(false, gd(psix), gd(psiy), _rR, maxpow_) * Dgd(psix, psiy);
}
RhumbLine::RhumbLine(const Rhumb& rh, real lat1, real lon1, real azi12)
: _rh(rh)
, _lat1(Math::LatFix(lat1))
, _lon1(lon1)
, _azi12(Math::AngNormalize(azi12))
{
real alp12 = _azi12 * Math::degree();
_salp = _azi12 == -Math::hd ? 0 : sin(alp12);
_calp = fabs(_azi12) == Math::qd ? 0 : cos(alp12);
_mu1 = _rh._ell.RectifyingLatitude(lat1);
_psi1 = _rh._ell.IsometricLatitude(lat1);
_r1 = _rh._ell.CircleRadius(lat1);
}
void RhumbLine::GenPosition(real s12, unsigned outmask,
real& lat2, real& lon2, real& S12) const {
real
mu12 = s12 * _calp * Math::qd / _rh._ell.QuarterMeridian(),
mu2 = _mu1 + mu12;
real psi2, lat2x, lon2x;
if (fabs(mu2) <= Math::qd) {
if (_calp != 0) {
lat2x = _rh._ell.InverseRectifyingLatitude(mu2);
real psi12 = _rh.DRectifyingToIsometric( mu2 * Math::degree(),
_mu1 * Math::degree()) * mu12;
lon2x = _salp * psi12 / _calp;
psi2 = _psi1 + psi12;
} else {
lat2x = _lat1;
lon2x = _salp * s12 / (_r1 * Math::degree());
psi2 = _psi1;
}
if (outmask & AREA)
S12 = _rh._c2 * lon2x *
_rh.MeanSinXi(_psi1 * Math::degree(), psi2 * Math::degree());
lon2x = outmask & LONG_UNROLL ? _lon1 + lon2x :
Math::AngNormalize(Math::AngNormalize(_lon1) + lon2x);
} else {
// Reduce to the interval [-180, 180)
mu2 = Math::AngNormalize(mu2);
// Deal with points on the anti-meridian
if (fabs(mu2) > Math::qd) mu2 = Math::AngNormalize(Math::hd - mu2);
lat2x = _rh._ell.InverseRectifyingLatitude(mu2);
lon2x = Math::NaN();
if (outmask & AREA)
S12 = Math::NaN();
}
if (outmask & LATITUDE) lat2 = lat2x;
if (outmask & LONGITUDE) lon2 = lon2x;
}
} // namespace GeographicLib

510
libs/geographiclib/src/SphericalEngine.cpp Normal file
View File

@@ -0,0 +1,510 @@
/**
* \file SphericalEngine.cpp
* \brief Implementation for GeographicLib::SphericalEngine class
*
* Copyright (c) Charles Karney (2011-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
*
* The general sum is\verbatim
V(r, theta, lambda) = sum(n = 0..N) sum(m = 0..n)
q^(n+1) * (C[n,m] * cos(m*lambda) + S[n,m] * sin(m*lambda)) * P[n,m](t)
\endverbatim
* where <tt>t = cos(theta)</tt>, <tt>q = a/r</tt>. In addition write <tt>u =
* sin(theta)</tt>.
*
* <tt>P[n,m]</tt> is a normalized associated Legendre function of degree
* <tt>n</tt> and order <tt>m</tt>. Here the formulas are given for full
* normalized functions (usually denoted <tt>Pbar</tt>).
*
* Rewrite outer sum\verbatim
V(r, theta, lambda) = sum(m = 0..N) * P[m,m](t) * q^(m+1) *
[Sc[m] * cos(m*lambda) + Ss[m] * sin(m*lambda)]
\endverbatim
* where the inner sums are\verbatim
Sc[m] = sum(n = m..N) q^(n-m) * C[n,m] * P[n,m](t)/P[m,m](t)
Ss[m] = sum(n = m..N) q^(n-m) * S[n,m] * P[n,m](t)/P[m,m](t)
\endverbatim
* Evaluate sums via Clenshaw method. The overall framework is similar to
* Deakin with the following changes:
* - Clenshaw summation is used to roll the computation of
* <tt>cos(m*lambda)</tt> and <tt>sin(m*lambda)</tt> into the evaluation of
* the outer sum (rather than independently computing an array of these
* trigonometric terms).
* - Scale the coefficients to guard against overflow when <tt>N</tt> is large.
* .
* For the general framework of Clenshaw, see
* http://mathworld.wolfram.com/ClenshawRecurrenceFormula.html
*
* Let\verbatim
S = sum(k = 0..N) c[k] * F[k](x)
F[n+1](x) = alpha[n](x) * F[n](x) + beta[n](x) * F[n-1](x)
\endverbatim
* Evaluate <tt>S</tt> with\verbatim
y[N+2] = y[N+1] = 0
y[k] = alpha[k] * y[k+1] + beta[k+1] * y[k+2] + c[k]
S = c[0] * F[0] + y[1] * F[1] + beta[1] * F[0] * y[2]
\endverbatim
* \e IF <tt>F[0](x) = 1</tt> and <tt>beta(0,x) = 0</tt>, then <tt>F[1](x) =
* alpha(0,x)</tt> and we can continue the recursion for <tt>y[k]</tt> until
* <tt>y[0]</tt>, giving\verbatim
S = y[0]
\endverbatim
*
* Evaluating the inner sum\verbatim
l = n-m; n = l+m
Sc[m] = sum(l = 0..N-m) C[l+m,m] * q^l * P[l+m,m](t)/P[m,m](t)
F[l] = q^l * P[l+m,m](t)/P[m,m](t)
\endverbatim
* Holmes + Featherstone, Eq. (11), give\verbatim
P[n,m] = sqrt((2*n-1)*(2*n+1)/((n-m)*(n+m))) * t * P[n-1,m] -
sqrt((2*n+1)*(n+m-1)*(n-m-1)/((n-m)*(n+m)*(2*n-3))) * P[n-2,m]
\endverbatim
* thus\verbatim
alpha[l] = t * q * sqrt(((2*n+1)*(2*n+3))/
((n-m+1)*(n+m+1)))
beta[l+1] = - q^2 * sqrt(((n-m+1)*(n+m+1)*(2*n+5))/
((n-m+2)*(n+m+2)*(2*n+1)))
\endverbatim
* In this case, <tt>F[0] = 1</tt> and <tt>beta[0] = 0</tt>, so the <tt>Sc[m]
* = y[0]</tt>.
*
* Evaluating the outer sum\verbatim
V = sum(m = 0..N) Sc[m] * q^(m+1) * cos(m*lambda) * P[m,m](t)
+ sum(m = 0..N) Ss[m] * q^(m+1) * cos(m*lambda) * P[m,m](t)
F[m] = q^(m+1) * cos(m*lambda) * P[m,m](t) [or sin(m*lambda)]
\endverbatim
* Holmes + Featherstone, Eq. (13), give\verbatim
P[m,m] = u * sqrt((2*m+1)/((m>1?2:1)*m)) * P[m-1,m-1]
\endverbatim
* also, we have\verbatim
cos((m+1)*lambda) = 2*cos(lambda)*cos(m*lambda) - cos((m-1)*lambda)
\endverbatim
* thus\verbatim
alpha[m] = 2*cos(lambda) * sqrt((2*m+3)/(2*(m+1))) * u * q
= cos(lambda) * sqrt( 2*(2*m+3)/(m+1) ) * u * q
beta[m+1] = -sqrt((2*m+3)*(2*m+5)/(4*(m+1)*(m+2))) * u^2 * q^2
* (m == 0 ? sqrt(2) : 1)
\endverbatim
* Thus\verbatim
F[0] = q [or 0]
F[1] = cos(lambda) * sqrt(3) * u * q^2 [or sin(lambda)]
beta[1] = - sqrt(15/4) * u^2 * q^2
\endverbatim
*
* Here is how the various components of the gradient are computed
*
* Differentiate wrt <tt>r</tt>\verbatim
d q^(n+1) / dr = (-1/r) * (n+1) * q^(n+1)
\endverbatim
* so multiply <tt>C[n,m]</tt> by <tt>n+1</tt> in inner sum and multiply the
* sum by <tt>-1/r</tt>.
*
* Differentiate wrt <tt>lambda</tt>\verbatim
d cos(m*lambda) = -m * sin(m*lambda)
d sin(m*lambda) = m * cos(m*lambda)
\endverbatim
* so multiply terms by <tt>m</tt> in outer sum and swap sine and cosine
* variables.
*
* Differentiate wrt <tt>theta</tt>\verbatim
dV/dtheta = V' = -u * dV/dt = -u * V'
\endverbatim
* here <tt>'</tt> denotes differentiation wrt <tt>theta</tt>.\verbatim
d/dtheta (Sc[m] * P[m,m](t)) = Sc'[m] * P[m,m](t) + Sc[m] * P'[m,m](t)
\endverbatim
* Now <tt>P[m,m](t) = const * u^m</tt>, so <tt>P'[m,m](t) = m * t/u *
* P[m,m](t)</tt>, thus\verbatim
d/dtheta (Sc[m] * P[m,m](t)) = (Sc'[m] + m * t/u * Sc[m]) * P[m,m](t)
\endverbatim
* Clenshaw recursion for <tt>Sc[m]</tt> reads\verbatim
y[k] = alpha[k] * y[k+1] + beta[k+1] * y[k+2] + c[k]
\endverbatim
* Substituting <tt>alpha[k] = const * t</tt>, <tt>alpha'[k] = -u/t *
* alpha[k]</tt>, <tt>beta'[k] = c'[k] = 0</tt> gives\verbatim
y'[k] = alpha[k] * y'[k+1] + beta[k+1] * y'[k+2] - u/t * alpha[k] * y[k+1]
\endverbatim
*
* Finally, given the derivatives of <tt>V</tt>, we can compute the components
* of the gradient in spherical coordinates and transform the result into
* cartesian coordinates.
**********************************************************************/
#include <GeographicLib/SphericalEngine.hpp>
#include <GeographicLib/CircularEngine.hpp>
#include <GeographicLib/Utility.hpp>
#if defined(_MSC_VER)
// Squelch warnings about constant conditional expressions and potentially
// uninitialized local variables
# pragma warning (disable: 4127 4701)
#endif
namespace GeographicLib {
using namespace std;
vector<Math::real>& SphericalEngine::sqrttable() {
static vector<real> sqrttable(0);
return sqrttable;
}
template<bool gradp, SphericalEngine::normalization norm, int L>
Math::real SphericalEngine::Value(const coeff c[], const real f[],
real x, real y, real z, real a,
real& gradx, real& grady, real& gradz)
{
static_assert(L > 0, "L must be positive");
static_assert(norm == FULL || norm == SCHMIDT, "Unknown normalization");
int N = c[0].nmx(), M = c[0].mmx();
real
p = hypot(x, y),
cl = p != 0 ? x / p : 1, // cos(lambda); at pole, pick lambda = 0
sl = p != 0 ? y / p : 0, // sin(lambda)
r = hypot(z, p),
t = r != 0 ? z / r : 0, // cos(theta); at origin, pick theta = pi/2
u = r != 0 ? fmax(p / r, eps()) : 1, // sin(theta); but avoid the pole
q = a / r;
real
q2 = Math::sq(q),
uq = u * q,
uq2 = Math::sq(uq),
tu = t / u;
// Initialize outer sum
real vc = 0, vc2 = 0, vs = 0, vs2 = 0; // v [N + 1], v [N + 2]
// vr, vt, vl and similar w variable accumulate the sums for the
// derivatives wrt r, theta, and lambda, respectively.
real vrc = 0, vrc2 = 0, vrs = 0, vrs2 = 0; // vr[N + 1], vr[N + 2]
real vtc = 0, vtc2 = 0, vts = 0, vts2 = 0; // vt[N + 1], vt[N + 2]
real vlc = 0, vlc2 = 0, vls = 0, vls2 = 0; // vl[N + 1], vl[N + 2]
int k[L];
const vector<real>& root( sqrttable() );
for (int m = M; m >= 0; --m) { // m = M .. 0
// Initialize inner sum
real
wc = 0, wc2 = 0, ws = 0, ws2 = 0, // w [N - m + 1], w [N - m + 2]
wrc = 0, wrc2 = 0, wrs = 0, wrs2 = 0, // wr[N - m + 1], wr[N - m + 2]
wtc = 0, wtc2 = 0, wts = 0, wts2 = 0; // wt[N - m + 1], wt[N - m + 2]
for (int l = 0; l < L; ++l)
k[l] = c[l].index(N, m) + 1;
for (int n = N; n >= m; --n) { // n = N .. m; l = N - m .. 0
real w, A, Ax, B, R; // alpha[l], beta[l + 1]
switch (norm) {
case FULL:
w = root[2 * n + 1] / (root[n - m + 1] * root[n + m + 1]);
Ax = q * w * root[2 * n + 3];
A = t * Ax;
B = - q2 * root[2 * n + 5] /
(w * root[n - m + 2] * root[n + m + 2]);
break;
case SCHMIDT:
w = root[n - m + 1] * root[n + m + 1];
Ax = q * (2 * n + 1) / w;
A = t * Ax;
B = - q2 * w / (root[n - m + 2] * root[n + m + 2]);
break;
default: break; // To suppress warning message from Visual Studio
}
R = c[0].Cv(--k[0]);
for (int l = 1; l < L; ++l)
R += c[l].Cv(--k[l], n, m, f[l]);
R *= scale();
w = A * wc + B * wc2 + R; wc2 = wc; wc = w;
if (gradp) {
w = A * wrc + B * wrc2 + (n + 1) * R; wrc2 = wrc; wrc = w;
w = A * wtc + B * wtc2 - u*Ax * wc2; wtc2 = wtc; wtc = w;
}
if (m) {
R = c[0].Sv(k[0]);
for (int l = 1; l < L; ++l)
R += c[l].Sv(k[l], n, m, f[l]);
R *= scale();
w = A * ws + B * ws2 + R; ws2 = ws; ws = w;
if (gradp) {
w = A * wrs + B * wrs2 + (n + 1) * R; wrs2 = wrs; wrs = w;
w = A * wts + B * wts2 - u*Ax * ws2; wts2 = wts; wts = w;
}
}
}
// Now Sc[m] = wc, Ss[m] = ws
// Sc'[m] = wtc, Ss'[m] = wtc
if (m) {
real v, A, B; // alpha[m], beta[m + 1]
switch (norm) {
case FULL:
v = root[2] * root[2 * m + 3] / root[m + 1];
A = cl * v * uq;
B = - v * root[2 * m + 5] / (root[8] * root[m + 2]) * uq2;
break;
case SCHMIDT:
v = root[2] * root[2 * m + 1] / root[m + 1];
A = cl * v * uq;
B = - v * root[2 * m + 3] / (root[8] * root[m + 2]) * uq2;
break;
default: break; // To suppress warning message from Visual Studio
}
v = A * vc + B * vc2 + wc ; vc2 = vc ; vc = v;
v = A * vs + B * vs2 + ws ; vs2 = vs ; vs = v;
if (gradp) {
// Include the terms Sc[m] * P'[m,m](t) and Ss[m] * P'[m,m](t)
wtc += m * tu * wc; wts += m * tu * ws;
v = A * vrc + B * vrc2 + wrc; vrc2 = vrc; vrc = v;
v = A * vrs + B * vrs2 + wrs; vrs2 = vrs; vrs = v;
v = A * vtc + B * vtc2 + wtc; vtc2 = vtc; vtc = v;
v = A * vts + B * vts2 + wts; vts2 = vts; vts = v;
v = A * vlc + B * vlc2 + m*ws; vlc2 = vlc; vlc = v;
v = A * vls + B * vls2 - m*wc; vls2 = vls; vls = v;
}
} else {
real A, B, qs;
switch (norm) {
case FULL:
A = root[3] * uq; // F[1]/(q*cl) or F[1]/(q*sl)
B = - root[15]/2 * uq2; // beta[1]/q
break;
case SCHMIDT:
A = uq;
B = - root[3]/2 * uq2;
break;
default: break; // To suppress warning message from Visual Studio
}
qs = q / scale();
vc = qs * (wc + A * (cl * vc + sl * vs ) + B * vc2);
if (gradp) {
qs /= r;
// The components of the gradient in spherical coordinates are
// r: dV/dr
// theta: 1/r * dV/dtheta
// lambda: 1/(r*u) * dV/dlambda
vrc = - qs * (wrc + A * (cl * vrc + sl * vrs) + B * vrc2);
vtc = qs * (wtc + A * (cl * vtc + sl * vts) + B * vtc2);
vlc = qs / u * ( A * (cl * vlc + sl * vls) + B * vlc2);
}
}
}
if (gradp) {
// Rotate into cartesian (geocentric) coordinates
gradx = cl * (u * vrc + t * vtc) - sl * vlc;
grady = sl * (u * vrc + t * vtc) + cl * vlc;
gradz = t * vrc - u * vtc ;
}
return vc;
}
template<bool gradp, SphericalEngine::normalization norm, int L>
CircularEngine SphericalEngine::Circle(const coeff c[], const real f[],
real p, real z, real a) {
static_assert(L > 0, "L must be positive");
static_assert(norm == FULL || norm == SCHMIDT, "Unknown normalization");
int N = c[0].nmx(), M = c[0].mmx();
real
r = hypot(z, p),
t = r != 0 ? z / r : 0, // cos(theta); at origin, pick theta = pi/2
u = r != 0 ? fmax(p / r, eps()) : 1, // sin(theta); but avoid the pole
q = a / r;
real
q2 = Math::sq(q),
tu = t / u;
CircularEngine circ(M, gradp, norm, a, r, u, t);
int k[L];
const vector<real>& root( sqrttable() );
for (int m = M; m >= 0; --m) { // m = M .. 0
// Initialize inner sum
real
wc = 0, wc2 = 0, ws = 0, ws2 = 0, // w [N - m + 1], w [N - m + 2]
wrc = 0, wrc2 = 0, wrs = 0, wrs2 = 0, // wr[N - m + 1], wr[N - m + 2]
wtc = 0, wtc2 = 0, wts = 0, wts2 = 0; // wt[N - m + 1], wt[N - m + 2]
for (int l = 0; l < L; ++l)
k[l] = c[l].index(N, m) + 1;
for (int n = N; n >= m; --n) { // n = N .. m; l = N - m .. 0
real w, A, Ax, B, R; // alpha[l], beta[l + 1]
switch (norm) {
case FULL:
w = root[2 * n + 1] / (root[n - m + 1] * root[n + m + 1]);
Ax = q * w * root[2 * n + 3];
A = t * Ax;
B = - q2 * root[2 * n + 5] /
(w * root[n - m + 2] * root[n + m + 2]);
break;
case SCHMIDT:
w = root[n - m + 1] * root[n + m + 1];
Ax = q * (2 * n + 1) / w;
A = t * Ax;
B = - q2 * w / (root[n - m + 2] * root[n + m + 2]);
break;
default: break; // To suppress warning message from Visual Studio
}
R = c[0].Cv(--k[0]);
for (int l = 1; l < L; ++l)
R += c[l].Cv(--k[l], n, m, f[l]);
R *= scale();
w = A * wc + B * wc2 + R; wc2 = wc; wc = w;
if (gradp) {
w = A * wrc + B * wrc2 + (n + 1) * R; wrc2 = wrc; wrc = w;
w = A * wtc + B * wtc2 - u*Ax * wc2; wtc2 = wtc; wtc = w;
}
if (m) {
R = c[0].Sv(k[0]);
for (int l = 1; l < L; ++l)
R += c[l].Sv(k[l], n, m, f[l]);
R *= scale();
w = A * ws + B * ws2 + R; ws2 = ws; ws = w;
if (gradp) {
w = A * wrs + B * wrs2 + (n + 1) * R; wrs2 = wrs; wrs = w;
w = A * wts + B * wts2 - u*Ax * ws2; wts2 = wts; wts = w;
}
}
}
if (!gradp)
circ.SetCoeff(m, wc, ws);
else {
// Include the terms Sc[m] * P'[m,m](t) and Ss[m] * P'[m,m](t)
wtc += m * tu * wc; wts += m * tu * ws;
circ.SetCoeff(m, wc, ws, wrc, wrs, wtc, wts);
}
}
return circ;
}
void SphericalEngine::RootTable(int N) {
// Need square roots up to max(2 * N + 5, 15).
vector<real>& root( sqrttable() );
int L = max(2 * N + 5, 15) + 1, oldL = int(root.size());
if (oldL >= L)
return;
root.resize(L);
for (int l = oldL; l < L; ++l)
root[l] = sqrt(real(l));
}
void SphericalEngine::coeff::readcoeffs(istream& stream, int& N, int& M,
vector<real>& C,
vector<real>& S,
bool truncate) {
if (truncate) {
if (!((N >= M && M >= 0) || (N == -1 && M == -1)))
// The last condition is that M = -1 implies N = -1.
throw GeographicErr("Bad requested degree and order " +
Utility::str(N) + " " + Utility::str(M));
}
int nm[2];
Utility::readarray<int, int, false>(stream, nm, 2);
int N0 = nm[0], M0 = nm[1];
if (!((N0 >= M0 && M0 >= 0) || (N0 == -1 && M0 == -1)))
// The last condition is that M0 = -1 implies N0 = -1.
throw GeographicErr("Bad degree and order " +
Utility::str(N0) + " " + Utility::str(M0));
N = truncate ? min(N, N0) : N0;
M = truncate ? min(M, M0) : M0;
C.resize(SphericalEngine::coeff::Csize(N, M));
S.resize(SphericalEngine::coeff::Ssize(N, M));
int skip = (SphericalEngine::coeff::Csize(N0, M0) -
SphericalEngine::coeff::Csize(N0, M )) * sizeof(double);
if (N == N0) {
Utility::readarray<double, real, false>(stream, C);
if (skip) stream.seekg(streamoff(skip), ios::cur);
Utility::readarray<double, real, false>(stream, S);
if (skip) stream.seekg(streamoff(skip), ios::cur);
} else {
for (int m = 0, k = 0; m <= M; ++m) {
Utility::readarray<double, real, false>(stream, &C[k], N + 1 - m);
stream.seekg((N0 - N) * sizeof(double), ios::cur);
k += N + 1 - m;
}
if (skip) stream.seekg(streamoff(skip), ios::cur);
for (int m = 1, k = 0; m <= M; ++m) {
Utility::readarray<double, real, false>(stream, &S[k], N + 1 - m);
stream.seekg((N0 - N) * sizeof(double), ios::cur);
k += N + 1 - m;
}
if (skip) stream.seekg(streamoff(skip), ios::cur);
}
return;
}
/// \cond SKIP
template Math::real GEOGRAPHICLIB_EXPORT
SphericalEngine::Value<true, SphericalEngine::FULL, 1>
(const coeff[], const real[], real, real, real, real, real&, real&, real&);
template Math::real GEOGRAPHICLIB_EXPORT
SphericalEngine::Value<false, SphericalEngine::FULL, 1>
(const coeff[], const real[], real, real, real, real, real&, real&, real&);
template Math::real GEOGRAPHICLIB_EXPORT
SphericalEngine::Value<true, SphericalEngine::SCHMIDT, 1>
(const coeff[], const real[], real, real, real, real, real&, real&, real&);
template Math::real GEOGRAPHICLIB_EXPORT
SphericalEngine::Value<false, SphericalEngine::SCHMIDT, 1>
(const coeff[], const real[], real, real, real, real, real&, real&, real&);
template Math::real GEOGRAPHICLIB_EXPORT
SphericalEngine::Value<true, SphericalEngine::FULL, 2>
(const coeff[], const real[], real, real, real, real, real&, real&, real&);
template Math::real GEOGRAPHICLIB_EXPORT
SphericalEngine::Value<false, SphericalEngine::FULL, 2>
(const coeff[], const real[], real, real, real, real, real&, real&, real&);
template Math::real GEOGRAPHICLIB_EXPORT
SphericalEngine::Value<true, SphericalEngine::SCHMIDT, 2>
(const coeff[], const real[], real, real, real, real, real&, real&, real&);
template Math::real GEOGRAPHICLIB_EXPORT
SphericalEngine::Value<false, SphericalEngine::SCHMIDT, 2>
(const coeff[], const real[], real, real, real, real, real&, real&, real&);
template Math::real GEOGRAPHICLIB_EXPORT
SphericalEngine::Value<true, SphericalEngine::FULL, 3>
(const coeff[], const real[], real, real, real, real, real&, real&, real&);
template Math::real GEOGRAPHICLIB_EXPORT
SphericalEngine::Value<false, SphericalEngine::FULL, 3>
(const coeff[], const real[], real, real, real, real, real&, real&, real&);
template Math::real GEOGRAPHICLIB_EXPORT
SphericalEngine::Value<true, SphericalEngine::SCHMIDT, 3>
(const coeff[], const real[], real, real, real, real, real&, real&, real&);
template Math::real GEOGRAPHICLIB_EXPORT
SphericalEngine::Value<false, SphericalEngine::SCHMIDT, 3>
(const coeff[], const real[], real, real, real, real, real&, real&, real&);
template CircularEngine GEOGRAPHICLIB_EXPORT
SphericalEngine::Circle<true, SphericalEngine::FULL, 1>
(const coeff[], const real[], real, real, real);
template CircularEngine GEOGRAPHICLIB_EXPORT
SphericalEngine::Circle<false, SphericalEngine::FULL, 1>
(const coeff[], const real[], real, real, real);
template CircularEngine GEOGRAPHICLIB_EXPORT
SphericalEngine::Circle<true, SphericalEngine::SCHMIDT, 1>
(const coeff[], const real[], real, real, real);
template CircularEngine GEOGRAPHICLIB_EXPORT
SphericalEngine::Circle<false, SphericalEngine::SCHMIDT, 1>
(const coeff[], const real[], real, real, real);
template CircularEngine GEOGRAPHICLIB_EXPORT
SphericalEngine::Circle<true, SphericalEngine::FULL, 2>
(const coeff[], const real[], real, real, real);
template CircularEngine GEOGRAPHICLIB_EXPORT
SphericalEngine::Circle<false, SphericalEngine::FULL, 2>
(const coeff[], const real[], real, real, real);
template CircularEngine GEOGRAPHICLIB_EXPORT
SphericalEngine::Circle<true, SphericalEngine::SCHMIDT, 2>
(const coeff[], const real[], real, real, real);
template CircularEngine GEOGRAPHICLIB_EXPORT
SphericalEngine::Circle<false, SphericalEngine::SCHMIDT, 2>
(const coeff[], const real[], real, real, real);
template CircularEngine GEOGRAPHICLIB_EXPORT
SphericalEngine::Circle<true, SphericalEngine::FULL, 3>
(const coeff[], const real[], real, real, real);
template CircularEngine GEOGRAPHICLIB_EXPORT
SphericalEngine::Circle<false, SphericalEngine::FULL, 3>
(const coeff[], const real[], real, real, real);
template CircularEngine GEOGRAPHICLIB_EXPORT
SphericalEngine::Circle<true, SphericalEngine::SCHMIDT, 3>
(const coeff[], const real[], real, real, real);
template CircularEngine GEOGRAPHICLIB_EXPORT
SphericalEngine::Circle<false, SphericalEngine::SCHMIDT, 3>
(const coeff[], const real[], real, real, real);
/// \endcond
} // namespace GeographicLib

599
libs/geographiclib/src/TransverseMercator.cpp Normal file
View File

@@ -0,0 +1,599 @@
/**
* \file TransverseMercator.cpp
* \brief Implementation for GeographicLib::TransverseMercator class
*
* Copyright (c) Charles Karney (2008-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
*
* This implementation follows closely JHS 154, ETRS89 -
* j&auml;rjestelm&auml;&auml;n liittyv&auml;t karttaprojektiot,
* tasokoordinaatistot ja karttalehtijako</a> (Map projections, plane
* coordinates, and map sheet index for ETRS89), published by JUHTA, Finnish
* Geodetic Institute, and the National Land Survey of Finland (2006).
*
* The relevant section is available as the 2008 PDF file
* http://docs.jhs-suositukset.fi/jhs-suositukset/JHS154/JHS154_liite1.pdf
*
* This is a straight transcription of the formulas in this paper with the
* following exceptions:
* - use of 6th order series instead of 4th order series. This reduces the
* error to about 5nm for the UTM range of coordinates (instead of 200nm),
* with a speed penalty of only 1%;
* - use Newton's method instead of plain iteration to solve for latitude in
* terms of isometric latitude in the Reverse method;
* - use of Horner's representation for evaluating polynomials and Clenshaw's
* method for summing trigonometric series;
* - several modifications of the formulas to improve the numerical accuracy;
* - evaluating the convergence and scale using the expression for the
* projection or its inverse.
*
* If the preprocessor variable GEOGRAPHICLIB_TRANSVERSEMERCATOR_ORDER is set
* to an integer between 4 and 8, then this specifies the order of the series
* used for the forward and reverse transformations. The default value is 6.
* (The series accurate to 12th order is given in \ref tmseries.)
**********************************************************************/
#include <complex>
#include <GeographicLib/TransverseMercator.hpp>
#if defined(_MSC_VER)
// Squelch warnings about enum-float expressions
# pragma warning (disable: 5055)
#endif
namespace GeographicLib {
using namespace std;
TransverseMercator::TransverseMercator(real a, real f, real k0)
: _a(a)
, _f(f)
, _k0(k0)
, _e2(_f * (2 - _f))
, _es((_f < 0 ? -1 : 1) * sqrt(fabs(_e2)))
, _e2m(1 - _e2)
// _c = sqrt( pow(1 + _e, 1 + _e) * pow(1 - _e, 1 - _e) ) )
// See, for example, Lee (1976), p 100.
, _c( sqrt(_e2m) * exp(Math::eatanhe(real(1), _es)) )
, _n(_f / (2 - _f))
{
if (!(isfinite(_a) && _a > 0))
throw GeographicErr("Equatorial radius is not positive");
if (!(isfinite(_f) && _f < 1))
throw GeographicErr("Polar semi-axis is not positive");
if (!(isfinite(_k0) && _k0 > 0))
throw GeographicErr("Scale is not positive");
// Generated by Maxima on 2015-05-14 22:55:13-04:00
#if GEOGRAPHICLIB_TRANSVERSEMERCATOR_ORDER/2 == 2
static const real b1coeff[] = {
// b1*(n+1), polynomial in n2 of order 2
1, 16, 64, 64,
}; // count = 4
#elif GEOGRAPHICLIB_TRANSVERSEMERCATOR_ORDER/2 == 3
static const real b1coeff[] = {
// b1*(n+1), polynomial in n2 of order 3
1, 4, 64, 256, 256,
}; // count = 5
#elif GEOGRAPHICLIB_TRANSVERSEMERCATOR_ORDER/2 == 4
static const real b1coeff[] = {
// b1*(n+1), polynomial in n2 of order 4
25, 64, 256, 4096, 16384, 16384,
}; // count = 6
#else
#error "Bad value for GEOGRAPHICLIB_TRANSVERSEMERCATOR_ORDER"
#endif
#if GEOGRAPHICLIB_TRANSVERSEMERCATOR_ORDER == 4
static const real alpcoeff[] = {
// alp[1]/n^1, polynomial in n of order 3
164, 225, -480, 360, 720,
// alp[2]/n^2, polynomial in n of order 2
557, -864, 390, 1440,
// alp[3]/n^3, polynomial in n of order 1
-1236, 427, 1680,
// alp[4]/n^4, polynomial in n of order 0
49561, 161280,
}; // count = 14
#elif GEOGRAPHICLIB_TRANSVERSEMERCATOR_ORDER == 5
static const real alpcoeff[] = {
// alp[1]/n^1, polynomial in n of order 4
-635, 328, 450, -960, 720, 1440,
// alp[2]/n^2, polynomial in n of order 3
4496, 3899, -6048, 2730, 10080,
// alp[3]/n^3, polynomial in n of order 2
15061, -19776, 6832, 26880,
// alp[4]/n^4, polynomial in n of order 1
-171840, 49561, 161280,
// alp[5]/n^5, polynomial in n of order 0
34729, 80640,
}; // count = 20
#elif GEOGRAPHICLIB_TRANSVERSEMERCATOR_ORDER == 6
static const real alpcoeff[] = {
// alp[1]/n^1, polynomial in n of order 5
31564, -66675, 34440, 47250, -100800, 75600, 151200,
// alp[2]/n^2, polynomial in n of order 4
-1983433, 863232, 748608, -1161216, 524160, 1935360,
// alp[3]/n^3, polynomial in n of order 3
670412, 406647, -533952, 184464, 725760,
// alp[4]/n^4, polynomial in n of order 2
6601661, -7732800, 2230245, 7257600,
// alp[5]/n^5, polynomial in n of order 1
-13675556, 3438171, 7983360,
// alp[6]/n^6, polynomial in n of order 0
212378941, 319334400,
}; // count = 27
#elif GEOGRAPHICLIB_TRANSVERSEMERCATOR_ORDER == 7
static const real alpcoeff[] = {
// alp[1]/n^1, polynomial in n of order 6
1804025, 2020096, -4267200, 2204160, 3024000, -6451200, 4838400, 9676800,
// alp[2]/n^2, polynomial in n of order 5
4626384, -9917165, 4316160, 3743040, -5806080, 2620800, 9676800,
// alp[3]/n^3, polynomial in n of order 4
-67102379, 26816480, 16265880, -21358080, 7378560, 29030400,
// alp[4]/n^4, polynomial in n of order 3
155912000, 72618271, -85060800, 24532695, 79833600,
// alp[5]/n^5, polynomial in n of order 2
102508609, -109404448, 27505368, 63866880,
// alp[6]/n^6, polynomial in n of order 1
-12282192400LL, 2760926233LL, 4151347200LL,
// alp[7]/n^7, polynomial in n of order 0
1522256789, 1383782400,
}; // count = 35
#elif GEOGRAPHICLIB_TRANSVERSEMERCATOR_ORDER == 8
static const real alpcoeff[] = {
// alp[1]/n^1, polynomial in n of order 7
-75900428, 37884525, 42422016, -89611200, 46287360, 63504000, -135475200,
101606400, 203212800,
// alp[2]/n^2, polynomial in n of order 6
148003883, 83274912, -178508970, 77690880, 67374720, -104509440,
47174400, 174182400,
// alp[3]/n^3, polynomial in n of order 5
318729724, -738126169, 294981280, 178924680, -234938880, 81164160,
319334400,
// alp[4]/n^4, polynomial in n of order 4
-40176129013LL, 14967552000LL, 6971354016LL, -8165836800LL, 2355138720LL,
7664025600LL,
// alp[5]/n^5, polynomial in n of order 3
10421654396LL, 3997835751LL, -4266773472LL, 1072709352, 2490808320LL,
// alp[6]/n^6, polynomial in n of order 2
175214326799LL, -171950693600LL, 38652967262LL, 58118860800LL,
// alp[7]/n^7, polynomial in n of order 1
-67039739596LL, 13700311101LL, 12454041600LL,
// alp[8]/n^8, polynomial in n of order 0
1424729850961LL, 743921418240LL,
}; // count = 44
#else
#error "Bad value for GEOGRAPHICLIB_TRANSVERSEMERCATOR_ORDER"
#endif
#if GEOGRAPHICLIB_TRANSVERSEMERCATOR_ORDER == 4
static const real betcoeff[] = {
// bet[1]/n^1, polynomial in n of order 3
-4, 555, -960, 720, 1440,
// bet[2]/n^2, polynomial in n of order 2
-437, 96, 30, 1440,
// bet[3]/n^3, polynomial in n of order 1
-148, 119, 3360,
// bet[4]/n^4, polynomial in n of order 0
4397, 161280,
}; // count = 14
#elif GEOGRAPHICLIB_TRANSVERSEMERCATOR_ORDER == 5
static const real betcoeff[] = {
// bet[1]/n^1, polynomial in n of order 4
-3645, -64, 8880, -15360, 11520, 23040,
// bet[2]/n^2, polynomial in n of order 3
4416, -3059, 672, 210, 10080,
// bet[3]/n^3, polynomial in n of order 2
-627, -592, 476, 13440,
// bet[4]/n^4, polynomial in n of order 1
-3520, 4397, 161280,
// bet[5]/n^5, polynomial in n of order 0
4583, 161280,
}; // count = 20
#elif GEOGRAPHICLIB_TRANSVERSEMERCATOR_ORDER == 6
static const real betcoeff[] = {
// bet[1]/n^1, polynomial in n of order 5
384796, -382725, -6720, 932400, -1612800, 1209600, 2419200,
// bet[2]/n^2, polynomial in n of order 4
-1118711, 1695744, -1174656, 258048, 80640, 3870720,
// bet[3]/n^3, polynomial in n of order 3
22276, -16929, -15984, 12852, 362880,
// bet[4]/n^4, polynomial in n of order 2
-830251, -158400, 197865, 7257600,
// bet[5]/n^5, polynomial in n of order 1
-435388, 453717, 15966720,
// bet[6]/n^6, polynomial in n of order 0
20648693, 638668800,
}; // count = 27
#elif GEOGRAPHICLIB_TRANSVERSEMERCATOR_ORDER == 7
static const real betcoeff[] = {
// bet[1]/n^1, polynomial in n of order 6
-5406467, 6156736, -6123600, -107520, 14918400, -25804800, 19353600,
38707200,
// bet[2]/n^2, polynomial in n of order 5
829456, -5593555, 8478720, -5873280, 1290240, 403200, 19353600,
// bet[3]/n^3, polynomial in n of order 4
9261899, 3564160, -2708640, -2557440, 2056320, 58060800,
// bet[4]/n^4, polynomial in n of order 3
14928352, -9132761, -1742400, 2176515, 79833600,
// bet[5]/n^5, polynomial in n of order 2
-8005831, -1741552, 1814868, 63866880,
// bet[6]/n^6, polynomial in n of order 1
-261810608, 268433009, 8302694400LL,
// bet[7]/n^7, polynomial in n of order 0
219941297, 5535129600LL,
}; // count = 35
#elif GEOGRAPHICLIB_TRANSVERSEMERCATOR_ORDER == 8
static const real betcoeff[] = {
// bet[1]/n^1, polynomial in n of order 7
31777436, -37845269, 43097152, -42865200, -752640, 104428800, -180633600,
135475200, 270950400,
// bet[2]/n^2, polynomial in n of order 6
24749483, 14930208, -100683990, 152616960, -105719040, 23224320, 7257600,
348364800,
// bet[3]/n^3, polynomial in n of order 5
-232468668, 101880889, 39205760, -29795040, -28131840, 22619520,
638668800,
// bet[4]/n^4, polynomial in n of order 4
324154477, 1433121792, -876745056, -167270400, 208945440, 7664025600LL,
// bet[5]/n^5, polynomial in n of order 3
457888660, -312227409, -67920528, 70779852, 2490808320LL,
// bet[6]/n^6, polynomial in n of order 2
-19841813847LL, -3665348512LL, 3758062126LL, 116237721600LL,
// bet[7]/n^7, polynomial in n of order 1
-1989295244, 1979471673, 49816166400LL,
// bet[8]/n^8, polynomial in n of order 0
191773887257LL, 3719607091200LL,
}; // count = 44
#else
#error "Bad value for GEOGRAPHICLIB_TRANSVERSEMERCATOR_ORDER"
#endif
static_assert(sizeof(b1coeff) / sizeof(real) == maxpow_/2 + 2,
"Coefficient array size mismatch for b1");
static_assert(sizeof(alpcoeff) / sizeof(real) ==
(maxpow_ * (maxpow_ + 3))/2,
"Coefficient array size mismatch for alp");
static_assert(sizeof(betcoeff) / sizeof(real) ==
(maxpow_ * (maxpow_ + 3))/2,
"Coefficient array size mismatch for bet");
int m = maxpow_/2;
_b1 = Math::polyval(m, b1coeff, Math::sq(_n)) / (b1coeff[m + 1] * (1+_n));
// _a1 is the equivalent radius for computing the circumference of
// ellipse.
_a1 = _b1 * _a;
int o = 0;
real d = _n;
for (int l = 1; l <= maxpow_; ++l) {
m = maxpow_ - l;
_alp[l] = d * Math::polyval(m, alpcoeff + o, _n) / alpcoeff[o + m + 1];
_bet[l] = d * Math::polyval(m, betcoeff + o, _n) / betcoeff[o + m + 1];
o += m + 2;
d *= _n;
}
// Post condition: o == sizeof(alpcoeff) / sizeof(real) &&
// o == sizeof(betcoeff) / sizeof(real)
}
const TransverseMercator& TransverseMercator::UTM() {
static const TransverseMercator utm(Constants::WGS84_a(),
Constants::WGS84_f(),
Constants::UTM_k0());
return utm;
}
// Engsager and Poder (2007) use trigonometric series to convert between phi
// and phip. Here are the series...
//
// Conversion from phi to phip:
//
// phip = phi + sum(c[j] * sin(2*j*phi), j, 1, 6)
//
// c[1] = - 2 * n
// + 2/3 * n^2
// + 4/3 * n^3
// - 82/45 * n^4
// + 32/45 * n^5
// + 4642/4725 * n^6;
// c[2] = 5/3 * n^2
// - 16/15 * n^3
// - 13/9 * n^4
// + 904/315 * n^5
// - 1522/945 * n^6;
// c[3] = - 26/15 * n^3
// + 34/21 * n^4
// + 8/5 * n^5
// - 12686/2835 * n^6;
// c[4] = 1237/630 * n^4
// - 12/5 * n^5
// - 24832/14175 * n^6;
// c[5] = - 734/315 * n^5
// + 109598/31185 * n^6;
// c[6] = 444337/155925 * n^6;
//
// Conversion from phip to phi:
//
// phi = phip + sum(d[j] * sin(2*j*phip), j, 1, 6)
//
// d[1] = 2 * n
// - 2/3 * n^2
// - 2 * n^3
// + 116/45 * n^4
// + 26/45 * n^5
// - 2854/675 * n^6;
// d[2] = 7/3 * n^2
// - 8/5 * n^3
// - 227/45 * n^4
// + 2704/315 * n^5
// + 2323/945 * n^6;
// d[3] = 56/15 * n^3
// - 136/35 * n^4
// - 1262/105 * n^5
// + 73814/2835 * n^6;
// d[4] = 4279/630 * n^4
// - 332/35 * n^5
// - 399572/14175 * n^6;
// d[5] = 4174/315 * n^5
// - 144838/6237 * n^6;
// d[6] = 601676/22275 * n^6;
//
// In order to maintain sufficient relative accuracy close to the pole use
//
// S = sum(c[i]*sin(2*i*phi),i,1,6)
// taup = (tau + tan(S)) / (1 - tau * tan(S))
// In Math::taupf and Math::tauf we evaluate the forward transform explicitly
// and solve the reverse one by Newton's method.
//
// There are adapted from TransverseMercatorExact (taup and taupinv). tau =
// tan(phi), taup = sinh(psi)
void TransverseMercator::Forward(real lon0, real lat, real lon,
real& x, real& y,
real& gamma, real& k) const {
lat = Math::LatFix(lat);
lon = Math::AngDiff(lon0, lon);
// Explicitly enforce the parity
int
latsign = signbit(lat) ? -1 : 1,
lonsign = signbit(lon) ? -1 : 1;
lon *= lonsign;
lat *= latsign;
bool backside = lon > Math::qd;
if (backside) {
if (lat == 0)
latsign = -1;
lon = Math::hd - lon;
}
real sphi, cphi, slam, clam;
Math::sincosd(lat, sphi, cphi);
Math::sincosd(lon, slam, clam);
// phi = latitude
// phi' = conformal latitude
// psi = isometric latitude
// tau = tan(phi)
// tau' = tan(phi')
// [xi', eta'] = Gauss-Schreiber TM coordinates
// [xi, eta] = Gauss-Krueger TM coordinates
//
// We use
// tan(phi') = sinh(psi)
// sin(phi') = tanh(psi)
// cos(phi') = sech(psi)
// denom^2 = 1-cos(phi')^2*sin(lam)^2 = 1-sech(psi)^2*sin(lam)^2
// sin(xip) = sin(phi')/denom = tanh(psi)/denom
// cos(xip) = cos(phi')*cos(lam)/denom = sech(psi)*cos(lam)/denom
// cosh(etap) = 1/denom = 1/denom
// sinh(etap) = cos(phi')*sin(lam)/denom = sech(psi)*sin(lam)/denom
real etap, xip;
if (lat != Math::qd) {
real
tau = sphi / cphi,
taup = Math::taupf(tau, _es);
xip = atan2(taup, clam);
// Used to be
// etap = Math::atanh(sin(lam) / cosh(psi));
etap = asinh(slam / hypot(taup, clam));
// convergence and scale for Gauss-Schreiber TM (xip, etap) -- gamma0 =
// atan(tan(xip) * tanh(etap)) = atan(tan(lam) * sin(phi'));
// sin(phi') = tau'/sqrt(1 + tau'^2)
// Krueger p 22 (44)
gamma = Math::atan2d(slam * taup, clam * hypot(real(1), taup));
// k0 = sqrt(1 - _e2 * sin(phi)^2) * (cos(phi') / cos(phi)) * cosh(etap)
// Note 1/cos(phi) = cosh(psip);
// and cos(phi') * cosh(etap) = 1/hypot(sinh(psi), cos(lam))
//
// This form has cancelling errors. This property is lost if cosh(psip)
// is replaced by 1/cos(phi), even though it's using "primary" data (phi
// instead of psip).
k = sqrt(_e2m + _e2 * Math::sq(cphi)) * hypot(real(1), tau)
/ hypot(taup, clam);
} else {
xip = Math::pi()/2;
etap = 0;
gamma = lon;
k = _c;
}
// {xi',eta'} is {northing,easting} for Gauss-Schreiber transverse Mercator
// (for eta' = 0, xi' = bet). {xi,eta} is {northing,easting} for transverse
// Mercator with constant scale on the central meridian (for eta = 0, xip =
// rectifying latitude). Define
//
// zeta = xi + i*eta
// zeta' = xi' + i*eta'
//
// The conversion from conformal to rectifying latitude can be expressed as
// a series in _n:
//
// zeta = zeta' + sum(h[j-1]' * sin(2 * j * zeta'), j = 1..maxpow_)
//
// where h[j]' = O(_n^j). The reversion of this series gives
//
// zeta' = zeta - sum(h[j-1] * sin(2 * j * zeta), j = 1..maxpow_)
//
// which is used in Reverse.
//
// Evaluate sums via Clenshaw method. See
// https://en.wikipedia.org/wiki/Clenshaw_algorithm
//
// Let
//
// S = sum(a[k] * phi[k](x), k = 0..n)
// phi[k+1](x) = alpha[k](x) * phi[k](x) + beta[k](x) * phi[k-1](x)
//
// Evaluate S with
//
// b[n+2] = b[n+1] = 0
// b[k] = alpha[k](x) * b[k+1] + beta[k+1](x) * b[k+2] + a[k]
// S = (a[0] + beta[1](x) * b[2]) * phi[0](x) + b[1] * phi[1](x)
//
// Here we have
//
// x = 2 * zeta'
// phi[k](x) = sin(k * x)
// alpha[k](x) = 2 * cos(x)
// beta[k](x) = -1
// [ sin(A+B) - 2*cos(B)*sin(A) + sin(A-B) = 0, A = k*x, B = x ]
// n = maxpow_
// a[k] = _alp[k]
// S = b[1] * sin(x)
//
// For the derivative we have
//
// x = 2 * zeta'
// phi[k](x) = cos(k * x)
// alpha[k](x) = 2 * cos(x)
// beta[k](x) = -1
// [ cos(A+B) - 2*cos(B)*cos(A) + cos(A-B) = 0, A = k*x, B = x ]
// a[0] = 1; a[k] = 2*k*_alp[k]
// S = (a[0] - b[2]) + b[1] * cos(x)
//
// Matrix formulation (not used here):
// phi[k](x) = [sin(k * x); k * cos(k * x)]
// alpha[k](x) = 2 * [cos(x), 0; -sin(x), cos(x)]
// beta[k](x) = -1 * [1, 0; 0, 1]
// a[k] = _alp[k] * [1, 0; 0, 1]
// b[n+2] = b[n+1] = [0, 0; 0, 0]
// b[k] = alpha[k](x) * b[k+1] + beta[k+1](x) * b[k+2] + a[k]
// N.B., for all k: b[k](1,2) = 0; b[k](1,1) = b[k](2,2)
// S = (a[0] + beta[1](x) * b[2]) * phi[0](x) + b[1] * phi[1](x)
// phi[0](x) = [0; 0]
// phi[1](x) = [sin(x); cos(x)]
real
c0 = cos(2 * xip), ch0 = cosh(2 * etap),
s0 = sin(2 * xip), sh0 = sinh(2 * etap);
complex<real> a(2 * c0 * ch0, -2 * s0 * sh0); // 2 * cos(2*zeta')
int n = maxpow_;
complex<real>
y0(n & 1 ? _alp[n] : 0), y1, // default initializer is 0+i0
z0(n & 1 ? 2*n * _alp[n] : 0), z1;
if (n & 1) --n;
while (n) {
y1 = a * y0 - y1 + _alp[n];
z1 = a * z0 - z1 + 2*n * _alp[n];
--n;
y0 = a * y1 - y0 + _alp[n];
z0 = a * z1 - z0 + 2*n * _alp[n];
--n;
}
a /= real(2); // cos(2*zeta')
z1 = real(1) - z1 + a * z0;
a = complex<real>(s0 * ch0, c0 * sh0); // sin(2*zeta')
y1 = complex<real>(xip, etap) + a * y0;
// Fold in change in convergence and scale for Gauss-Schreiber TM to
// Gauss-Krueger TM.
gamma -= Math::atan2d(z1.imag(), z1.real());
k *= _b1 * abs(z1);
real xi = y1.real(), eta = y1.imag();
y = _a1 * _k0 * (backside ? Math::pi() - xi : xi) * latsign;
x = _a1 * _k0 * eta * lonsign;
if (backside)
gamma = Math::hd - gamma;
gamma *= latsign * lonsign;
gamma = Math::AngNormalize(gamma);
k *= _k0;
}
void TransverseMercator::Reverse(real lon0, real x, real y,
real& lat, real& lon,
real& gamma, real& k) const {
// This undoes the steps in Forward. The wrinkles are: (1) Use of the
// reverted series to express zeta' in terms of zeta. (2) Newton's method
// to solve for phi in terms of tan(phi).
real
xi = y / (_a1 * _k0),
eta = x / (_a1 * _k0);
// Explicitly enforce the parity
int
xisign = signbit(xi) ? -1 : 1,
etasign = signbit(eta) ? -1 : 1;
xi *= xisign;
eta *= etasign;
bool backside = xi > Math::pi()/2;
if (backside)
xi = Math::pi() - xi;
real
c0 = cos(2 * xi), ch0 = cosh(2 * eta),
s0 = sin(2 * xi), sh0 = sinh(2 * eta);
complex<real> a(2 * c0 * ch0, -2 * s0 * sh0); // 2 * cos(2*zeta)
int n = maxpow_;
complex<real>
y0(n & 1 ? -_bet[n] : 0), y1, // default initializer is 0+i0
z0(n & 1 ? -2*n * _bet[n] : 0), z1;
if (n & 1) --n;
while (n) {
y1 = a * y0 - y1 - _bet[n];
z1 = a * z0 - z1 - 2*n * _bet[n];
--n;
y0 = a * y1 - y0 - _bet[n];
z0 = a * z1 - z0 - 2*n * _bet[n];
--n;
}
a /= real(2); // cos(2*zeta)
z1 = real(1) - z1 + a * z0;
a = complex<real>(s0 * ch0, c0 * sh0); // sin(2*zeta)
y1 = complex<real>(xi, eta) + a * y0;
// Convergence and scale for Gauss-Schreiber TM to Gauss-Krueger TM.
gamma = Math::atan2d(z1.imag(), z1.real());
k = _b1 / abs(z1);
// JHS 154 has
//
// phi' = asin(sin(xi') / cosh(eta')) (Krueger p 17 (25))
// lam = asin(tanh(eta') / cos(phi')
// psi = asinh(tan(phi'))
real
xip = y1.real(), etap = y1.imag(),
s = sinh(etap),
c = fmax(real(0), cos(xip)), // cos(pi/2) might be negative
r = hypot(s, c);
if (r != 0) {
lon = Math::atan2d(s, c); // Krueger p 17 (25)
// Use Newton's method to solve for tau
real
sxip = sin(xip),
tau = Math::tauf(sxip/r, _es);
gamma += Math::atan2d(sxip * tanh(etap), c); // Krueger p 19 (31)
lat = Math::atand(tau);
// Note cos(phi') * cosh(eta') = r
k *= sqrt(_e2m + _e2 / (1 + Math::sq(tau))) *
hypot(real(1), tau) * r;
} else {
lat = Math::qd;
lon = 0;
k *= _c;
}
lat *= xisign;
if (backside)
lon = Math::hd - lon;
lon *= etasign;
lon = Math::AngNormalize(lon + lon0);
if (backside)
gamma = Math::hd - gamma;
gamma *= xisign * etasign;
gamma = Math::AngNormalize(gamma);
k *= _k0;
}
} // namespace GeographicLib

464
libs/geographiclib/src/TransverseMercatorExact.cpp Normal file
View File

@@ -0,0 +1,464 @@
/**
* \file TransverseMercatorExact.cpp
* \brief Implementation for GeographicLib::TransverseMercatorExact class
*
* Copyright (c) Charles Karney (2008-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
*
* The relevant section of Lee's paper is part V, pp 67--101,
* <a href="https://doi.org/10.3138/X687-1574-4325-WM62">Conformal
* Projections Based On Jacobian Elliptic Functions</a>;
* <a href="https://archive.org/details/conformalproject0000leel/page/92">
* borrow from archive.org</a>.
*
* The method entails using the Thompson Transverse Mercator as an
* intermediate projection. The projections from the intermediate
* coordinates to [\e phi, \e lam] and [\e x, \e y] are given by elliptic
* functions. The inverse of these projections are found by Newton's method
* with a suitable starting guess.
*
* This implementation and notation closely follows Lee, with the following
* exceptions:
* <center><table>
* <tr><th>Lee <th>here <th>Description
* <tr><td>x/a <td>xi <td>Northing (unit Earth)
* <tr><td>y/a <td>eta <td>Easting (unit Earth)
* <tr><td>s/a <td>sigma <td>xi + i * eta
* <tr><td>y <td>x <td>Easting
* <tr><td>x <td>y <td>Northing
* <tr><td>k <td>e <td>eccentricity
* <tr><td>k^2 <td>mu <td>elliptic function parameter
* <tr><td>k'^2 <td>mv <td>elliptic function complementary parameter
* <tr><td>m <td>k <td>scale
* <tr><td>zeta <td>zeta <td>complex longitude = Mercator = chi in paper
* <tr><td>s <td>sigma <td>complex GK = zeta in paper
* </table></center>
*
* Minor alterations have been made in some of Lee's expressions in an
* attempt to control round-off. For example atanh(sin(phi)) is replaced by
* asinh(tan(phi)) which maintains accuracy near phi = pi/2. Such changes
* are noted in the code.
**********************************************************************/
#include <GeographicLib/TransverseMercatorExact.hpp>
#if defined(_MSC_VER)
// Squelch warnings about constant conditional and enum-float expressions
# pragma warning (disable: 4127 5055)
#endif
namespace GeographicLib {
using namespace std;
TransverseMercatorExact::TransverseMercatorExact(real a, real f, real k0,
bool extendp)
: tol_(numeric_limits<real>::epsilon())
, tol2_(real(0.1) * tol_)
, taytol_(pow(tol_, real(0.6)))
, _a(a)
, _f(f)
, _k0(k0)
, _mu(_f * (2 - _f)) // e^2
, _mv(1 - _mu) // 1 - e^2
, _e(sqrt(_mu))
, _extendp(extendp)
, _eEu(_mu)
, _eEv(_mv)
{
if (!(isfinite(_a) && _a > 0))
throw GeographicErr("Equatorial radius is not positive");
if (!(_f > 0))
throw GeographicErr("Flattening is not positive");
if (!(_f < 1))
throw GeographicErr("Polar semi-axis is not positive");
if (!(isfinite(_k0) && _k0 > 0))
throw GeographicErr("Scale is not positive");
}
const TransverseMercatorExact& TransverseMercatorExact::UTM() {
static const TransverseMercatorExact utm(Constants::WGS84_a(),
Constants::WGS84_f(),
Constants::UTM_k0());
return utm;
}
void TransverseMercatorExact::zeta(real /*u*/, real snu, real cnu, real dnu,
real /*v*/, real snv, real cnv, real dnv,
real& taup, real& lam) const {
// Lee 54.17 but write
// atanh(snu * dnv) = asinh(snu * dnv / sqrt(cnu^2 + _mv * snu^2 * snv^2))
// atanh(_e * snu / dnv) =
// asinh(_e * snu / sqrt(_mu * cnu^2 + _mv * cnv^2))
// Overflow value s.t. atan(overflow) = pi/2
static const real
overflow = 1 / Math::sq(numeric_limits<real>::epsilon());
real
d1 = sqrt(Math::sq(cnu) + _mv * Math::sq(snu * snv)),
d2 = sqrt(_mu * Math::sq(cnu) + _mv * Math::sq(cnv)),
t1 = (d1 != 0 ? snu * dnv / d1 : (signbit(snu) ? -overflow : overflow)),
t2 = (d2 != 0 ? sinh( _e * asinh(_e * snu / d2) ) :
(signbit(snu) ? -overflow : overflow));
// psi = asinh(t1) - asinh(t2)
// taup = sinh(psi)
taup = t1 * hypot(real(1), t2) - t2 * hypot(real(1), t1);
lam = (d1 != 0 && d2 != 0) ?
atan2(dnu * snv, cnu * cnv) - _e * atan2(_e * cnu * snv, dnu * cnv) :
0;
}
void TransverseMercatorExact::dwdzeta(real /*u*/,
real snu, real cnu, real dnu,
real /*v*/,
real snv, real cnv, real dnv,
real& du, real& dv) const {
// Lee 54.21 but write (1 - dnu^2 * snv^2) = (cnv^2 + _mu * snu^2 * snv^2)
// (see A+S 16.21.4)
real d = _mv * Math::sq(Math::sq(cnv) + _mu * Math::sq(snu * snv));
du = cnu * dnu * dnv * (Math::sq(cnv) - _mu * Math::sq(snu * snv)) / d;
dv = -snu * snv * cnv * (Math::sq(dnu * dnv) + _mu * Math::sq(cnu)) / d;
}
// Starting point for zetainv
bool TransverseMercatorExact::zetainv0(real psi, real lam,
real& u, real& v) const {
bool retval = false;
if (psi < -_e * Math::pi()/4 &&
lam > (1 - 2 * _e) * Math::pi()/2 &&
psi < lam - (1 - _e) * Math::pi()/2) {
// N.B. this branch is normally not taken because psi < 0 is converted
// psi > 0 by Forward.
//
// There's a log singularity at w = w0 = Eu.K() + i * Ev.K(),
// corresponding to the south pole, where we have, approximately
//
// psi = _e + i * pi/2 - _e * atanh(cos(i * (w - w0)/(1 + _mu/2)))
//
// Inverting this gives:
real
psix = 1 - psi / _e,
lamx = (Math::pi()/2 - lam) / _e;
u = asinh(sin(lamx) / hypot(cos(lamx), sinh(psix))) *
(1 + _mu/2);
v = atan2(cos(lamx), sinh(psix)) * (1 + _mu/2);
u = _eEu.K() - u;
v = _eEv.K() - v;
} else if (psi < _e * Math::pi()/2 &&
lam > (1 - 2 * _e) * Math::pi()/2) {
// At w = w0 = i * Ev.K(), we have
//
// zeta = zeta0 = i * (1 - _e) * pi/2
// zeta' = zeta'' = 0
//
// including the next term in the Taylor series gives:
//
// zeta = zeta0 - (_mv * _e) / 3 * (w - w0)^3
//
// When inverting this, we map arg(w - w0) = [-90, 0] to
// arg(zeta - zeta0) = [-90, 180]
real
dlam = lam - (1 - _e) * Math::pi()/2,
rad = hypot(psi, dlam),
// atan2(dlam-psi, psi+dlam) + 45d gives arg(zeta - zeta0) in range
// [-135, 225). Subtracting 180 (since multiplier is negative) makes
// range [-315, 45). Multiplying by 1/3 (for cube root) gives range
// [-105, 15). In particular the range [-90, 180] in zeta space maps
// to [-90, 0] in w space as required.
ang = atan2(dlam-psi, psi+dlam) - real(0.75) * Math::pi();
// Error using this guess is about 0.21 * (rad/e)^(5/3)
retval = rad < _e * taytol_;
rad = cbrt(3 / (_mv * _e) * rad);
ang /= 3;
u = rad * cos(ang);
v = rad * sin(ang) + _eEv.K();
} else {
// Use spherical TM, Lee 12.6 -- writing atanh(sin(lam) / cosh(psi)) =
// asinh(sin(lam) / hypot(cos(lam), sinh(psi))). This takes care of the
// log singularity at zeta = Eu.K() (corresponding to the north pole)
v = asinh(sin(lam) / hypot(cos(lam), sinh(psi)));
u = atan2(sinh(psi), cos(lam));
// But scale to put 90,0 on the right place
u *= _eEu.K() / (Math::pi()/2);
v *= _eEu.K() / (Math::pi()/2);
}
return retval;
}
// Invert zeta using Newton's method
void TransverseMercatorExact::zetainv(real taup, real lam,
real& u, real& v) const {
real
psi = asinh(taup),
scal = 1/hypot(real(1), taup);
if (zetainv0(psi, lam, u, v))
return;
real stol2 = tol2_ / Math::sq(fmax(psi, real(1)));
// min iterations = 2, max iterations = 6; mean = 4.0
for (int i = 0, trip = 0; i < numit_ || GEOGRAPHICLIB_PANIC; ++i) {
real snu, cnu, dnu, snv, cnv, dnv;
_eEu.sncndn(u, snu, cnu, dnu);
_eEv.sncndn(v, snv, cnv, dnv);
real tau1, lam1, du1, dv1;
zeta(u, snu, cnu, dnu, v, snv, cnv, dnv, tau1, lam1);
dwdzeta(u, snu, cnu, dnu, v, snv, cnv, dnv, du1, dv1);
tau1 -= taup;
lam1 -= lam;
tau1 *= scal;
real
delu = tau1 * du1 - lam1 * dv1,
delv = tau1 * dv1 + lam1 * du1;
u -= delu;
v -= delv;
if (trip)
break;
real delw2 = Math::sq(delu) + Math::sq(delv);
if (!(delw2 >= stol2))
++trip;
}
}
void TransverseMercatorExact::sigma(real /*u*/, real snu, real cnu, real dnu,
real v, real snv, real cnv, real dnv,
real& xi, real& eta) const {
// Lee 55.4 writing
// dnu^2 + dnv^2 - 1 = _mu * cnu^2 + _mv * cnv^2
real d = _mu * Math::sq(cnu) + _mv * Math::sq(cnv);
xi = _eEu.E(snu, cnu, dnu) - _mu * snu * cnu * dnu / d;
eta = v - _eEv.E(snv, cnv, dnv) + _mv * snv * cnv * dnv / d;
}
void TransverseMercatorExact::dwdsigma(real /*u*/,
real snu, real cnu, real dnu,
real /*v*/,
real snv, real cnv, real dnv,
real& du, real& dv) const {
// Reciprocal of 55.9: dw/ds = dn(w)^2/_mv, expanding complex dn(w) using
// A+S 16.21.4
real d = _mv * Math::sq(Math::sq(cnv) + _mu * Math::sq(snu * snv));
real
dnr = dnu * cnv * dnv,
dni = - _mu * snu * cnu * snv;
du = (Math::sq(dnr) - Math::sq(dni)) / d;
dv = 2 * dnr * dni / d;
}
// Starting point for sigmainv
bool TransverseMercatorExact::sigmainv0(real xi, real eta,
real& u, real& v) const {
bool retval = false;
if (eta > real(1.25) * _eEv.KE() ||
(xi < -real(0.25) * _eEu.E() && xi < eta - _eEv.KE())) {
// sigma as a simple pole at w = w0 = Eu.K() + i * Ev.K() and sigma is
// approximated by
//
// sigma = (Eu.E() + i * Ev.KE()) + 1/(w - w0)
real
x = xi - _eEu.E(),
y = eta - _eEv.KE(),
r2 = Math::sq(x) + Math::sq(y);
u = _eEu.K() + x/r2;
v = _eEv.K() - y/r2;
} else if ((eta > real(0.75) * _eEv.KE() && xi < real(0.25) * _eEu.E())
|| eta > _eEv.KE()) {
// At w = w0 = i * Ev.K(), we have
//
// sigma = sigma0 = i * Ev.KE()
// sigma' = sigma'' = 0
//
// including the next term in the Taylor series gives:
//
// sigma = sigma0 - _mv / 3 * (w - w0)^3
//
// When inverting this, we map arg(w - w0) = [-pi/2, -pi/6] to
// arg(sigma - sigma0) = [-pi/2, pi/2]
// mapping arg = [-pi/2, -pi/6] to [-pi/2, pi/2]
real
deta = eta - _eEv.KE(),
rad = hypot(xi, deta),
// Map the range [-90, 180] in sigma space to [-90, 0] in w space. See
// discussion in zetainv0 on the cut for ang.
ang = atan2(deta-xi, xi+deta) - real(0.75) * Math::pi();
// Error using this guess is about 0.068 * rad^(5/3)
retval = rad < 2 * taytol_;
rad = cbrt(3 / _mv * rad);
ang /= 3;
u = rad * cos(ang);
v = rad * sin(ang) + _eEv.K();
} else {
// Else use w = sigma * Eu.K/Eu.E (which is correct in the limit _e -> 0)
u = xi * _eEu.K()/_eEu.E();
v = eta * _eEu.K()/_eEu.E();
}
return retval;
}
// Invert sigma using Newton's method
void TransverseMercatorExact::sigmainv(real xi, real eta,
real& u, real& v) const {
if (sigmainv0(xi, eta, u, v))
return;
// min iterations = 2, max iterations = 7; mean = 3.9
for (int i = 0, trip = 0; i < numit_ || GEOGRAPHICLIB_PANIC; ++i) {
real snu, cnu, dnu, snv, cnv, dnv;
_eEu.sncndn(u, snu, cnu, dnu);
_eEv.sncndn(v, snv, cnv, dnv);
real xi1, eta1, du1, dv1;
sigma(u, snu, cnu, dnu, v, snv, cnv, dnv, xi1, eta1);
dwdsigma(u, snu, cnu, dnu, v, snv, cnv, dnv, du1, dv1);
xi1 -= xi;
eta1 -= eta;
real
delu = xi1 * du1 - eta1 * dv1,
delv = xi1 * dv1 + eta1 * du1;
u -= delu;
v -= delv;
if (trip)
break;
real delw2 = Math::sq(delu) + Math::sq(delv);
if (!(delw2 >= tol2_))
++trip;
}
}
void TransverseMercatorExact::Scale(real tau, real /*lam*/,
real snu, real cnu, real dnu,
real snv, real cnv, real dnv,
real& gamma, real& k) const {
real sec2 = 1 + Math::sq(tau); // sec(phi)^2
// Lee 55.12 -- negated for our sign convention. gamma gives the bearing
// (clockwise from true north) of grid north
gamma = atan2(_mv * snu * snv * cnv, cnu * dnu * dnv);
// Lee 55.13 with nu given by Lee 9.1 -- in sqrt change the numerator
// from
//
// (1 - snu^2 * dnv^2) to (_mv * snv^2 + cnu^2 * dnv^2)
//
// to maintain accuracy near phi = 90 and change the denomintor from
//
// (dnu^2 + dnv^2 - 1) to (_mu * cnu^2 + _mv * cnv^2)
//
// to maintain accuracy near phi = 0, lam = 90 * (1 - e). Similarly
// rewrite sqrt term in 9.1 as
//
// _mv + _mu * c^2 instead of 1 - _mu * sin(phi)^2
k = sqrt(_mv + _mu / sec2) * sqrt(sec2) *
sqrt( (_mv * Math::sq(snv) + Math::sq(cnu * dnv)) /
(_mu * Math::sq(cnu) + _mv * Math::sq(cnv)) );
}
void TransverseMercatorExact::Forward(real lon0, real lat, real lon,
real& x, real& y,
real& gamma, real& k) const {
lat = Math::LatFix(lat);
lon = Math::AngDiff(lon0, lon);
// Explicitly enforce the parity
int
latsign = (!_extendp && signbit(lat)) ? -1 : 1,
lonsign = (!_extendp && signbit(lon)) ? -1 : 1;
lon *= lonsign;
lat *= latsign;
bool backside = !_extendp && lon > Math::qd;
if (backside) {
if (lat == 0)
latsign = -1;
lon = Math::hd - lon;
}
real
lam = lon * Math::degree(),
tau = Math::tand(lat);
// u,v = coordinates for the Thompson TM, Lee 54
real u, v;
if (lat == Math::qd) {
u = _eEu.K();
v = 0;
} else if (lat == 0 && lon == Math::qd * (1 - _e)) {
u = 0;
v = _eEv.K();
} else
// tau = tan(phi), taup = sinh(psi)
zetainv(Math::taupf(tau, _e), lam, u, v);
real snu, cnu, dnu, snv, cnv, dnv;
_eEu.sncndn(u, snu, cnu, dnu);
_eEv.sncndn(v, snv, cnv, dnv);
real xi, eta;
sigma(u, snu, cnu, dnu, v, snv, cnv, dnv, xi, eta);
if (backside)
xi = 2 * _eEu.E() - xi;
y = xi * _a * _k0 * latsign;
x = eta * _a * _k0 * lonsign;
if (lat == Math::qd) {
gamma = lon;
k = 1;
} else {
// Recompute (tau, lam) from (u, v) to improve accuracy of Scale
zeta(u, snu, cnu, dnu, v, snv, cnv, dnv, tau, lam);
tau = Math::tauf(tau, _e);
Scale(tau, lam, snu, cnu, dnu, snv, cnv, dnv, gamma, k);
gamma /= Math::degree();
}
if (backside)
gamma = Math::hd - gamma;
gamma *= latsign * lonsign;
k *= _k0;
}
void TransverseMercatorExact::Reverse(real lon0, real x, real y,
real& lat, real& lon,
real& gamma, real& k) const {
// This undoes the steps in Forward.
real
xi = y / (_a * _k0),
eta = x / (_a * _k0);
// Explicitly enforce the parity
int
xisign = (!_extendp && signbit(xi)) ? -1 : 1,
etasign = (!_extendp && signbit(eta)) ? -1 : 1;
xi *= xisign;
eta *= etasign;
bool backside = !_extendp && xi > _eEu.E();
if (backside)
xi = 2 * _eEu.E()- xi;
// u,v = coordinates for the Thompson TM, Lee 54
real u, v;
if (xi == 0 && eta == _eEv.KE()) {
u = 0;
v = _eEv.K();
} else
sigmainv(xi, eta, u, v);
real snu, cnu, dnu, snv, cnv, dnv;
_eEu.sncndn(u, snu, cnu, dnu);
_eEv.sncndn(v, snv, cnv, dnv);
real phi, lam, tau;
if (v != 0 || u != _eEu.K()) {
zeta(u, snu, cnu, dnu, v, snv, cnv, dnv, tau, lam);
tau = Math::tauf(tau, _e);
phi = atan(tau);
lat = phi / Math::degree();
lon = lam / Math::degree();
Scale(tau, lam, snu, cnu, dnu, snv, cnv, dnv, gamma, k);
gamma /= Math::degree();
} else {
lat = Math::qd;
lon = lam = gamma = 0;
k = 1;
}
if (backside)
lon = Math::hd - lon;
lon *= etasign;
lon = Math::AngNormalize(lon + Math::AngNormalize(lon0));
lat *= xisign;
if (backside)
gamma = Math::hd - gamma;
gamma *= xisign * etasign;
k *= _k0;
}
} // namespace GeographicLib

304
libs/geographiclib/src/UTMUPS.cpp Normal file
View File

@@ -0,0 +1,304 @@
/**
* \file UTMUPS.cpp
* \brief Implementation for GeographicLib::UTMUPS class
*
* Copyright (c) Charles Karney (2008-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <GeographicLib/UTMUPS.hpp>
#include <GeographicLib/MGRS.hpp>
#include <GeographicLib/PolarStereographic.hpp>
#include <GeographicLib/TransverseMercator.hpp>
#include <GeographicLib/Utility.hpp>
#if defined(_MSC_VER)
// Squelch warnings about enum-float expressions
# pragma warning (disable: 5055)
#endif
namespace GeographicLib {
using namespace std;
const int UTMUPS::falseeasting_[] =
{ MGRS::upseasting_ * MGRS::tile_, MGRS::upseasting_ * MGRS::tile_,
MGRS::utmeasting_ * MGRS::tile_, MGRS::utmeasting_ * MGRS::tile_ };
const int UTMUPS::falsenorthing_[] =
{ MGRS::upseasting_ * MGRS::tile_, MGRS::upseasting_ * MGRS::tile_,
MGRS::maxutmSrow_ * MGRS::tile_, MGRS::minutmNrow_ * MGRS::tile_ };
const int UTMUPS::mineasting_[] =
{ MGRS::minupsSind_ * MGRS::tile_, MGRS::minupsNind_ * MGRS::tile_,
MGRS::minutmcol_ * MGRS::tile_, MGRS::minutmcol_ * MGRS::tile_ };
const int UTMUPS::maxeasting_[] =
{ MGRS::maxupsSind_ * MGRS::tile_, MGRS::maxupsNind_ * MGRS::tile_,
MGRS::maxutmcol_ * MGRS::tile_, MGRS::maxutmcol_ * MGRS::tile_ };
const int UTMUPS::minnorthing_[] =
{ MGRS::minupsSind_ * MGRS::tile_, MGRS::minupsNind_ * MGRS::tile_,
MGRS::minutmSrow_ * MGRS::tile_,
(MGRS::minutmNrow_ + MGRS::minutmSrow_ - MGRS::maxutmSrow_)
* MGRS::tile_ };
const int UTMUPS::maxnorthing_[] =
{ MGRS::maxupsSind_ * MGRS::tile_, MGRS::maxupsNind_ * MGRS::tile_,
(MGRS::maxutmSrow_ + MGRS::maxutmNrow_ - MGRS::minutmNrow_) *
MGRS::tile_,
MGRS::maxutmNrow_ * MGRS::tile_ };
int UTMUPS::StandardZone(real lat, real lon, int setzone) {
using std::isnan; // Needed for Centos 7, ubuntu 14
if (!(setzone >= MINPSEUDOZONE && setzone <= MAXZONE))
throw GeographicErr("Illegal zone requested " + Utility::str(setzone));
if (setzone >= MINZONE || setzone == INVALID)
return setzone;
if (isnan(lat) || isnan(lon)) // Check if lat or lon is a NaN
return INVALID;
if (setzone == UTM || (lat >= -80 && lat < 84)) {
int ilon = int(floor(Math::AngNormalize(lon)));
if (ilon == Math::hd) ilon = -Math::hd; // ilon now in [-180,180)
int zone = (ilon + 186)/6;
int band = MGRS::LatitudeBand(lat);
if (band == 7 && zone == 31 && ilon >= 3) // The Norway exception
zone = 32;
else if (band == 9 && ilon >= 0 && ilon < 42) // The Svalbard exception
zone = 2 * ((ilon + 183)/12) + 1;
return zone;
} else
return UPS;
}
void UTMUPS::Forward(real lat, real lon,
int& zone, bool& northp, real& x, real& y,
real& gamma, real& k,
int setzone, bool mgrslimits) {
if (fabs(lat) > Math::qd)
throw GeographicErr("Latitude " + Utility::str(lat)
+ "d not in [-" + to_string(Math::qd)
+ "d, " + to_string(Math::qd) + "d]");
bool northp1 = !(signbit(lat));
int zone1 = StandardZone(lat, lon, setzone);
if (zone1 == INVALID) {
zone = zone1;
northp = northp1;
x = y = gamma = k = Math::NaN();
return;
}
real x1, y1, gamma1, k1;
bool utmp = zone1 != UPS;
if (utmp) {
real
lon0 = CentralMeridian(zone1),
dlon = Math::AngDiff(lon0, lon);
if (!(dlon <= 60))
// Check isn't really necessary because CheckCoords catches this case.
// But this allows a more meaningful error message to be given.
throw GeographicErr("Longitude " + Utility::str(lon)
+ "d more than 60d from center of UTM zone "
+ Utility::str(zone1));
TransverseMercator::UTM().Forward(lon0, lat, lon, x1, y1, gamma1, k1);
} else {
if (fabs(lat) < 70)
// Check isn't really necessary ... (see above).
throw GeographicErr("Latitude " + Utility::str(lat)
+ "d more than 20d from "
+ (northp1 ? "N" : "S") + " pole");
PolarStereographic::UPS().Forward(northp1, lat, lon, x1, y1, gamma1, k1);
}
int ind = (utmp ? 2 : 0) + (northp1 ? 1 : 0);
x1 += falseeasting_[ind];
y1 += falsenorthing_[ind];
if (! CheckCoords(zone1 != UPS, northp1, x1, y1, mgrslimits, false) )
throw GeographicErr("Latitude " + Utility::str(lat)
+ ", longitude " + Utility::str(lon)
+ " out of legal range for "
+ (utmp ? "UTM zone " + Utility::str(zone1) :
"UPS"));
zone = zone1;
northp = northp1;
x = x1;
y = y1;
gamma = gamma1;
k = k1;
}
void UTMUPS::Reverse(int zone, bool northp, real x, real y,
real& lat, real& lon, real& gamma, real& k,
bool mgrslimits) {
using std::isnan; // Needed for Centos 7, ubuntu 14
if (zone == INVALID || isnan(x) || isnan(y)) {
lat = lon = gamma = k = Math::NaN();
return;
}
if (!(zone >= MINZONE && zone <= MAXZONE))
throw GeographicErr("Zone " + Utility::str(zone)
+ " not in range [0, 60]");
bool utmp = zone != UPS;
CheckCoords(utmp, northp, x, y, mgrslimits);
int ind = (utmp ? 2 : 0) + (northp ? 1 : 0);
x -= falseeasting_[ind];
y -= falsenorthing_[ind];
if (utmp)
TransverseMercator::UTM().Reverse(CentralMeridian(zone),
x, y, lat, lon, gamma, k);
else
PolarStereographic::UPS().Reverse(northp, x, y, lat, lon, gamma, k);
}
bool UTMUPS::CheckCoords(bool utmp, bool northp, real x, real y,
bool mgrslimits, bool throwp) {
// Limits are all multiples of 100km and are all closed on the both ends.
// Failure tests are such that NaNs succeed.
real slop = mgrslimits ? 0 : MGRS::tile_;
int ind = (utmp ? 2 : 0) + (northp ? 1 : 0);
if (x < mineasting_[ind] - slop || x > maxeasting_[ind] + slop) {
if (!throwp) return false;
throw GeographicErr("Easting " + Utility::str(x/1000) + "km not in "
+ (mgrslimits ? "MGRS/" : "")
+ (utmp ? "UTM" : "UPS") + " range for "
+ (northp ? "N" : "S" ) + " hemisphere ["
+ Utility::str((mineasting_[ind] - slop)/1000)
+ "km, "
+ Utility::str((maxeasting_[ind] + slop)/1000)
+ "km]");
}
if (y < minnorthing_[ind] - slop || y > maxnorthing_[ind] + slop) {
if (!throwp) return false;
throw GeographicErr("Northing " + Utility::str(y/1000) + "km not in "
+ (mgrslimits ? "MGRS/" : "")
+ (utmp ? "UTM" : "UPS") + " range for "
+ (northp ? "N" : "S" ) + " hemisphere ["
+ Utility::str((minnorthing_[ind] - slop)/1000)
+ "km, "
+ Utility::str((maxnorthing_[ind] + slop)/1000)
+ "km]");
}
return true;
}
void UTMUPS::Transfer(int zonein, bool northpin, real xin, real yin,
int zoneout, bool northpout, real& xout, real& yout,
int& zone) {
bool northp = northpin;
if (zonein != zoneout) {
// Determine lat, lon
real lat, lon;
GeographicLib::UTMUPS::Reverse(zonein, northpin, xin, yin, lat, lon);
// Try converting to zoneout
real x, y;
int zone1;
GeographicLib::UTMUPS::Forward(lat, lon, zone1, northp, x, y,
zoneout == UTMUPS::MATCH
? zonein : zoneout);
if (zone1 == 0 && northp != northpout)
throw GeographicErr
("Attempt to transfer UPS coordinates between hemispheres");
zone = zone1;
xout = x;
yout = y;
} else {
if (zoneout == 0 && northp != northpout)
throw GeographicErr
("Attempt to transfer UPS coordinates between hemispheres");
zone = zoneout;
xout = xin;
yout = yin;
}
if (northp != northpout)
// Can't get here if UPS
yout += (northpout ? -1 : 1) * MGRS::utmNshift_;
return;
}
void UTMUPS::DecodeZone(const string& zonestr, int& zone, bool& northp)
{
unsigned zlen = unsigned(zonestr.size());
if (zlen == 0)
throw GeographicErr("Empty zone specification");
// Longest zone spec is 32north, 42south, invalid = 7
if (zlen > 7)
throw GeographicErr("More than 7 characters in zone specification "
+ zonestr);
const char* c = zonestr.c_str();
char* q;
int zone1 = strtol(c, &q, 10);
// if (zone1 == 0) zone1 = UPS; (not necessary)
if (zone1 == UPS) {
if (!(q == c))
// Don't allow 0n as an alternative to n for UPS coordinates
throw GeographicErr("Illegal zone 0 in " + zonestr +
", use just the hemisphere for UPS");
} else if (!(zone1 >= MINUTMZONE && zone1 <= MAXUTMZONE))
throw GeographicErr("Zone " + Utility::str(zone1)
+ " not in range [1, 60]");
else if (!isdigit(zonestr[0]))
throw GeographicErr("Must use unsigned number for zone "
+ Utility::str(zone1));
else if (q - c > 2)
throw GeographicErr("More than 2 digits use to specify zone "
+ Utility::str(zone1));
string hemi(zonestr, q - c);
for (string::iterator p = hemi.begin(); p != hemi.end(); ++p)
*p = char(tolower(*p));
if (q == c && (hemi == "inv" || hemi == "invalid")) {
zone = INVALID;
northp = false;
return;
}
bool northp1 = hemi == "north" || hemi == "n";
if (!(northp1 || hemi == "south" || hemi == "s"))
throw GeographicErr(string("Illegal hemisphere ") + hemi + " in "
+ zonestr + ", specify north or south");
zone = zone1;
northp = northp1;
}
string UTMUPS::EncodeZone(int zone, bool northp, bool abbrev) {
if (zone == INVALID)
return string(abbrev ? "inv" : "invalid");
if (!(zone >= MINZONE && zone <= MAXZONE))
throw GeographicErr("Zone " + Utility::str(zone)
+ " not in range [0, 60]");
ostringstream os;
if (zone != UPS)
os << setfill('0') << setw(2) << zone;
if (abbrev)
os << (northp ? 'n' : 's');
else
os << (northp ? "north" : "south");
return os.str();
}
void UTMUPS::DecodeEPSG(int epsg, int& zone, bool& northp) {
northp = false;
if (epsg >= epsg01N && epsg <= epsg60N) {
zone = (epsg - epsg01N) + MINUTMZONE;
northp = true;
} else if (epsg == epsgN) {
zone = UPS;
northp = true;
} else if (epsg >= epsg01S && epsg <= epsg60S) {
zone = (epsg - epsg01S) + MINUTMZONE;
} else if (epsg == epsgS) {
zone = UPS;
} else {
zone = INVALID;
}
}
int UTMUPS::EncodeEPSG(int zone, bool northp) {
int epsg = -1;
if (zone == UPS)
epsg = epsgS;
else if (zone >= MINUTMZONE && zone <= MAXUTMZONE)
epsg = (zone - MINUTMZONE) + epsg01S;
if (epsg >= 0 && northp)
epsg += epsgN - epsgS;
return epsg;
}
Math::real UTMUPS::UTMShift() { return real(MGRS::utmNshift_); }
} // namespace GeographicLib

197
libs/geographiclib/src/Utility.cpp Normal file
View File

@@ -0,0 +1,197 @@
/**
* \file Utility.cpp
* \brief Implementation for GeographicLib::Utility class
*
* Copyright (c) Charles Karney (2011-2022) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include <cstdlib>
#include <GeographicLib/Utility.hpp>
#if defined(_MSC_VER)
// Squelch warnings about unsafe use of getenv
# pragma warning (disable: 4996)
#endif
namespace GeographicLib {
using namespace std;
int Utility::day(int y, int m, int d) {
// Convert from date to sequential day and vice versa
//
// Here is some code to convert a date to sequential day and vice
// versa. The sequential day is numbered so that January 1, 1 AD is day 1
// (a Saturday). So this is offset from the "Julian" day which starts the
// numbering with 4713 BC.
//
// This is inspired by a talk by John Conway at the John von Neumann
// National Supercomputer Center when he described his Doomsday algorithm
// for figuring the day of the week. The code avoids explicitly doing ifs
// (except for the decision of whether to use the Julian or Gregorian
// calendar). Instead the equivalent result is achieved using integer
// arithmetic. I got this idea from the routine for the day of the week
// in MACLisp (I believe that that routine was written by Guy Steele).
//
// There are three issues to take care of
//
// 1. the rules for leap years,
// 2. the inconvenient placement of leap days at the end of February,
// 3. the irregular pattern of month lengths.
//
// We deal with these as follows:
//
// 1. Leap years are given by simple rules which are straightforward to
// accommodate.
//
// 2. We simplify the calculations by moving January and February to the
// previous year. Here we internally number the months March–December,
// January, February as 0–9, 10, 11.
//
// 3. The pattern of month lengths from March through January is regular
// with a 5-month period—31, 30, 31, 30, 31, 31, 30, 31, 30, 31, 31. The
// 5-month period is 153 days long. Since February is now at the end of
// the year, we don't need to include its length in this part of the
// calculation.
bool greg = gregorian(y, m, d);
y += (m + 9) / 12 - 1; // Move Jan and Feb to previous year,
m = (m + 9) % 12; // making March month 0.
return
(1461 * y) / 4 // Julian years converted to days. Julian year is 365 +
// 1/4 = 1461/4 days.
// Gregorian leap year corrections. The 2 offset with respect to the
// Julian calendar synchronizes the vernal equinox with that at the
// time of the Council of Nicea (325 AD).
+ (greg ? (y / 100) / 4 - (y / 100) + 2 : 0)
+ (153 * m + 2) / 5 // The zero-based start of the m'th month
+ d - 1 // The zero-based day
- 305; // The number of days between March 1 and December 31.
// This makes 0001-01-01 day 1
}
int Utility::day(int y, int m, int d, bool check) {
int s = day(y, m, d);
if (!check)
return s;
int y1, m1, d1;
date(s, y1, m1, d1);
if (!(s > 0 && y == y1 && m == m1 && d == d1))
throw GeographicErr("Invalid date " +
str(y) + "-" + str(m) + "-" + str(d)
+ (s > 0 ? "; use " +
str(y1) + "-" + str(m1) + "-" + str(d1) :
" before 0001-01-01"));
return s;
}
void Utility::date(int s, int& y, int& m, int& d) {
int c = 0;
bool greg = gregorian(s);
s += 305; // s = 0 on March 1, 1BC
if (greg) {
s -= 2; // The 2 day Gregorian offset
// Determine century with the Gregorian rules for leap years. The
// Gregorian year is 365 + 1/4 - 1/100 + 1/400 = 146097/400 days.
c = (4 * s + 3) / 146097;
s -= (c * 146097) / 4; // s = 0 at beginning of century
}
y = (4 * s + 3) / 1461; // Determine the year using Julian rules.
s -= (1461 * y) / 4; // s = 0 at start of year, i.e., March 1
y += c * 100; // Assemble full year
m = (5 * s + 2) / 153; // Determine the month
s -= (153 * m + 2) / 5; // s = 0 at beginning of month
d = s + 1; // Determine day of month
y += (m + 2) / 12; // Move Jan and Feb back to original year
m = (m + 2) % 12 + 1; // Renumber the months so January = 1
}
void Utility::date(const std::string& s, int& y, int& m, int& d) {
if (s == "now") {
time_t t = time(0);
struct tm* now = gmtime(&t);
y = now->tm_year + 1900;
m = now->tm_mon + 1;
d = now->tm_mday;
return;
}
int y1, m1 = 1, d1 = 1;
const char* digits = "0123456789";
string::size_type p1 = s.find_first_not_of(digits);
if (p1 == string::npos)
y1 = val<int>(s);
else if (s[p1] != '-')
throw GeographicErr("Delimiter not hyphen in date " + s);
else if (p1 == 0)
throw GeographicErr("Empty year field in date " + s);
else {
y1 = val<int>(s.substr(0, p1));
if (++p1 == s.size())
throw GeographicErr("Empty month field in date " + s);
string::size_type p2 = s.find_first_not_of(digits, p1);
if (p2 == string::npos)
m1 = val<int>(s.substr(p1));
else if (s[p2] != '-')
throw GeographicErr("Delimiter not hyphen in date " + s);
else if (p2 == p1)
throw GeographicErr("Empty month field in date " + s);
else {
m1 = val<int>(s.substr(p1, p2 - p1));
if (++p2 == s.size())
throw GeographicErr("Empty day field in date " + s);
d1 = val<int>(s.substr(p2));
}
}
y = y1; m = m1; d = d1;
}
std::string Utility::trim(const std::string& s) {
unsigned
beg = 0,
end = unsigned(s.size());
while (beg < end && isspace(s[beg]))
++beg;
while (beg < end && isspace(s[end - 1]))
--end;
return string(s, beg, end-beg);
}
int Utility::lookup(const std::string& s, char c) {
string::size_type r = s.find(char(toupper(c)));
return r == string::npos ? -1 : int(r);
}
int Utility::lookup(const char* s, char c) {
const char* p = strchr(s, toupper(c));
return p != NULL ? int(p - s) : -1;
}
bool Utility::ParseLine(const std::string& line,
std::string& key, std::string& value,
char equals, char comment) {
key.clear(); value.clear();
string::size_type n = comment ? line.find(comment) : line.size();
string linea = trim(line.substr(0, n));
if (linea.empty()) return false;
n = equals ? linea.find(equals) : linea.find_first_of(" \t\n\v\f\r");
key = trim(linea.substr(0, n));
if (key.empty()) return false;
if (n != string::npos) value = trim(linea.substr(n + 1));
return true;
}
int Utility::set_digits(int ndigits) {
#if GEOGRAPHICLIB_PRECISION == 5
if (ndigits <= 0) {
char* digitenv = getenv("GEOGRAPHICLIB_DIGITS");
if (digitenv)
ndigits = strtol(digitenv, NULL, 0);
if (ndigits <= 0)
ndigits = 256;
}
#endif
return Math::set_digits(ndigits);
}
} // namespace GeographicLib

381
libs/geographiclib/src/kissfft.hh Normal file
View File

@@ -0,0 +1,381 @@
/*
* Copyright (c) 2003-2010, Mark Borgerding. All rights reserved.
* This file is part of KISS FFT - https://github.com/mborgerding/kissfft
*
* SPDX-License-Identifier: BSD-3-Clause
* See COPYING file for more information.
*/
#ifndef KISSFFT_CLASS_HH
#define KISSFFT_CLASS_HH
#include <complex>
#include <utility>
#include <vector>
template <typename scalar_t>
class kissfft
{
public:
typedef std::complex<scalar_t> cpx_t;
kissfft( const std::size_t nfft,
const bool inverse )
:_nfft(nfft)
,_inverse(inverse)
{
using std::acos; using std::cos; using std::sin;
if (_nfft == 0) return;
// fill twiddle factors
_twiddles.resize(_nfft);
{
const scalar_t s = _inverse ? 1 : -1;
const scalar_t d = acos( (scalar_t) -1) / (2 * _nfft);
int i = 0, N = int(_nfft); // signed ints needed for subtractions
// enforce trigonometric symmetries by evaluating sin and cos
// with arguments in the range [-pi/4, pi/4]
for (; 8*i < N; ++i)
_twiddles[i] = cpx_t(+ cos((4*i )*d),
+s*sin((4*i )*d)); // pi/4*[0, 1)
for (; 8*i < 3*N; ++i)
_twiddles[i] = cpx_t(- sin((4*i- N)*d),
+s*cos((4*i- N)*d)); // pi/4*[1, 3)
for (; 8*i < 5*N; ++i)
_twiddles[i] = cpx_t(- cos((4*i-2*N)*d),
-s*sin((4*i-2*N)*d)); // pi/4*[3, 5)
for (; 8*i < 7*N; ++i)
_twiddles[i] = cpx_t(+ sin((4*i-3*N)*d),
-s*cos((4*i-3*N)*d)); // pi/4*[5, 7)
for (; i < N; ++i)
_twiddles[i] = cpx_t(+ cos((4*i-4*N)*d),
+s*sin((4*i-4*N)*d)); // pi/4*[5, 8)
}
//factorize
//start factoring out 4's, then 2's, then 3,5,7,9,...
std::size_t n= _nfft;
std::size_t p=4;
do {
while (n % p) {
switch (p) {
case 4: p = 2; break;
case 2: p = 3; break;
default: p += 2; break;
}
if (p*p>n)
p = n;// no more factors
}
n /= p;
_stageRadix.push_back(p);
_stageRemainder.push_back(n);
}while(n>1);
}
/// Changes the FFT-length and/or the transform direction.
///
/// @post The @c kissfft object will be in the same state as if it
/// had been newly constructed with the passed arguments.
/// However, the implementation may be faster than constructing a
/// new fft object.
void assign( const std::size_t nfft,
const bool inverse )
{
if ( nfft != _nfft )
{
kissfft tmp( nfft, inverse ); // O(n) time.
std::swap( tmp, *this ); // this is O(1) in C++11, O(n) otherwise.
}
else if ( inverse != _inverse )
{
// conjugate the twiddle factors.
for ( typename std::vector<cpx_t>::iterator it = _twiddles.begin();
it != _twiddles.end(); ++it )
it->imag( -it->imag() );
}
}
/// Calculates the complex Discrete Fourier Transform.
///
/// The size of the passed arrays must be passed in the constructor.
/// The sum of the squares of the absolute values in the @c dst
/// array will be @c N times the sum of the squares of the absolute
/// values in the @c src array, where @c N is the size of the array.
/// In other words, the l_2 norm of the resulting array will be
/// @c sqrt(N) times as big as the l_2 norm of the input array.
/// This is also the case when the inverse flag is set in the
/// constructor. Hence when applying the same transform twice, but with
/// the inverse flag changed the second time, then the result will
/// be equal to the original input times @c N.
void transform(const cpx_t * fft_in, cpx_t * fft_out, const std::size_t stage = 0, const std::size_t fstride = 1, const std::size_t in_stride = 1) const
{
if (_nfft == 0) return;
const std::size_t p = _stageRadix[stage];
const std::size_t m = _stageRemainder[stage];
cpx_t * const Fout_beg = fft_out;
cpx_t * const Fout_end = fft_out + p*m;
if (m==1) {
do{
*fft_out = *fft_in;
fft_in += fstride*in_stride;
}while(++fft_out != Fout_end );
}else{
do{
// recursive call:
// DFT of size m*p performed by doing
// p instances of smaller DFTs of size m,
// each one takes a decimated version of the input
transform(fft_in, fft_out, stage+1, fstride*p,in_stride);
fft_in += fstride*in_stride;
}while( (fft_out += m) != Fout_end );
}
fft_out=Fout_beg;
// recombine the p smaller DFTs
switch (p) {
case 2: kf_bfly2(fft_out,fstride,m); break;
case 3: kf_bfly3(fft_out,fstride,m); break;
case 4: kf_bfly4(fft_out,fstride,m); break;
case 5: kf_bfly5(fft_out,fstride,m); break;
default: kf_bfly_generic(fft_out,fstride,m,p); break;
}
}
/// Calculates the Discrete Fourier Transform (DFT) of a real input
/// of size @c 2*N.
///
/// The 0-th and N-th value of the DFT are real numbers. These are
/// stored in @c dst[0].real() and @c dst[0].imag() respectively.
/// The remaining DFT values up to the index N-1 are stored in
/// @c dst[1] to @c dst[N-1].
/// The other half of the DFT values can be calculated from the
/// symmetry relation
/// @code
/// DFT(src)[2*N-k] == conj( DFT(src)[k] );
/// @endcode
/// The same scaling factors as in @c transform() apply.
///
/// @note For this to work, the types @c scalar_t and @c cpx_t
/// must fulfill the following requirements:
///
/// For any object @c z of type @c cpx_t,
/// @c reinterpret_cast<scalar_t(&)[2]>(z)[0] is the real part of @c z and
/// @c reinterpret_cast<scalar_t(&)[2]>(z)[1] is the imaginary part of @c z.
/// For any pointer to an element of an array of @c cpx_t named @c p
/// and any valid array index @c i, @c reinterpret_cast<T*>(p)[2*i]
/// is the real part of the complex number @c p[i], and
/// @c reinterpret_cast<T*>(p)[2*i+1] is the imaginary part of the
/// complex number @c p[i].
///
/// Since C++11, these requirements are guaranteed to be satisfied for
/// @c scalar_ts being @c float, @c double or @c long @c double
/// together with @c cpx_t being @c std::complex<scalar_t>.
void transform_real( const scalar_t * const src,
cpx_t * const dst ) const
{
using std::acos; using std::exp;
const std::size_t N = _nfft;
if ( N == 0 )
return;
// perform complex FFT
transform( reinterpret_cast<const cpx_t*>(src), dst );
// post processing for k = 0 and k = N
dst[0] = cpx_t( dst[0].real() + dst[0].imag(),
dst[0].real() - dst[0].imag() );
// post processing for all the other k = 1, 2, ..., N-1
const scalar_t pi = acos( (scalar_t) -1);
const scalar_t half_phi_inc = ( _inverse ? pi : -pi ) / N;
const cpx_t twiddle_mul = exp( cpx_t(0, half_phi_inc) );
for ( std::size_t k = 1; 2*k < N; ++k )
{
const cpx_t w = (scalar_t)0.5 * cpx_t(
dst[k].real() + dst[N-k].real(),
dst[k].imag() - dst[N-k].imag() );
const cpx_t z = (scalar_t)0.5 * cpx_t(
dst[k].imag() + dst[N-k].imag(),
-dst[k].real() + dst[N-k].real() );
const cpx_t twiddle =
k % 2 == 0 ?
_twiddles[k/2] :
_twiddles[k/2] * twiddle_mul;
dst[ k] = w + twiddle * z;
dst[N-k] = std::conj( w - twiddle * z );
}
if ( N % 2 == 0 )
dst[N/2] = std::conj( dst[N/2] );
}
private:
void kf_bfly2( cpx_t * Fout, const size_t fstride, const std::size_t m) const
{
for (std::size_t k=0;k<m;++k) {
const cpx_t t = Fout[m+k] * _twiddles[k*fstride];
Fout[m+k] = Fout[k] - t;
Fout[k] += t;
}
}
void kf_bfly3( cpx_t * Fout, const std::size_t fstride, const std::size_t m) const
{
std::size_t k=m;
const std::size_t m2 = 2*m;
const cpx_t *tw1,*tw2;
cpx_t scratch[5];
const cpx_t epi3 = _twiddles[fstride*m];
tw1=tw2=&_twiddles[0];
do{
scratch[1] = Fout[m] * *tw1;
scratch[2] = Fout[m2] * *tw2;
scratch[3] = scratch[1] + scratch[2];
scratch[0] = scratch[1] - scratch[2];
tw1 += fstride;
tw2 += fstride*2;
Fout[m] = Fout[0] - scratch[3]*scalar_t(0.5);
scratch[0] *= epi3.imag();
Fout[0] += scratch[3];
Fout[m2] = cpx_t( Fout[m].real() + scratch[0].imag() , Fout[m].imag() - scratch[0].real() );
Fout[m] += cpx_t( -scratch[0].imag(),scratch[0].real() );
++Fout;
}while(--k);
}
void kf_bfly4( cpx_t * const Fout, const std::size_t fstride, const std::size_t m) const
{
cpx_t scratch[7];
const scalar_t negative_if_inverse = _inverse ? -1 : +1;
for (std::size_t k=0;k<m;++k) {
scratch[0] = Fout[k+ m] * _twiddles[k*fstride ];
scratch[1] = Fout[k+2*m] * _twiddles[k*fstride*2];
scratch[2] = Fout[k+3*m] * _twiddles[k*fstride*3];
scratch[5] = Fout[k] - scratch[1];
Fout[k] += scratch[1];
scratch[3] = scratch[0] + scratch[2];
scratch[4] = scratch[0] - scratch[2];
scratch[4] = cpx_t( scratch[4].imag()*negative_if_inverse ,
-scratch[4].real()*negative_if_inverse );
Fout[k+2*m] = Fout[k] - scratch[3];
Fout[k ]+= scratch[3];
Fout[k+ m] = scratch[5] + scratch[4];
Fout[k+3*m] = scratch[5] - scratch[4];
}
}
void kf_bfly5( cpx_t * const Fout, const std::size_t fstride, const std::size_t m) const
{
cpx_t *Fout0,*Fout1,*Fout2,*Fout3,*Fout4;
cpx_t scratch[13];
const cpx_t ya = _twiddles[fstride*m];
const cpx_t yb = _twiddles[fstride*2*m];
Fout0=Fout;
Fout1=Fout0+m;
Fout2=Fout0+2*m;
Fout3=Fout0+3*m;
Fout4=Fout0+4*m;
for ( std::size_t u=0; u<m; ++u ) {
scratch[0] = *Fout0;
scratch[1] = *Fout1 * _twiddles[ u*fstride];
scratch[2] = *Fout2 * _twiddles[2*u*fstride];
scratch[3] = *Fout3 * _twiddles[3*u*fstride];
scratch[4] = *Fout4 * _twiddles[4*u*fstride];
scratch[7] = scratch[1] + scratch[4];
scratch[10]= scratch[1] - scratch[4];
scratch[8] = scratch[2] + scratch[3];
scratch[9] = scratch[2] - scratch[3];
*Fout0 += scratch[7];
*Fout0 += scratch[8];
scratch[5] = scratch[0] + cpx_t(
scratch[7].real()*ya.real() + scratch[8].real()*yb.real(),
scratch[7].imag()*ya.real() + scratch[8].imag()*yb.real()
);
scratch[6] = cpx_t(
scratch[10].imag()*ya.imag() + scratch[9].imag()*yb.imag(),
-scratch[10].real()*ya.imag() - scratch[9].real()*yb.imag()
);
*Fout1 = scratch[5] - scratch[6];
*Fout4 = scratch[5] + scratch[6];
scratch[11] = scratch[0] +
cpx_t(
scratch[7].real()*yb.real() + scratch[8].real()*ya.real(),
scratch[7].imag()*yb.real() + scratch[8].imag()*ya.real()
);
scratch[12] = cpx_t(
-scratch[10].imag()*yb.imag() + scratch[9].imag()*ya.imag(),
scratch[10].real()*yb.imag() - scratch[9].real()*ya.imag()
);
*Fout2 = scratch[11] + scratch[12];
*Fout3 = scratch[11] - scratch[12];
++Fout0;
++Fout1;
++Fout2;
++Fout3;
++Fout4;
}
}
/* perform the butterfly for one stage of a mixed radix FFT */
void kf_bfly_generic(
cpx_t * const Fout,
const size_t fstride,
const std::size_t m,
const std::size_t p
) const
{
const cpx_t * twiddles = &_twiddles[0];
if(p > _scratchbuf.size()) _scratchbuf.resize(p);
for ( std::size_t u=0; u<m; ++u ) {
std::size_t k = u;
for ( std::size_t q1=0 ; q1<p ; ++q1 ) {
_scratchbuf[q1] = Fout[ k ];
k += m;
}
k=u;
for ( std::size_t q1=0 ; q1<p ; ++q1 ) {
std::size_t twidx=0;
Fout[ k ] = _scratchbuf[0];
for ( std::size_t q=1;q<p;++q ) {
twidx += fstride * k;
if (twidx>=_nfft)
twidx-=_nfft;
Fout[ k ] += _scratchbuf[q] * twiddles[twidx];
}
k += m;
}
}
}
std::size_t _nfft;
bool _inverse;
std::vector<cpx_t> _twiddles;
std::vector<std::size_t> _stageRadix;
std::vector<std::size_t> _stageRemainder;
mutable std::vector<cpx_t> _scratchbuf;
};
#endif