ADD: new track message, Entity class and Position class
This commit is contained in:
Henry Winkel
498
libs/geographiclib/develop/GeodTest.cpp
Normal file
498
libs/geographiclib/develop/GeodTest.cpp
Normal file
@@ -0,0 +1,498 @@
|
||||
/**
|
||||
* \file GeodTest.cpp
|
||||
**********************************************************************/
|
||||
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <sstream>
|
||||
|
||||
#include "GeographicLib/Geodesic.hpp"
|
||||
#include "GeographicLib/GeodesicLine.hpp"
|
||||
#include "GeographicLib/GeodesicExact.hpp"
|
||||
#include "GeographicLib/GeodesicLineExact.hpp"
|
||||
#include "GeographicLib/Constants.hpp"
|
||||
#include <GeographicLib/Utility.hpp>
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
|
||||
using namespace std;
|
||||
using namespace GeographicLib;
|
||||
|
||||
int usage(int retval) {
|
||||
( retval ? cerr : cout ) <<
|
||||
"GeodTest [ -a | -c | -t0 | -t1 | -t2 | -t3 | -h ]\n\
|
||||
\n\
|
||||
Check GeographicLib::Geodesic class.\n\
|
||||
-a (default) accuracy test (reads test data on standard input)\n\
|
||||
-E accuracy test with GeodesicExact (reads test data on standard input)\n\
|
||||
-F accuracy test with GeodesicExact (reads test data on standard input\n\
|
||||
first line gives a and f)\n\
|
||||
-c coverage test (reads test data on standard input)\n\
|
||||
-t0 time GeodecicLine with distances using synthetic data\n\
|
||||
-t1 time GeodecicLine with angles using synthetic data\n\
|
||||
-t2 time Geodecic::Direct using synthetic data\n\
|
||||
-t3 time Geodecic::Inverse with synthetic data\n\
|
||||
-T0 time GeodecicLineExact with distances using synthetic data\n\
|
||||
-T1 time GeodecicLineExact with angles using synthetic data\n\
|
||||
-T2 time GeodecicExact::Direct using synthetic data\n\
|
||||
-T3 time GeodecicExact::Inverse with synthetic data\n\
|
||||
\n\
|
||||
-c requires an instrumented version of Geodesic.\n";
|
||||
return retval;
|
||||
}
|
||||
|
||||
Math::real angdiff(Math::real a1, Math::real a2) {
|
||||
Math::real d = a2 - a1;
|
||||
if (d >= 180)
|
||||
d -= 360;
|
||||
else if (d < -180)
|
||||
d += 360;
|
||||
return d;
|
||||
}
|
||||
|
||||
Math::real azidiff(Math::real lat,
|
||||
Math::real lon1, Math::real lon2,
|
||||
Math::real azi1, Math::real azi2) {
|
||||
Math::real
|
||||
phi = lat * Math::degree(),
|
||||
alpha1 = azi1 * Math::degree(),
|
||||
alpha2 = azi2 * Math::degree(),
|
||||
dlam = angdiff(lon1, lon2) * Math::degree();
|
||||
Math::real res = sin(alpha2-alpha1)*cos(dlam)
|
||||
-cos(alpha2-alpha1)*sin(dlam)*sin(phi)
|
||||
// -sin(alpha1)*cos(alpha2)*(1-cos(dlam))*cos(phi)*cos(phi)
|
||||
;
|
||||
return res;
|
||||
}
|
||||
|
||||
Math::real dist(Math::real a, Math::real f,
|
||||
Math::real lat0, Math::real lon0,
|
||||
Math::real lat1, Math::real lon1) {
|
||||
// typedef GeographicLib::Math::real real;
|
||||
// real s12;
|
||||
// GeographicLib::Geodesic::
|
||||
// WGS84.Inverse(real(lat0), real(lon0), real(lat1), real(lon1), s12);
|
||||
// return Math::real(s12);
|
||||
a *= Math::degree();
|
||||
if (abs(lat0 + lat1) > Math::real(179.998)) {
|
||||
// Near pole, transform into polar coordinates
|
||||
Math::real
|
||||
r0 = 90 - abs(lat0),
|
||||
r1 = 90 - abs(lat1),
|
||||
lam0 = lon0 * Math::degree(),
|
||||
lam1 = lon1 * Math::degree();
|
||||
return (a / (1 - f)) *
|
||||
hypot(r0 * cos(lam0) - r1 * cos(lam1), r0 * sin(lam0) - r1 * sin(lam1));
|
||||
} else {
|
||||
// Otherwise use cylindrical formula
|
||||
Math::real
|
||||
phi = lat0 * Math::degree(),
|
||||
cphi = abs(lat0) <= 45 ? cos(phi)
|
||||
: sin((90 - abs(lat0)) * Math::degree()),
|
||||
e2 = f * (2 - f),
|
||||
sinphi = sin(phi),
|
||||
n = 1/sqrt(1 - e2 * sinphi * sinphi),
|
||||
// See Wikipedia article on latitude
|
||||
degreeLon = a * cphi * n,
|
||||
degreeLat = a * (1 - e2) * n * n * n,
|
||||
dlon = angdiff(lon1, lon0),
|
||||
dlat = lat1 - lat0;
|
||||
dlat *= degreeLat;
|
||||
dlon *= degreeLon;
|
||||
return hypot(dlat, dlon);
|
||||
}
|
||||
}
|
||||
|
||||
// err[0] error in position of point 2 for the direct problem.
|
||||
// err[1] error in azimuth at point 2 for the direct problem.
|
||||
// err[2] error in m12 for the direct problem & inverse (except near conjugacy)
|
||||
// err[3] error in s12 for the inverse problem.
|
||||
// err[4] error in the azimuths for the inverse problem scaled by m12.
|
||||
// err[5] consistency of the azimuths for the inverse problem.
|
||||
// err[6] area error direct & inverse (except near conjugacy)
|
||||
template<class test>
|
||||
void GeodError(const test& tgeod,
|
||||
Math::real lat1, Math::real lon1, Math::real azi1,
|
||||
Math::real lat2, Math::real lon2, Math::real azi2,
|
||||
Math::real s12, Math::real /*a12*/,
|
||||
Math::real m12, Math::real S12,
|
||||
vector<Math::real>& err) {
|
||||
Math::real tlat1, tlon1, tazi1, tlat2, tlon2, tazi2, ts12, tm12a, tm12b,
|
||||
tM12, tM21, tS12a, tS12b /*, ta12*/;
|
||||
Math::real rlat1, rlon1, razi1, rlat2, rlon2, razi2, rm12;
|
||||
tgeod.Direct(lat1, lon1, azi1, s12,
|
||||
tlat2, tlon2, tazi2, tm12a,
|
||||
tM12, tM21, tS12a);
|
||||
tS12a -= tgeod.EllipsoidArea() * (tazi2-azi2)/720;
|
||||
tgeod.Direct(lat2, lon2, azi2, -s12,
|
||||
tlat1, tlon1, tazi1, tm12b,
|
||||
tM12, tM21, tS12b);
|
||||
tS12b -= tgeod.EllipsoidArea() * (tazi1-azi1)/720;
|
||||
err[0] = max(dist(tgeod.EquatorialRadius(), tgeod.Flattening(),
|
||||
lat2, lon2, tlat2, tlon2),
|
||||
dist(tgeod.EquatorialRadius(), tgeod.Flattening(),
|
||||
lat1, lon1, tlat1, tlon1));
|
||||
err[1] = max(abs(azidiff(lat2, lon2, tlon2, azi2, tazi2)),
|
||||
abs(azidiff(lat1, lon1, tlon1, azi1, tazi1))) *
|
||||
tgeod.EquatorialRadius();
|
||||
err[2] = max(abs(tm12a - m12), abs(tm12b + m12));
|
||||
if (!isnan(S12))
|
||||
err[6] = max(abs(tS12a - S12), abs(tS12b + S12)) / tgeod.EquatorialRadius();
|
||||
|
||||
/* ta12 = */ tgeod.Inverse(lat1, lon1, lat2, lon2,
|
||||
ts12, tazi1, tazi2, tm12a,
|
||||
tM12, tM21, tS12a);
|
||||
tS12a -= tgeod.EllipsoidArea() * ((tazi2-azi2)-(tazi1-azi1))/720;
|
||||
err[3] = abs(ts12 - s12);
|
||||
err[4] = max(abs(angdiff(azi1, tazi1)), abs(angdiff(azi2, tazi2))) *
|
||||
Math::degree() * abs(m12);
|
||||
if (lat1 + lat2 == 0)
|
||||
err[4] = min(err[4],
|
||||
max(abs(angdiff(azi1, tazi2)), abs(angdiff(azi2, tazi1))) *
|
||||
Math::degree() * abs(m12));
|
||||
// m12 and S12 are very sensitive with the inverse problem near conjugacy
|
||||
if (!(s12 > tgeod.EquatorialRadius() && m12 < 10e3)) {
|
||||
err[2] = max(err[2], abs(tm12a - m12));
|
||||
if (!isnan(S12))
|
||||
err[6] = max(err[6], abs(tS12a - S12) / tgeod.EquatorialRadius());
|
||||
}
|
||||
if (s12 > tgeod.EquatorialRadius()) {
|
||||
tgeod.Direct(lat1, lon1, tazi1, ts12/2, rlat2, rlon2, razi2, rm12);
|
||||
tgeod.Direct(lat2, lon2, tazi2, - ts12/2, rlat1, rlon1, razi1, rm12);
|
||||
err[5] = dist(tgeod.EquatorialRadius(), tgeod.Flattening(),
|
||||
rlat1, rlon1, rlat2, rlon2);
|
||||
} else {
|
||||
tgeod.Direct(lat1, lon1, tazi1,
|
||||
ts12 + tgeod.EquatorialRadius(),
|
||||
rlat2, rlon2, razi2, rm12);
|
||||
tgeod.Direct(lat2, lon2, tazi2, tgeod.EquatorialRadius(),
|
||||
rlat1, rlon1, razi1, rm12);
|
||||
err[5] = dist(tgeod.EquatorialRadius(), tgeod.Flattening(),
|
||||
rlat1, rlon1, rlat2, rlon2);
|
||||
tgeod.Direct(lat1, lon1, tazi1, - tgeod.EquatorialRadius(),
|
||||
rlat2, rlon2, razi2, rm12);
|
||||
tgeod.Direct(lat2, lon2, tazi2,
|
||||
- ts12 - tgeod.EquatorialRadius(),
|
||||
rlat1, rlon1, razi1, rm12);
|
||||
err[5] = max(err[5], dist(tgeod.EquatorialRadius(), tgeod.Flattening(),
|
||||
rlat1, rlon1, rlat2, rlon2));
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
Utility::set_digits();
|
||||
Math::real a = Constants::WGS84_a();
|
||||
Math::real f = Constants::WGS84_f();
|
||||
bool timing = false;
|
||||
int timecase = 0; // 0 = line, 1 = line ang, 2 = direct, 3 = inverse
|
||||
bool accuracytest = true;
|
||||
bool coverage = false;
|
||||
bool exact = false;
|
||||
if (argc == 2) {
|
||||
string arg = argv[1];
|
||||
if (arg == "-a") {
|
||||
accuracytest = true;
|
||||
coverage = false;
|
||||
timing = false;
|
||||
exact = false;
|
||||
} else if (arg == "-E") {
|
||||
accuracytest = true;
|
||||
coverage = false;
|
||||
timing = false;
|
||||
exact = true;
|
||||
} else if (arg == "-F") {
|
||||
accuracytest = true;
|
||||
coverage = false;
|
||||
timing = false;
|
||||
exact = true;
|
||||
string s;
|
||||
getline(cin, s);
|
||||
istringstream str(s);
|
||||
str >> a >> f;
|
||||
} else if (arg == "-c") {
|
||||
accuracytest = false;
|
||||
coverage = true;
|
||||
timing = false;
|
||||
exact = false;
|
||||
} else if (arg == "-t0") {
|
||||
accuracytest = false;
|
||||
coverage = false;
|
||||
timing = true;
|
||||
timecase = 0;
|
||||
exact = false;
|
||||
} else if (arg == "-t1") {
|
||||
accuracytest = false;
|
||||
coverage = false;
|
||||
timing = true;
|
||||
timecase = 1;
|
||||
exact = false;
|
||||
} else if (arg == "-t2") {
|
||||
accuracytest = false;
|
||||
coverage = false;
|
||||
timing = true;
|
||||
timecase = 2;
|
||||
exact = false;
|
||||
} else if (arg == "-t3") {
|
||||
accuracytest = false;
|
||||
coverage = false;
|
||||
timing = true;
|
||||
timecase = 3;
|
||||
exact = false;
|
||||
} else if (arg == "-T0") {
|
||||
accuracytest = false;
|
||||
coverage = false;
|
||||
timing = true;
|
||||
timecase = 0;
|
||||
exact = true;
|
||||
} else if (arg == "-T1") {
|
||||
accuracytest = false;
|
||||
coverage = false;
|
||||
timing = true;
|
||||
timecase = 1;
|
||||
exact = true;
|
||||
} else if (arg == "-T2") {
|
||||
accuracytest = false;
|
||||
coverage = false;
|
||||
timing = true;
|
||||
timecase = 2;
|
||||
exact = true;
|
||||
} else if (arg == "-T3") {
|
||||
accuracytest = false;
|
||||
coverage = false;
|
||||
timing = true;
|
||||
timecase = 3;
|
||||
exact = true;
|
||||
} else
|
||||
return usage(arg == "-h" ? 0 : 1);
|
||||
} else if (argc > 2)
|
||||
return usage(1);
|
||||
|
||||
if (timing) {
|
||||
if (!exact) {
|
||||
const Geodesic& geod = Geodesic::WGS84();
|
||||
unsigned cnt = 0;
|
||||
Math::real s = 0;
|
||||
Math::real dl;
|
||||
switch (timecase) {
|
||||
case 0:
|
||||
// Time Line
|
||||
dl = 2e7/1000;
|
||||
for (int i = 0; i <= 90; ++i) {
|
||||
Math::real lat1 = i;
|
||||
for (int j = 0; j <= 180; ++j) {
|
||||
Math::real azi1 = j;
|
||||
const GeodesicLine l(geod, lat1, 0.0, azi1);
|
||||
for (int k = 0; k <= 1000; ++k) {
|
||||
Math::real s12 = dl * k;
|
||||
Math::real lat2, lon2;
|
||||
l.Position(s12, lat2, lon2);
|
||||
++cnt;
|
||||
s += lat2;
|
||||
}
|
||||
}
|
||||
}
|
||||
cout << cnt << " " << s << "\n";
|
||||
break;
|
||||
case 1:
|
||||
// Time Line ang
|
||||
dl = Math::real(180)/1000;
|
||||
for (int i = 0; i <= 90; ++i) {
|
||||
Math::real lat1 = i;
|
||||
for (int j = 0; j <= 180; ++j) {
|
||||
Math::real azi1 = j;
|
||||
GeodesicLine l(geod, lat1, 0.0, azi1);
|
||||
for (int k = 0; k <= 1000; ++k) {
|
||||
Math::real s12 = dl * k;
|
||||
Math::real lat2, lon2;
|
||||
l.ArcPosition(s12, lat2, lon2);
|
||||
++cnt;
|
||||
s += lat2;
|
||||
}
|
||||
}
|
||||
}
|
||||
cout << cnt << " " << s << "\n";
|
||||
break;
|
||||
case 2:
|
||||
// Time Direct
|
||||
dl = 2e7/200;
|
||||
for (int i = 0; i <= 90; ++i) {
|
||||
Math::real lat1 = i;
|
||||
for (int j = 0; j <= 180; ++j) {
|
||||
Math::real azi1 = j;
|
||||
for (int k = 0; k <= 200; ++k) {
|
||||
Math::real s12 = dl * k;
|
||||
Math::real lat2, lon2;
|
||||
geod.Direct(lat1, 0.0, azi1, s12, lat2, lon2);
|
||||
++cnt;
|
||||
s += lat2;
|
||||
}
|
||||
}
|
||||
}
|
||||
cout << cnt << " " << s << "\n";
|
||||
break;
|
||||
case 3:
|
||||
// Time Inverse
|
||||
for (int i = 1; i <= 179; i += 2) {
|
||||
Math::real lat1 = i * Math::real(0.5);
|
||||
for (int j = -179; j <= 179; j += 2) {
|
||||
Math::real lat2 = j * Math::real(0.5);
|
||||
for (int k = 1; k <= 359; k += 2) {
|
||||
Math::real lon2 = k * Math::real(0.5);
|
||||
Math::real s12;
|
||||
geod.Inverse(lat1, 0.0, lat2, lon2, s12);
|
||||
++cnt;
|
||||
s += s12;
|
||||
}
|
||||
}
|
||||
}
|
||||
cout << cnt << " " << s << "\n";
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
const GeodesicExact& geod = GeodesicExact::WGS84();
|
||||
unsigned cnt = 0;
|
||||
Math::real s = 0;
|
||||
Math::real dl;
|
||||
switch (timecase) {
|
||||
case 0:
|
||||
// Time Line
|
||||
dl = 2e7/1000;
|
||||
for (int i = 0; i <= 90; ++i) {
|
||||
Math::real lat1 = i;
|
||||
for (int j = 0; j <= 180; ++j) {
|
||||
Math::real azi1 = j;
|
||||
const GeodesicLineExact l(geod, lat1, 0.0, azi1);
|
||||
for (int k = 0; k <= 1000; ++k) {
|
||||
Math::real s12 = dl * k;
|
||||
Math::real lat2, lon2;
|
||||
l.Position(s12, lat2, lon2);
|
||||
++cnt;
|
||||
s += lat2;
|
||||
}
|
||||
}
|
||||
}
|
||||
cout << cnt << " " << s << "\n";
|
||||
break;
|
||||
case 1:
|
||||
// Time Line ang
|
||||
dl = Math::real(180)/1000;
|
||||
for (int i = 0; i <= 90; ++i) {
|
||||
Math::real lat1 = i;
|
||||
for (int j = 0; j <= 180; ++j) {
|
||||
Math::real azi1 = j;
|
||||
GeodesicLineExact l(geod, lat1, 0.0, azi1);
|
||||
for (int k = 0; k <= 1000; ++k) {
|
||||
Math::real s12 = dl * k;
|
||||
Math::real lat2, lon2;
|
||||
l.ArcPosition(s12, lat2, lon2);
|
||||
++cnt;
|
||||
s += lat2;
|
||||
}
|
||||
}
|
||||
}
|
||||
cout << cnt << " " << s << "\n";
|
||||
break;
|
||||
case 2:
|
||||
// Time Direct
|
||||
dl = 2e7/200;
|
||||
for (int i = 0; i <= 90; ++i) {
|
||||
Math::real lat1 = i;
|
||||
for (int j = 0; j <= 180; ++j) {
|
||||
Math::real azi1 = j;
|
||||
for (int k = 0; k <= 200; ++k) {
|
||||
Math::real s12 = dl * k;
|
||||
Math::real lat2, lon2;
|
||||
geod.Direct(lat1, 0.0, azi1, s12, lat2, lon2);
|
||||
++cnt;
|
||||
s += lat2;
|
||||
}
|
||||
}
|
||||
}
|
||||
cout << cnt << " " << s << "\n";
|
||||
break;
|
||||
case 3:
|
||||
// Time Inverse
|
||||
for (int i = 1; i <= 179; i += 2) {
|
||||
Math::real lat1 = i * Math::real(0.5);
|
||||
for (int j = -179; j <= 179; j += 2) {
|
||||
Math::real lat2 = j * Math::real(0.5);
|
||||
for (int k = 1; k <= 359; k += 2) {
|
||||
Math::real lon2 = k * Math::real(0.5);
|
||||
Math::real s12;
|
||||
geod.Inverse(lat1, 0.0, lat2, lon2, s12);
|
||||
++cnt;
|
||||
s += s12;
|
||||
}
|
||||
}
|
||||
}
|
||||
cout << cnt << " " << s << "\n";
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (accuracytest || coverage) {
|
||||
const Geodesic geod(a, f);
|
||||
const GeodesicExact geode(a, f);
|
||||
|
||||
const unsigned NUMERR = 7;
|
||||
|
||||
cout << fixed << setprecision(2);
|
||||
vector<Math::real> erra(NUMERR);
|
||||
vector<Math::real> err(NUMERR, 0.0);
|
||||
vector<unsigned> errind(NUMERR);
|
||||
unsigned cnt = 0;
|
||||
string s;
|
||||
while (getline(cin, s)) {
|
||||
istringstream str(s);
|
||||
Math::real lat1l, lon1l, azi1l, lat2l, lon2l, azi2l,
|
||||
s12l, a12l, m12l, S12l;
|
||||
if (!(str >> lat1l >> lon1l >> azi1l
|
||||
>> lat2l >> lon2l >> azi2l
|
||||
>> s12l >> a12l >> m12l))
|
||||
break;
|
||||
if (!(str >> S12l))
|
||||
S12l = Math::NaN();
|
||||
if (coverage) {
|
||||
#if defined(GEOD_DIAG) && GEOD_DIAG
|
||||
Math::real
|
||||
lat1 = lat1l, lon1 = lon1l,
|
||||
lat2 = lat2l, lon2 = lon2l,
|
||||
azi1, azi2, s12, m12;
|
||||
geod.Inverse(lat1, lon1, lat2, lon2, s12, azi1, azi2, m12);
|
||||
cout << geod.coverage << " " << geod.niter << "\n";
|
||||
#endif
|
||||
} else {
|
||||
exact ?
|
||||
GeodError< GeodesicExact >
|
||||
(geode, lat1l, lon1l, azi1l,
|
||||
lat2l, lon2l, azi2l,
|
||||
s12l, a12l, m12l, S12l,
|
||||
erra) :
|
||||
GeodError< Geodesic >
|
||||
(geod, lat1l, lon1l, azi1l,
|
||||
lat2l, lon2l, azi2l,
|
||||
s12l, a12l, m12l, S12l,
|
||||
erra);
|
||||
for (unsigned i = 0; i < NUMERR; ++i) {
|
||||
if (isfinite(err[i]) && !(erra[i] <= err[i])) {
|
||||
err[i] = erra[i];
|
||||
errind[i] = cnt;
|
||||
}
|
||||
}
|
||||
++cnt;
|
||||
}
|
||||
}
|
||||
if (accuracytest) {
|
||||
Math::real mult = Math::real(Math::extra_digits() == 0 ? 1e9l :
|
||||
Math::extra_digits() <= 3 ? 1e12l : 1e15l);
|
||||
for (unsigned i = 0; i < NUMERR; ++i)
|
||||
cout << i << " " << mult * err[i]
|
||||
<< " " << errind[i] << "\n";
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user