simgear/math/sg_geodesy.cxx

   1 #include <simgear/constants.h>
   2 #include "sg_geodesy.hxx"
   3
   4 // Notes:
   5 //
   6 // The XYZ/cartesian coordinate system in use puts the X axis through
   7 // zero lat/lon (off west Africa), the Z axis through the north pole,
   8 // and the Y axis through 90 degrees longitude (in the Indian Ocean).
   9 //
  10 // All latitude and longitude values are in radians.  Altitude is in
  11 // meters, with zero on the WGS84 ellipsoid.
  12 //
  13 // The code below makes use of the notion of "squashed" space.  This
  14 // is a 2D cylindrical coordinate system where the radius from the Z
  15 // axis is multiplied by SQUASH; the earth in this space is a perfect
  16 // circle with a radius of POLRAD.
  17 //
  18 // Performance: with full optimization, a transformation from
  19 // lat/lon/alt to XYZ and back takes 5263 CPU cycles on my 2.2GHz
  20 // Pentium 4.  About 83% of this is spent in the iterative sgCartToGeod()
  21 // algorithm.
  22
  23 // These are hard numbers from the WGS84 standard.  DON'T MODIFY
  24 // unless you want to change the datum.
  25 static const double EQURAD = 6378137;
  26 static const double iFLATTENING = 298.257223563;
  27
  28 // These are derived quantities more useful to the code:
  29 #if 0
  30 static const double SQUASH = 1 - 1/iFLATTENING;
  31 static const double STRETCH = 1/SQUASH;
  32 static const double POLRAD = EQURAD * SQUASH;
  33 #else
  34 // High-precision versions of the above produced with an arbitrary
  35 // precision calculator (the compiler might lose a few bits in the FPU
  36 // operations).  These are specified to 81 bits of mantissa, which is
  37 // higher than any FPU known to me:
  38 static const double SQUASH  = 0.9966471893352525192801545;
  39 static const double STRETCH = 1.0033640898209764189003079;
  40 static const double POLRAD  = 6356752.3142451794975639668;
  41 #endif
  42
  43 // Returns a "local" geodetic latitude: an approximation that will be
  44 // correct only at zero altitude.
  45 static double localLat(double r, double z)
  46 {
  47     // Squash to a spherical earth, compute a tangent vector to the
  48     // surface circle (in squashed space, the surface is a perfect
  49     // sphere) by swapping the components and negating one, stretch to
  50     // real coordinates, and take an inverse-tangent/perpedicular
  51     // vector to get a local geodetic "up" vector.  (Those steps all
  52     // cook down to just a few multiplies).  Then just turn it into an
  53     // angle.
  54     double upr = r * SQUASH;
  55     double upz = z * STRETCH;
  56     return atan2(upz, upr);
  57 }
  58
  59 // This is the inverse of the algorithm in localLat().  It returns the
  60 // (cylindrical) coordinates of a surface latitude expressed as an
  61 // "up" unit vector.
  62 static void surfRZ(double upr, double upz, double* r, double* z)
  63 {
  64     // We are
  65     // converting a (2D, cylindrical) "up" vector defined by the
  66     // geodetic latitude into unitless R and Z coordinates in
  67     // cartesian space.
  68     double R = upr * STRETCH;
  69     double Z = upz * SQUASH;
  70
  71     // Now we need to turn R and Z into a surface point.  That is,
  72     // pick a coefficient C for them such that the point is on the
  73     // surface when converted to "squashed" space:
  74     //  (C*R*SQUASH)^2 + (C*Z)^2 = POLRAD^2
  75     //   C^2 = POLRAD^2 / ((R*SQUASH)^2 + Z^2)
  76     double sr = R * SQUASH;
  77     double c = POLRAD / sqrt(sr*sr + Z*Z);
  78     R *= c;
  79     Z *= c;
  80
  81     *r = R; *z = Z;
  82 }
  83
  84 // Returns the insersection of the line joining the center of the
  85 // earth and the specified cylindrical point with the surface of the
  86 // WGS84 ellipsoid.  Works by finding a normalization constant (in
  87 // squashed space) that places the squashed point on the surface of
  88 // the sphere.
  89 static double seaLevelRadius(double r, double z)
  90 {
  91     double sr = r * SQUASH;
  92     double norm = POLRAD/sqrt(sr*sr + z*z);
  93     r *= norm;
  94     z *= norm;
  95     return sqrt(r*r + z*z);
  96 }
  97
  98 // Convert a cartexian XYZ coordinate to a geodetic lat/lon/alt.  This
  99 // is a "recursion relation".  In essence, it iterates over the 2D
 100 // part of sgGeodToCart refining its approximation at each step.  The
 101 // MAX_LAT_ERROR threshold is picked carefully to allow us to reach
 102 // the full precision of an IEEE double.  While this algorithm might
 103 // look slow, it's not.  It actually converges very fast indeed --
 104 // I've never seen it take more than six iterations under normal
 105 // conditions.  Three or four is more typical. (It gets slower as the
 106 // altitude/error gets larger; at 50000m altitude, it starts to need
 107 // seven loops.)  One caveat is that at *very* large altitudes, it
 108 // starts making very poor guesses as to latitude.  As altitude
 109 // approaches infinity, it should be guessing with geocentric
 110 // coordinates, not "local geodetic up" ones.
 111 void sgCartToGeod(const double* xyz, double* lat, double* lon, double* alt)
 112 {
 113     // The error is expressed as a radian angle, and we want accuracy
 114     // to 1 part in 2^50 (an IEEE double has between 51 and 52
 115     // significant bits of magnitude due to the "hidden" digit; leave
 116     // at least one bit free for potential slop).  In real units, this
 117     // works out to about 6 nanometers.
 118     static const double MAX_LAT_ERROR = 8.881784197001252e-16;
 119     double x = xyz[0], y = xyz[1], z = xyz[2];
 120
 121     // Longitude is trivial.  Convert to cylindrical "(r, z)"
 122     // coordinates while we're at it.
 123     *lon = atan2(y, x);
 124     double r = sqrt(x*x + y*y);
 125
 126     double lat1, lat2 = localLat(r, z);
 127     double r2, z2, dot;
 128     do {
 129         lat1 = lat2;
 130
 131         // Compute an "up" vector
 132         double upr = cos(lat1);
 133         double upz = sin(lat1);
 134
 135         // Find the surface point with that latitude
 136         surfRZ(upr, upz, &r2, &z2);
 137
 138         // Convert r2z2 to the vector pointing from the surface to rz
 139         r2 = r - r2;
 140         z2 = z - z2;
 141
 142         // Dot it with "up" to get an approximate altitude
 143         dot = r2*upr + z2*upz;
 144
 145         // And compute an approximate geodetic surface coordinate
 146         // using that altitude, so now: R2Z2 = RZ - ((RZ - SURF) dot
 147         // UP)
 148         r2 = r - dot * upr;
 149         z2 = z - dot * upz;
 150
 151         // Find the latitude of *that* point, and iterate
 152         lat2 = localLat(r2, z2);
 153     } while(fabs(lat2 - lat1) > MAX_LAT_ERROR);
 154
 155     // All done!  We have an accurate geodetic lattitude, now
 156     // calculate the altitude as a cartesian distance between the
 157     // final geodetic surface point and the initial r/z coordinate.
 158     *lat = lat1;
 159     double dr = r - r2;
 160     double dz = z - z2;
 161     double altsign = (dot > 0) ? 1 : -1;
 162     *alt = altsign * sqrt(dr*dr + dz*dz);
 163 }
 164
 165 void sgGeodToCart(double lat, double lon, double alt, double* xyz)
 166 {
 167     // This is the inverse of the algorithm in localLat().  We are
 168     // converting a (2D, cylindrical) "up" vector defined by the
 169     // geodetic latitude into unitless R and Z coordinates in
 170     // cartesian space.
 171     double upr = cos(lat);
 172     double upz = sin(lat);
 173     double r, z;
 174     surfRZ(upr, upz, &r, &z);
 175
 176     // Add the altitude using the "up" unit vector we calculated
 177     // initially.
 178     r += upr * alt;
 179     z += upz * alt;
 180
 181     // Finally, convert from cylindrical to cartesian
 182     xyz[0] = r * cos(lon);
 183     xyz[1] = r * sin(lon);
 184     xyz[2] = z;
 185 }
 186
 187 void sgGeocToGeod(double lat_geoc, double radius,
 188                   double *lat_geod, double *alt, double *sea_level_r)
 189 {
 190     // Build a fake cartesian point, and run it through CartToGeod
 191     double lon_dummy, xyz[3];
 192     xyz[0] = cos(lat_geoc) * radius;
 193     xyz[1] = 0;
 194     xyz[2] = sin(lat_geoc) * radius;
 195     sgCartToGeod(xyz, lat_geod, &lon_dummy, alt);
 196     *sea_level_r = seaLevelRadius(xyz[0], xyz[2]);
 197 }
 198
 199 void sgGeodToGeoc(double lat_geod, double alt,
 200                   double *sl_radius, double *lat_geoc)
 201 {
 202     double xyz[3];
 203     sgGeodToCart(lat_geod, 0, alt, xyz);
 204     *lat_geoc = atan2(xyz[2], xyz[0]);
 205     *sl_radius = seaLevelRadius(xyz[0], xyz[2]);
 206 }
 207
 208 ////////////////////////////////////////////////////////////////////////
 209 //
 210 // Direct and inverse distance functions
 211 //
 212 // Proceedings of the 7th International Symposium on Geodetic
 213 // Computations, 1985
 214 //
 215 // "The Nested Coefficient Method for Accurate Solutions of Direct and
 216 // Inverse Geodetic Problems With Any Length"
 217 //
 218 // Zhang Xue-Lian
 219 // pp 747-763
 220 //
 221 // modified for FlightGear to use WGS84 only -- Norman Vine
 222
 223 static const double GEOD_INV_PI = SGD_PI;
 224
 225 // s == distance
 226 // az = azimuth
 227
 228 static inline double M0( double e2 ) {
 229     //double e4 = e2*e2;
 230     return GEOD_INV_PI*(1.0 - e2*( 1.0/4.0 + e2*( 3.0/64.0 +
 231                                                   e2*(5.0/256.0) )))/2.0;
 232 }
 233
 234
 235 // given, alt, lat1, lon1, az1 and distance (s), calculate lat2, lon2
 236 // and az2.  Lat, lon, and azimuth are in degrees.  distance in meters
 237 int geo_direct_wgs_84 ( double alt, double lat1,
 238                         double lon1, double az1,
 239                         double s, double *lat2, double *lon2,
 240                         double *az2 )
 241 {
 242     double a = EQURAD, rf = iFLATTENING;
 243     double RADDEG = (GEOD_INV_PI)/180.0, testv = 1.0E-10;
 244     double f = ( rf > 0.0 ? 1.0/rf : 0.0 );
 245     double b = a*(1.0-f);
 246     double e2 = f*(2.0-f);
 247     double phi1 = lat1*RADDEG, lam1 = lon1*RADDEG;
 248     double sinphi1 = sin(phi1), cosphi1 = cos(phi1);
 249     double azm1 = az1*RADDEG;
 250     double sinaz1 = sin(azm1), cosaz1 = cos(azm1);
 251
 252
 253     if( fabs(s) < 0.01 ) {      // distance < centimeter => congruency
 254         *lat2 = lat1;
 255         *lon2 = lon1;
 256         *az2 = 180.0 + az1;
 257         if( *az2 > 360.0 ) *az2 -= 360.0;
 258         return 0;
 259     } else if( cosphi1 ) {      // non-polar origin
 260         // u1 is reduced latitude
 261         double tanu1 = sqrt(1.0-e2)*sinphi1/cosphi1;
 262         double sig1 = atan2(tanu1,cosaz1);
 263         double cosu1 = 1.0/sqrt( 1.0 + tanu1*tanu1 ), sinu1 = tanu1*cosu1;
 264         double sinaz =  cosu1*sinaz1, cos2saz = 1.0-sinaz*sinaz;
 265         double us = cos2saz*e2/(1.0-e2);
 266
 267         // Terms
 268         double  ta = 1.0+us*(4096.0+us*(-768.0+us*(320.0-175.0*us)))/16384.0,
 269             tb = us*(256.0+us*(-128.0+us*(74.0-47.0*us)))/1024.0,
 270             tc = 0;
 271
 272         // FIRST ESTIMATE OF SIGMA (SIG)
 273         double first = s/(b*ta);  // !!
 274         double sig = first;
 275         double c2sigm, sinsig,cossig, temp,denom,rnumer, dlams, dlam;
 276         do {
 277             c2sigm = cos(2.0*sig1+sig);
 278             sinsig = sin(sig); cossig = cos(sig);
 279             temp = sig;
 280             sig = first +
 281                 tb*sinsig*(c2sigm+tb*(cossig*(-1.0+2.0*c2sigm*c2sigm) -
 282                                       tb*c2sigm*(-3.0+4.0*sinsig*sinsig)
 283                                       *(-3.0+4.0*c2sigm*c2sigm)/6.0)
 284                            /4.0);
 285         } while( fabs(sig-temp) > testv);
 286
 287         // LATITUDE OF POINT 2
 288         // DENOMINATOR IN 2 PARTS (TEMP ALSO USED LATER)
 289         temp = sinu1*sinsig-cosu1*cossig*cosaz1;
 290         denom = (1.0-f)*sqrt(sinaz*sinaz+temp*temp);
 291
 292         // NUMERATOR
 293         rnumer = sinu1*cossig+cosu1*sinsig*cosaz1;
 294         *lat2 = atan2(rnumer,denom)/RADDEG;
 295
 296         // DIFFERENCE IN LONGITUDE ON AUXILARY SPHERE (DLAMS )
 297         rnumer = sinsig*sinaz1;
 298         denom = cosu1*cossig-sinu1*sinsig*cosaz1;
 299         dlams = atan2(rnumer,denom);
 300
 301         // TERM C
 302         tc = f*cos2saz*(4.0+f*(4.0-3.0*cos2saz))/16.0;
 303
 304         // DIFFERENCE IN LONGITUDE
 305         dlam = dlams-(1.0-tc)*f*sinaz*(sig+tc*sinsig*
 306                                        (c2sigm+
 307                                         tc*cossig*(-1.0+2.0*
 308                                                    c2sigm*c2sigm)));
 309         *lon2 = (lam1+dlam)/RADDEG;
 310         if (*lon2 > 180.0  ) *lon2 -= 360.0;
 311         if (*lon2 < -180.0 ) *lon2 += 360.0;
 312
 313         // AZIMUTH - FROM NORTH
 314         *az2 = atan2(-sinaz,temp)/RADDEG;
 315         if ( fabs(*az2) < testv ) *az2 = 0.0;
 316         if( *az2 < 0.0) *az2 += 360.0;
 317         return 0;
 318     } else {                    // phi1 == 90 degrees, polar origin
 319         double dM = a*M0(e2) - s;
 320         double paz = ( phi1 < 0.0 ? 180.0 : 0.0 );
 321         double zero = 0.0f;
 322         return geo_direct_wgs_84( alt, zero, lon1, paz, dM, lat2, lon2, az2 );
 323     }
 324 }
 325
 326
 327 // given alt, lat1, lon1, lat2, lon2, calculate starting and ending
 328 // az1, az2 and distance (s).  Lat, lon, and azimuth are in degrees.
 329 // distance in meters
 330 int geo_inverse_wgs_84( double alt, double lat1,
 331                         double lon1, double lat2,
 332                         double lon2, double *az1, double *az2,
 333                         double *s )
 334 {
 335     double a = EQURAD, rf = iFLATTENING;
 336     int iter=0;
 337     double RADDEG = (GEOD_INV_PI)/180.0, testv = 1.0E-10;
 338     double f = ( rf > 0.0 ? 1.0/rf : 0.0 );
 339     double b = a*(1.0-f);
 340     // double e2 = f*(2.0-f); // unused in this routine
 341     double phi1 = lat1*RADDEG, lam1 = lon1*RADDEG;
 342     double sinphi1 = sin(phi1), cosphi1 = cos(phi1);
 343     double phi2 = lat2*RADDEG, lam2 = lon2*RADDEG;
 344     double sinphi2 = sin(phi2), cosphi2 = cos(phi2);
 345
 346     if( (fabs(lat1-lat2) < testv &&
 347          ( fabs(lon1-lon2) < testv) || fabs(lat1-90.0) < testv ) )
 348     {
 349         // TWO STATIONS ARE IDENTICAL : SET DISTANCE & AZIMUTHS TO ZERO */
 350         *az1 = 0.0; *az2 = 0.0; *s = 0.0;
 351         return 0;
 352     } else if(  fabs(cosphi1) < testv ) {
 353         // initial point is polar
 354         int k = geo_inverse_wgs_84( alt, lat2,lon2,lat1,lon1, az1,az2,s );
 355         k = k; // avoid compiler error since return result is unused
 356         b = *az1; *az1 = *az2; *az2 = b;
 357         return 0;
 358     } else if( fabs(cosphi2) < testv ) {
 359         // terminal point is polar
 360         double _lon1 = lon1 + 180.0f;
 361         int k = geo_inverse_wgs_84( alt, lat1, lon1, lat1, _lon1,
 362                                     az1, az2, s );
 363         k = k; // avoid compiler error since return result is unused
 364         *s /= 2.0;
 365         *az2 = *az1 + 180.0;
 366         if( *az2 > 360.0 ) *az2 -= 360.0;
 367         return 0;
 368     } else if( (fabs( fabs(lon1-lon2) - 180 ) < testv) &&
 369                (fabs(lat1+lat2) < testv) )
 370     {
 371         // Geodesic passes through the pole (antipodal)
 372         double s1,s2;
 373         geo_inverse_wgs_84( alt, lat1,lon1, lat1,lon2, az1,az2, &s1 );
 374         geo_inverse_wgs_84( alt, lat2,lon2, lat1,lon2, az1,az2, &s2 );
 375         *az2 = *az1;
 376         *s = s1 + s2;
 377         return 0;
 378     } else {
 379         // antipodal and polar points don't get here
 380         double dlam = lam2 - lam1, dlams = dlam;
 381         double sdlams,cdlams, sig,sinsig,cossig, sinaz,
 382             cos2saz, c2sigm;
 383         double tc,temp, us,rnumer,denom, ta,tb;
 384         double cosu1,sinu1, sinu2,cosu2;
 385
 386         // Reduced latitudes
 387         temp = (1.0-f)*sinphi1/cosphi1;
 388         cosu1 = 1.0/sqrt(1.0+temp*temp);
 389         sinu1 = temp*cosu1;
 390         temp = (1.0-f)*sinphi2/cosphi2;
 391         cosu2 = 1.0/sqrt(1.0+temp*temp);
 392         sinu2 = temp*cosu2;
 393
 394         do {
 395             sdlams = sin(dlams), cdlams = cos(dlams);
 396             sinsig = sqrt(cosu2*cosu2*sdlams*sdlams+
 397                           (cosu1*sinu2-sinu1*cosu2*cdlams)*
 398                           (cosu1*sinu2-sinu1*cosu2*cdlams));
 399             cossig = sinu1*sinu2+cosu1*cosu2*cdlams;
 400
 401             sig = atan2(sinsig,cossig);
 402             sinaz = cosu1*cosu2*sdlams/sinsig;
 403             cos2saz = 1.0-sinaz*sinaz;
 404             c2sigm = (sinu1 == 0.0 || sinu2 == 0.0 ? cossig :
 405                       cossig-2.0*sinu1*sinu2/cos2saz);
 406             tc = f*cos2saz*(4.0+f*(4.0-3.0*cos2saz))/16.0;
 407             temp = dlams;
 408             dlams = dlam+(1.0-tc)*f*sinaz*
 409                 (sig+tc*sinsig*
 410                  (c2sigm+tc*cossig*(-1.0+2.0*c2sigm*c2sigm)));
 411             if (fabs(dlams) > GEOD_INV_PI && iter++ > 50) {
 412                 return iter;
 413             }
 414         } while ( fabs(temp-dlams) > testv);
 415
 416         us = cos2saz*(a*a-b*b)/(b*b); // !!
 417         // BACK AZIMUTH FROM NORTH
 418         rnumer = -(cosu1*sdlams);
 419         denom = sinu1*cosu2-cosu1*sinu2*cdlams;
 420         *az2 = atan2(rnumer,denom)/RADDEG;
 421         if( fabs(*az2) < testv ) *az2 = 0.0;
 422         if(*az2 < 0.0) *az2 += 360.0;
 423
 424         // FORWARD AZIMUTH FROM NORTH
 425         rnumer = cosu2*sdlams;
 426         denom = cosu1*sinu2-sinu1*cosu2*cdlams;
 427         *az1 = atan2(rnumer,denom)/RADDEG;
 428         if( fabs(*az1) < testv ) *az1 = 0.0;
 429         if(*az1 < 0.0) *az1 += 360.0;
 430
 431         // Terms a & b
 432         ta = 1.0+us*(4096.0+us*(-768.0+us*(320.0-175.0*us)))/
 433             16384.0;
 434         tb = us*(256.0+us*(-128.0+us*(74.0-47.0*us)))/1024.0;
 435
 436         // GEODETIC DISTANCE
 437         *s = b*ta*(sig-tb*sinsig*
 438                    (c2sigm+tb*(cossig*(-1.0+2.0*c2sigm*c2sigm)-tb*
 439                                c2sigm*(-3.0+4.0*sinsig*sinsig)*
 440                                (-3.0+4.0*c2sigm*c2sigm)/6.0)/
 441                     4.0));
 442         return 0;
 443     }
 444 }