My requirement isn't accuracy, it's precision. The difference is
subtle: I don't care about whether or not the terrain is placed
correctly to within a micrometer. As you point out, the local geoid
("sea level", basically) errors are often off by tens of meters from
the WGS84 ellipsoid.
But I *do* care that the answers the algorithms gives are the same in
all cases. If YASim decides on a gear position in cartesian space,
and hands it (as a geodetic lat/lon/alt tuple) to FlightGear for
processing, and FlightGear converts it back to a cartesian coordinate
for some reason (terrain intersection, maybe), those coordinates have
to be EXACTLY THE SAME, to within whatever precision constraint the
FDM can tolerate. That is, if the precision error produced by the
geodetic calculations is enough to produce a different result in the
FDM, it's too high. My algorithm, while somewhat slower, was designed
to give as much precision as possible. The Ralph Toms algorithm was
designed as a high-performance approximation to the right answer.
Attached is a test program that exhibits the issue. It contains
cut-n-pasted versions of both algorithms, so it can be compiled and
run in isolation. Basically, it iterates the following:
+ It picks a geodetic point randomly, and converts it to geocentric
with both algorithms. It calculates the distance between the
resulting points and calls this value "agree". This is the simple
algorithm, and the two implementations should be very close.
+ It takes one of the cartesian points (Toms, as it happens, but
either will do) and, using both algorithms, converts it to geodetic
and back. Ideally, you should get the same point back. In
practice, you don't. It calculates the induced error from the
"Andy" and "Toms" algorithms.
+ If any of these three error terms is higher than one we have seen
before, it prints a simple report showing the source point and the
current error values.
I left this running for a while on my machine, and this is the last
report it issued:
lat: -47.471200 lon: -172.810298 alt 19137.693163 (count: 21868455)
agree: 3.978611782806707e-09, errAndy 8.052877898105077e-09, errToms
0.006983241066336651
So after 22 million iterations, the "agree" value was about 4nm. If
you do the math, this error is one part in 2^50.5, i.e. an error in
the last bit of the double. That's exactly what you expect from two
implementations of the same algorithm. We produce exactly the same
answer for geodetic->geocentric conversions, to within the precision
of the FPU.
The "errAndy" value is the round-trip precision error from a complete
conversion using my sgGeocToGeoc implementation. This is a little
less than 2 bits of error, which is about right when you consider that
the number of FPU operations is least 4x greater for the round trip
than for the simple one way algorithm. Again, it looks like I'm
within the precision of the FPU.
The Ralph Toms algorithm doesn't do quite so well. It's error is
about 7mm -- about a million times higher than mine, or about 20 bits
of error. This is my problem; 7mm is just barely good enough for FDM
work. The gear model would see these errors as spurious
"bumps". Think of a road laid with 7mm gravel -- that's not quite
off-road driving, but it's not terribly far from it. It's
significantly bumpier than asphalt.
Using the Toms algorithm would work, but it would require care to make
sure that the number of operations on geodetic numbers stays small.
That kind of precision threshold situation is just annoying: consider
the "cockpit jitter" issue (most obvious on small cockpits like the
A-4 or Cub), which is very similar. I don't want to have to deal with
this in the gear model.
I'll put together a performance example along the same lines, so folks
can examine that too.
Andy
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
////////////////////////////////////////////////////////////////////////
// Test of Geodetic -> Geocentric/Cartesian transformations
////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////
// Andy's implementation
////////////////////////////////////////////////////////////////////////
static const double SQUASH = 0.9966471893352525192801545;
static const double STRETCH = 1.0033640898209764189003079;
static const double POLRAD = 6356752.3142451794975639668;
static double localLat(double r, double z)
{
double upr = r * SQUASH;
double upz = z * STRETCH;
return atan2(upz, upr);
}
static void surfRZ(double upr, double upz, double* r, double* z)
{
double R = upr * STRETCH;
double Z = upz * SQUASH;
double sr = R * SQUASH;
double c = POLRAD / sqrt(sr*sr + Z*Z);
R *= c;
Z *= c;
*r = R; *z = Z;
}
void sgCartToGeod(double* xyz, double* lat, double* lon, double* alt)
{
static const double MAX_LAT_ERROR = 8.881784197001252e-16;
double x = xyz[0], y = xyz[1], z = xyz[2];
*lon = atan2(y, x);
double r = sqrt(x*x + y*y);
double lat1, lat2 = localLat(r, z);
double r2, z2, dot;
do {
lat1 = lat2;
double upr = cos(lat1);
double upz = sin(lat1);
surfRZ(upr, upz, &r2, &z2);
r2 = r - r2;
z2 = z - z2;
dot = r2*upr + z2*upz;
r2 = r - dot * upr;
z2 = z - dot * upz;
lat2 = localLat(r2, z2);
} while(fabs(lat2 - lat1) > MAX_LAT_ERROR);
*lat = lat1;
double dr = r - r2;
double dz = z - z2;
double altsign = (dot > 0) ? 1 : -1;
*alt = altsign * sqrt(dr*dr + dz*dz);
}
void sgGeodToCart(double lat, double lon, double alt, double* xyz)
{
double upr = cos(lat);
double upz = sin(lat);
double r, z;
surfRZ(upr, upz, &r, &z);
r += upr * alt;
z += upz * alt;
xyz[0] = r * cos(lon);
xyz[1] = r * sin(lon);
xyz[2] = z;
}
////////////////////////////////////////////////////////////////////////
// "GEODETIC" implementation, via Norman Vine
////////////////////////////////////////////////////////////////////////
#define PI 3.14159265358979323e0
#define PI_OVER_2 (PI / 2.0e0)
#define FALSE 0
#define TRUE 1
#define COS_67P5 0.38268343236508977
#define AD_C 1.0026000
static double Geocent_a = 6378137.0;
static double Geocent_f = 1 / 298.257223563;
static double Geocent_e2 = 0.0066943799901413800;
static double Geocent_ep2 = 0.00673949675658690300;
long Convert_Geodetic_To_Geocentric (double Latitude, double Longitude,
double Height, double *X,
double *Y, double *Z, double *Rn )
{
double Sin_Lat, Sin2_Lat, Cos_Lat;
if (Longitude > PI) Longitude -= (2*PI);
Sin_Lat = sin(Latitude);
Cos_Lat = cos(Latitude);
Sin2_Lat = Sin_Lat * Sin_Lat;
*Rn = Geocent_a / (sqrt(1.0e0 - Geocent_e2 * Sin2_Lat));
*X = (*Rn + Height) * Cos_Lat * cos(Longitude);
*Y = (*Rn + Height) * Cos_Lat * sin(Longitude);
*Z = ((*Rn * (1 - Geocent_e2)) + Height) * Sin_Lat;
}
void Convert_Geocentric_To_Geodetic (double X, double Y, double Z,
double *Latitude, double *Longitude,
double *Height, double *Rn )
{
double W, W2, T0, T1, S0, S1, Sin_B0, Sin3_B0;
double Cos_B0, Sin_p1, Cos_p1, Sum;
int At_Pole;
double Geocent_b = Geocent_a * (1 - Geocent_f);
At_Pole = FALSE;
if (X != 0.0) { *Longitude = atan2(Y,X); }
else {
if (Y > 0) { *Longitude = PI_OVER_2; }
else if (Y < 0) { *Longitude = -PI_OVER_2; }
else {
At_Pole = TRUE;
*Longitude = 0.0;
if (Z > 0.0) { *Latitude = PI_OVER_2; }
else if (Z < 0.0) { *Latitude = -PI_OVER_2; }
else {
*Latitude = PI_OVER_2;
*Height = -Geocent_b;
return;
}
}
}
W2 = X*X + Y*Y;
W = sqrt(W2);
T0 = Z * AD_C;
S0 = sqrt(T0 * T0 + W2);
Sin_B0 = T0 / S0;
Cos_B0 = W / S0;
Sin3_B0 = Sin_B0 * Sin_B0 * Sin_B0;
T1 = Z + Geocent_b * Geocent_ep2 * Sin3_B0;
Sum = W - Geocent_a * Geocent_e2 * Cos_B0 * Cos_B0 * Cos_B0;
S1 = sqrt(T1*T1 + Sum * Sum);
Sin_p1 = T1 / S1;
Cos_p1 = Sum / S1;
*Rn = Geocent_a / sqrt(1.0 - Geocent_e2 * Sin_p1 * Sin_p1);
if (Cos_p1 >= COS_67P5)
*Height = W / Cos_p1 - *Rn;
else if (Cos_p1 <= -COS_67P5)
*Height = W / -Cos_p1 - *Rn;
else
*Height = Z / Sin_p1 + *Rn * (Geocent_e2 - 1.0);
if (At_Pole == FALSE)
*Latitude = atan(Sin_p1 / Cos_p1);
}
////////////////////////////////////////////////////////////////////////
// Test Code
//
// Pick a random geodetic point in a range appropriate for a flight
// simulator. Convert to geocentric with both algorithms and compare
// for agreement (both algorithms should be very close). Then convert
// the geocentric point to geodetic and back with both algorithms, and
// check each one for deviation.
////////////////////////////////////////////////////////////////////////
static const float RAD2DEG = 180/M_PI;
static const float DEG2RAD = M_PI/180;
double frand(double max)
{
return (max * rand()) * (1.0/RAND_MAX);
}
double dist(double* a, double* b)
{
double x = a[0]-b[0], y = a[1]-b[1], z = a[2]-b[2];
return sqrt(x*x + y*y + z*z);
}
int main()
{
double lat, lon, alt, dummy, lattmp, lontmp, alttmp;
double PA[3], PA2[3], PT[3], PT2[3]; // points: "Andy" and "Toms"
double maxAgree = 0, maxA = 0, maxT = 0;
int count;
while(1) {
// Generate a random point
lat = DEG2RAD * (frand(180) - 90);
lon = DEG2RAD * (frand(360) - 180);
alt = frand(20300) - 300;
// Convert it to XYZ with both algorithms
sgGeodToCart(lat, lon, alt, PA);
Convert_Geodetic_To_Geocentric(lat, lon, alt,
PT+0, PT+1, PT+2, &dummy);
double agree = dist(PA, PT);
// Convert PT (either point will do) to geodetic and back with
// both algorithms.
double* orig = PT;
sgCartToGeod(orig, &lattmp, &lontmp, &alttmp);
sgGeodToCart(lattmp, lontmp, alttmp, PA2);
double devA = dist(orig, PA2);
Convert_Geocentric_To_Geodetic(orig[0], orig[1], orig[2],
&lattmp, &lontmp, &alttmp, &dummy);
Convert_Geodetic_To_Geocentric(lattmp, lontmp, alttmp,
PT2+0, PT2+1, PT2+2, &dummy);
double devT = dist(orig, PT2);
int needUpdate = 0;
if(agree > maxAgree) { maxAgree = agree; needUpdate = 1; }
if(devA > maxA) { maxA = devA; needUpdate = 1; }
if(devT > maxT) { maxT = devT; needUpdate = 1; }
if(needUpdate) {
printf("lat: %.6f lon: %.6f alt %f (count: %d)\n"
" agree: %.16g, errAndy %.16g, errToms %.16g\n",
RAD2DEG*lat, RAD2DEG*lon, alt,
count, maxAgree, maxA, maxT);
}
count++;
}
}
_______________________________________________
Flightgear-devel mailing list
[EMAIL PROTECTED]
http://mail.flightgear.org/mailman/listinfo/flightgear-devel
