As promised, here is the performance test of the geocentric->geodetic
implementations. As before, it's a stand-alone C program that should
compile independantly and be easy to inspect and modify. Just run it
with a single argument of either "andy" or "toms" to select the
algorithm.
On my laptop (a 2.2GHz P4 that is currently reporting 1196MHz in
/proc/cpuinfo), compiled with -O2 (no fancy optimizer settings), I'm
seeing the following:
localhost:~$ time ./geod andy
1000000 iterations
real 0m4.097s
user 0m4.060s
sys 0m0.020s
localhost:~$ time ./geod toms
1000000 iterations
real 0m0.871s
user 0m0.870s
sys 0m0.000s
That comes out to about 4900 CPU cycles per call for my
implementation, and 1042 cycles for the GEODETIC code. Both are
fairly small numbers for a function called only a handful of times
each frame.
Andy
#include <strings.h>
#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;
return 0;
}
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
//
// Generate a bunch of test points, and iterate over them using an
// geocentric->geodetic implementation specified on the command line
// as either "andy" or "toms".
////////////////////////////////////////////////////////////////////////
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);
}
const int NPTS = 1000;
const int REPCOUNT = 1000;
int main(int argc, char** argv)
{
int i, j;
double lat, lon, alt, dummy;
double pts[3*NPTS];
// Parse command line
int doAndy;
if(argc < 2) {
printf("Usage: geod-perf [andy|toms]\n");
exit(1);
}
else if(strcasecmp(argv[1], "andy") == 0) { doAndy = 1; }
else if(strcasecmp(argv[1], "toms") == 0) { doAndy = 0; }
else {
printf("Usage: geod-perf [andy|toms]\n");
exit(1);
}
// Fill the input points with randomly chosen values, to avoid
// doing the rand() calls inside the loop.
for(i=0; i<NPTS; i++) {
double* p = pts + 3*i;
lat = DEG2RAD * (frand(180) - 90);
lon = DEG2RAD * (frand(360) - 180);
alt = frand(20300) - 300;
sgGeodToCart(lat, lon, alt, p);
}
// Do the test
if(doAndy) {
for(i=0; i<REPCOUNT; i++)
for(j=0; j<NPTS; j++)
sgCartToGeod(pts + 3*j, &lat, &lon, &alt);
} else {
for(i=0; i<REPCOUNT; i++)
for(j=0; j<NPTS; j++) {
double* p = pts + 3*j;
Convert_Geocentric_To_Geodetic(p[0], p[1], p[2],
&lat, &lon, &alt, &dummy);
}
}
printf("%d iterations\n", REPCOUNT * NPTS);
return 0;
}
_______________________________________________
Flightgear-devel mailing list
[EMAIL PROTECTED]
http://mail.flightgear.org/mailman/listinfo/flightgear-devel
