338 lines
12 KiB
C++
338 lines
12 KiB
C++
/**
|
|
* \file GeodShort.cpp
|
|
*
|
|
* Test various solutions to the inverse problem in the limit of short lines
|
|
* (following Bowring 1981).
|
|
**********************************************************************/
|
|
|
|
#include <ctime> // for std::time
|
|
#include <functional> // for std::function and std::bind
|
|
#include <random> // for C++11 random numbers
|
|
#include <limits> // for digits
|
|
#include <iostream>
|
|
#include <iomanip>
|
|
#include <algorithm>
|
|
#include <GeographicLib/Geodesic.hpp>
|
|
#include <GeographicLib/GeodesicExact.hpp>
|
|
#include <GeographicLib/Utility.hpp>
|
|
#include <GeographicLib/EllipticFunction.hpp>
|
|
|
|
using namespace GeographicLib;
|
|
using namespace std;
|
|
|
|
class GeodShort {
|
|
private:
|
|
typedef Math::real real;
|
|
real _a, _f, _f1, _b, _e2, _ep2, _e;
|
|
EllipticFunction _E;
|
|
inline real eatanhe(real x) const {
|
|
return _f >= 0 ? _e * atanh(_e * x) : - _e * atan(_e * x);
|
|
}
|
|
static inline real psi0f(real phi) { return asinh(tan(phi)); }
|
|
static inline real invpsi0f(real psi) { return atan(sinh(psi)); }
|
|
inline real psif(real phi) { return psi0f(phi) - eatanhe(sin(phi)); }
|
|
static inline void SinCosNorm(real& sinx, real& cosx) {
|
|
real r = hypot(sinx, cosx);
|
|
sinx /= r;
|
|
cosx /= r;
|
|
}
|
|
static real GreatCircle(real sbet1, real cbet1,
|
|
real sbet2, real cbet2,
|
|
real omg12, real& azi1, real& azi2) {
|
|
real
|
|
// bet12 = bet2 - bet1 in [0, pi)
|
|
sbet12 = sbet2 * cbet1 - cbet2 * sbet1,
|
|
sbet12a = sbet2 * cbet1 + cbet2 * sbet1,
|
|
somg12 = sin(omg12), comg12 = cos(omg12);
|
|
real
|
|
salp1 = cbet2 * somg12,
|
|
calp1 = (comg12 >= 0 ?
|
|
sbet12 + cbet2 * sbet1 * Math::sq(somg12) / (1 + comg12) :
|
|
sbet12a - cbet2 * sbet1 * Math::sq(somg12) / (1 - comg12));
|
|
real
|
|
ssig12 = hypot(salp1, calp1),
|
|
csig12 = sbet1 * sbet2 + cbet1 * cbet2 * comg12;
|
|
|
|
real
|
|
salp2 = cbet1 * somg12,
|
|
calp2 = sbet12 - cbet1 * sbet2 * Math::sq(somg12) / (1 + comg12),
|
|
sig12 = atan2(ssig12, csig12);
|
|
|
|
azi1 = atan2(salp1, calp1) / Math::degree();
|
|
azi2 = atan2(salp2, calp2) / Math::degree();
|
|
return sig12;
|
|
}
|
|
public:
|
|
GeodShort(real a, real f)
|
|
: _a(a)
|
|
, _f(f)
|
|
, _f1(1 - _f)
|
|
, _b(_a * _f1)
|
|
, _e2(_f * (2 - _f))
|
|
, _ep2(_e2/Math::sq(_f1))
|
|
, _e(sqrt(abs(_e2)))
|
|
, _E(-_ep2) {}
|
|
real Inverse(real lat1, real lon1, real lat2, real lon2,
|
|
real& azi1, real& azi2) {
|
|
int mode = 1;
|
|
real
|
|
phi1 = Math::degree() * lat1,
|
|
phi2 = Math::degree() * lat2,
|
|
lam12 = Math::degree() * Math::AngNormalize(lon2 - lon1),
|
|
sbet1 = _f1 * sin(phi1), cbet1 = cos(phi1),
|
|
sbet2 = _f1 * sin(phi2), cbet2 = cos(phi2);
|
|
SinCosNorm(sbet1, cbet1); SinCosNorm(sbet2, cbet2);
|
|
real
|
|
dn1 = sqrt(1 + _ep2 * Math::sq(sbet1)),
|
|
dn2 = sqrt(1 + _ep2 * Math::sq(sbet2)),
|
|
dnm;
|
|
if (mode == 0)
|
|
dnm = (dn1 + dn2) / 2;
|
|
else {
|
|
real sbetm2 = Math::sq(sbet1 + sbet2);
|
|
sbetm2 = sbetm2 / (sbetm2 + Math::sq(cbet1 + cbet2));
|
|
dnm = sqrt(1 + _ep2 * sbetm2);
|
|
}
|
|
return _b * dnm * GreatCircle(sbet1, cbet1, sbet2, cbet2,
|
|
lam12 / (_f1 * dnm),
|
|
azi1, azi2);
|
|
}
|
|
real Inverse2(real lat1, real lon1, real lat2, real lon2,
|
|
real& azi1, real& azi2) {
|
|
real
|
|
phi1 = Math::degree() * lat1,
|
|
phi2 = Math::degree() * lat2,
|
|
lam12 = Math::degree() * Math::AngNormalize(lon2 - lon1),
|
|
sbet1 = _f1 * sin(phi1), cbet1 = cos(phi1),
|
|
sbet2 = _f1 * sin(phi2), cbet2 = cos(phi2);
|
|
SinCosNorm(sbet1, cbet1); SinCosNorm(sbet2, cbet2);
|
|
real dnm;
|
|
real sbetm2 = Math::sq(sbet1 + sbet2);
|
|
sbetm2 = sbetm2 / (sbetm2 + Math::sq(cbet1 + cbet2));
|
|
dnm = sqrt(1 + _ep2 * sbetm2);
|
|
// Adjust bet1 and bet2 via conformal map
|
|
real
|
|
A = 1/(dnm * _f1),
|
|
phim = atan((sbet1+sbet2)/(_f1*(cbet1+cbet2))),
|
|
betm = atan((sbet1+sbet2)/(cbet1+cbet2)),
|
|
psim = psif(phim),
|
|
psipm = psi0f(betm),
|
|
K = psipm - A * psim,
|
|
bet1 = invpsi0f( A*psif(phi1) + K ),
|
|
bet2 = invpsi0f( A*psif(phi2) + K );
|
|
sbet1 = sin(bet1); cbet1 = cos(bet1);
|
|
sbet2 = sin(bet2); cbet2 = cos(bet2);
|
|
return _b * dnm * GreatCircle(sbet1, cbet1, sbet2, cbet2,
|
|
lam12 / (_f1 * dnm),
|
|
azi1, azi2);
|
|
}
|
|
real Bowring0(real lat1, real lon1, real lat2, real lon2,
|
|
real& azi1, real& azi2) {
|
|
int mode = 2;
|
|
real
|
|
phi1 = Math::degree() * lat1,
|
|
phi2 = Math::degree() * lat2,
|
|
lam12 = Math::degree() * Math::AngNormalize(lon2 - lon1),
|
|
m = 0.5,
|
|
phim = ( abs(phi1) >= abs(phi2) ?
|
|
(1 - m) * phi1 + m * phi2 :
|
|
(1 - m) * phi2 + m * phi1 ),
|
|
dphi1 = phi1 - phim,
|
|
dphi2 = phi2 - phim,
|
|
cosm = cos(phim),
|
|
sinm = sin(phim),
|
|
A = sqrt(1 + _ep2 * Math::sq(Math::sq(cosm))),
|
|
B = sqrt(1 + _ep2 * Math::sq(cosm)),
|
|
C = sqrt(1 + _ep2),
|
|
omg12 = A * lam12,
|
|
phipm = atan(tan(phim)/B),
|
|
R = _a * C/(B*B),
|
|
phip1, phip2;
|
|
if (mode == 0) {
|
|
phip1 = phipm + (dphi1/B) * (1 + (3*_ep2*dphi1)/(4*B*B) *
|
|
sin(2 * (phim + dphi1/3)));
|
|
phip2 = phipm + (dphi2/B) * (1 + (3*_ep2*dphi2)/(4*B*B) *
|
|
sin(2 * (phim + dphi2/3)));
|
|
} else if (mode == 1) {
|
|
real
|
|
psi1 = psif(phi1), psi2 = psif(phi2),
|
|
psim = psif(phim), psipm = psi0f(phipm);
|
|
phip1 = invpsi0f(A * (psi1-psim) + psipm);
|
|
phip2 = invpsi0f(A * (psi2-psim) + psipm);
|
|
} else {
|
|
phip1 = phipm + (dphi1/B) *
|
|
( 1 + _ep2*dphi1/(2*B*B) *
|
|
(3*cosm*sinm + dphi1*(1-2*sinm*sinm +
|
|
_ep2 * (cosm*cosm * (1 + 3*sinm*sinm)))/B*B));
|
|
phip2 = phipm + (dphi2/B) *
|
|
( 1 + _ep2*dphi2/(2*B*B) *
|
|
(3*cosm*sinm + dphi2*(1-2*sinm*sinm +
|
|
_ep2 * (cosm*cosm * (1 + 3*sinm*sinm)))/B*B));
|
|
}
|
|
return R * GreatCircle(sin(phip1), cos(phip1), sin(phip2), cos(phip2),
|
|
omg12, azi1, azi2);
|
|
}
|
|
real Bowring1(real lat1, real lon1, real lat2, real lon2,
|
|
real& azi1, real& azi2) {
|
|
real
|
|
phi1 = Math::degree() * lat1,
|
|
phi2 = Math::degree() * lat2,
|
|
lam12 = Math::degree() * Math::AngNormalize(lon2 - lon1),
|
|
bet1 = atan(_f1 * tan(phi1)),
|
|
bet2 = atan(_f1 * tan(phi2)),
|
|
betm = (bet1 + bet2)/2,
|
|
phim = atan(tan(betm) / _f1),
|
|
cosm = cos(phim),
|
|
A = sqrt(1 + _ep2 * Math::sq(Math::sq(cosm))),
|
|
B = sqrt(1 + _ep2 * Math::sq(cosm)),
|
|
C = sqrt(1 + _ep2),
|
|
omg12 = A * lam12,
|
|
phipm = atan(tan(phim)/B),
|
|
R = _a * C/(B*B),
|
|
m1 = _E.E(sin(bet1), cos(bet1), sqrt(1 + _ep2 * Math::sq(sin(bet1)))),
|
|
m2 = _E.E(sin(bet2), cos(bet2), sqrt(1 + _ep2 * Math::sq(sin(bet2)))),
|
|
mm = _E.E(sin(betm), cos(betm), sqrt(1 + _ep2 * Math::sq(sin(betm)))),
|
|
phip1 = phipm + (m1 - mm) * _a * _f1 / R,
|
|
phip2 = phipm + (m2 - mm) * _a * _f1 / R;
|
|
return R * GreatCircle(sin(phip1), cos(phip1), sin(phip2), cos(phip2),
|
|
omg12, azi1, azi2);
|
|
}
|
|
real Bowring2(real lat1, real lon1, real lat2, real lon2,
|
|
real& azi1, real& azi2) {
|
|
real highfact = 1;
|
|
real
|
|
phi1 = Math::degree() * lat1,
|
|
phi2 = Math::degree() * lat2,
|
|
lam12 = Math::degree() * Math::AngNormalize(lon2 - lon1),
|
|
bet1 = atan(_f1 * tan(phi1)),
|
|
bet2 = atan(_f1 * tan(phi2)),
|
|
betm = (bet1 + bet2)/2,
|
|
sbetm = sin(betm),
|
|
cbetm = cos(betm),
|
|
phim = atan(tan(betm) / _f1),
|
|
cosm = cos(phim),
|
|
A = sqrt(1 + _ep2 * Math::sq(Math::sq(cosm))),
|
|
B = sqrt(1 + _ep2 * Math::sq(cosm)),
|
|
C = sqrt(1 + _ep2),
|
|
omg12 = A * lam12,
|
|
phipm = atan(tan(phim)/B),
|
|
R = _a * C/(B*B),
|
|
dbet1 = bet1-betm,
|
|
dbet2 = bet2-betm,
|
|
dnm2 = 1+_ep2*Math::sq(sbetm),
|
|
dnm = sqrt(dnm2),
|
|
phip1 = phipm + dbet1/dnm *
|
|
(1 + dbet1 * _ep2/(2*dnm2) *
|
|
( cbetm*sbetm + highfact * dbet1 * ( Math::sq(cbetm) -
|
|
Math::sq(sbetm)*dnm2)/(3*dnm2) )),
|
|
phip2 = phipm + dbet2/dnm *
|
|
(1 + dbet2 * _ep2/(2*dnm2) *
|
|
( cbetm*sbetm + highfact * dbet2 * ( Math::sq(cbetm) -
|
|
Math::sq(sbetm)*dnm2)/(3*dnm2) ));
|
|
return R * GreatCircle(sin(phip1), cos(phip1), sin(phip2), cos(phip2),
|
|
omg12, azi1, azi2);
|
|
}
|
|
};
|
|
|
|
int main(int argc, char* argv[]) {
|
|
try {
|
|
// Args are f and sig
|
|
if (argc != 3) {
|
|
cerr << "Usage: GeodShort f sig\n";
|
|
return 1;
|
|
}
|
|
typedef Math::real real;
|
|
real
|
|
f = Utility::fract<real>(string(argv[1])),
|
|
sig = Utility::val<real>(string(argv[2]));
|
|
Geodesic g(1, f);
|
|
GeodesicExact ge(1, f);
|
|
GeodShort s(1, f);
|
|
bool exact = abs(f) > 0.02;
|
|
real norm, consist;
|
|
{
|
|
real m;
|
|
if (exact)
|
|
ge.Inverse(0, 0, 90, 0, m);
|
|
else
|
|
g.Inverse(0, 0, 90, 0, m);
|
|
norm = max(m, Math::pi()/2 * g.EquatorialRadius());
|
|
consist = min(m, Math::pi()/2 * g.EquatorialRadius()) /
|
|
(Math::pi()/2);
|
|
}
|
|
unsigned seed = random_device()(); // Set seed from random_device
|
|
mt19937 r(seed); // Initialize URNG
|
|
uniform_real_distribution<double> U;
|
|
cout << norm << " " << consist << " "
|
|
<< f << " " << sig << " " << seed << endl;
|
|
real maxerr1 = -1, maxerr2 = -1, maxerr3 = -1;
|
|
for (unsigned k = 0; k < 10000000; ++k) {
|
|
real
|
|
lat1 = 90*real(U(r)),
|
|
lon1 = 0,
|
|
azi1 = 180*real(U(r)),
|
|
lat2, lon2, s12, azi2;
|
|
if (exact)
|
|
ge.ArcDirect(lat1, lon1, azi1, sig, lat2, lon2, azi2, s12);
|
|
else
|
|
g.ArcDirect(lat1, lon1, azi1, sig, lat2, lon2, azi2, s12);
|
|
real
|
|
s12a = s.Bowring2(lat1, lon1, lat2, lon2, azi1, azi2),
|
|
err1 = abs(s12a - s12) / norm;
|
|
if (err1 > maxerr1) {
|
|
maxerr1 = err1;
|
|
cout << "A " << k << " "
|
|
<< lat1 << " " << azi1 << " " << maxerr1 << endl;
|
|
}
|
|
real lat, lon, azi1a, azi2a;
|
|
if (exact)
|
|
ge.Direct(lat1, lon1, azi1, s12a, lat, lon);
|
|
else
|
|
g.Direct(lat1, lon1, azi1, s12a, lat, lon);
|
|
real err2 = s.Inverse(lat2, lon2, lat, lon, azi1a, azi2a);
|
|
if (exact)
|
|
ge.Direct(lat2, lon2, azi2, -s12a, lat, lon);
|
|
else
|
|
g.Direct(lat2, lon2, azi2, -s12a, lat, lon);
|
|
err2 = max(err2, s.Inverse(lat1, lon1, lat, lon, azi1a, azi2a)) / norm;
|
|
if (err2 > maxerr2) {
|
|
maxerr2 = err2;
|
|
cout << "B " << k << " "
|
|
<< lat1 << " " << azi1 << " " << maxerr2 << endl;
|
|
}
|
|
real latx, lonx;
|
|
if (exact) {
|
|
ge.Direct(lat1, lon1, azi1, s12a + consist, lat, lon);
|
|
ge.Direct(lat2, lon2, azi2, consist, latx, lonx);
|
|
} else {
|
|
g.Direct(lat1, lon1, azi1, s12a + consist, lat, lon);
|
|
g.Direct(lat2, lon2, azi2, consist, latx, lonx);
|
|
}
|
|
real err3 = s.Inverse(lat, lon, latx, lonx, azi1a, azi2a);
|
|
if (exact) {
|
|
ge.Direct(lat1, lon1, azi1, -consist, lat, lon);
|
|
ge.Direct(lat2, lon2, azi2, -s12a - consist, latx, lonx);
|
|
} else {
|
|
g.Direct(lat1, lon1, azi1, -consist, lat, lon);
|
|
g.Direct(lat2, lon2, azi2, -s12a - consist, latx, lonx);
|
|
}
|
|
err3 = max(err3, s.Inverse(lat, lon, latx, lonx, azi1a, azi2a)) / norm;
|
|
if (err3 > maxerr3) {
|
|
maxerr3 = err3;
|
|
cout << "C " << k << " "
|
|
<< lat1 << " " << azi1 << " " << maxerr3 << endl;
|
|
}
|
|
}
|
|
}
|
|
catch (const exception& e) {
|
|
cerr << "Caught exception: " << e.what() << "\n";
|
|
return 1;
|
|
}
|
|
catch (...) {
|
|
cerr << "Caught unknown exception\n";
|
|
return 1;
|
|
}
|
|
cout << "DONE\n";
|
|
return 0;
|
|
}
|