ADD: new track message, Entity class and Position class
This commit is contained in:
Henry Winkel
337
libs/geographiclib/develop/GeodShort.cpp
Normal file
337
libs/geographiclib/develop/GeodShort.cpp
Normal file
@@ -0,0 +1,337 @@
|
||||
/**
|
||||
* \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;
|
||||
}
|
||||
Reference in New Issue
Block a user