simgear/math/SGGeodesy.cxx

   1
   2 #ifdef HAVE_CONFIG_H
   3 #  include <simgear_config.h>
   4 #endif
   5
   6 #include <cmath>
   7
   8 #include "SGMath.hxx"
   9
  10 // These are hard numbers from the WGS84 standard.  DON'T MODIFY
  11 // unless you want to change the datum.
  12 #define _EQURAD 6378137.0
  13 #define _FLATTENING 298.257223563
  14
  15 // These are derived quantities more useful to the code:
  16 #if 0
  17 #define _SQUASH (1 - 1/_FLATTENING)
  18 #define _STRETCH (1/_SQUASH)
  19 #define _POLRAD (EQURAD * _SQUASH)
  20 #else
  21 // High-precision versions of the above produced with an arbitrary
  22 // precision calculator (the compiler might lose a few bits in the FPU
  23 // operations).  These are specified to 81 bits of mantissa, which is
  24 // higher than any FPU known to me:
  25 #define _SQUASH    0.9966471893352525192801545
  26 #define _STRETCH   1.0033640898209764189003079
  27 #define _POLRAD    6356752.3142451794975639668
  28 #endif
  29
  30 // The constants from the WGS84 standard
  31 const double SGGeodesy::EQURAD = _EQURAD;
  32 const double SGGeodesy::iFLATTENING = _FLATTENING;
  33 const double SGGeodesy::SQUASH = _SQUASH;
  34 const double SGGeodesy::STRETCH = _STRETCH;
  35 const double SGGeodesy::POLRAD = _POLRAD;
  36
  37 // additional derived and precomputable ones
  38 // for the geodetic conversion algorithm
  39
  40 #define E2 fabs(1 - _SQUASH*_SQUASH)
  41 static double a = _EQURAD;
  42 static double ra2 = 1/(_EQURAD*_EQURAD);
  43 static double e = sqrt(E2);
  44 static double e2 = E2;
  45 static double e4 = E2*E2;
  46
  47 #undef _EQURAD
  48 #undef _FLATTENING
  49 #undef _SQUASH
  50 #undef _STRETCH
  51 #undef _POLRAD
  52 #undef E2
  53
  54 void
  55 SGGeodesy::SGCartToGeod(const SGVec3<double>& cart, SGGeod& geod)
  56 {
  57   // according to
  58   // H. Vermeille,
  59   // Direct transformation from geocentric to geodetic ccordinates,
  60   // Journal of Geodesy (2002) 76:451-454
  61   double X = cart(0);
  62   double Y = cart(1);
  63   double Z = cart(2);
  64   double XXpYY = X*X+Y*Y;
  65   double sqrtXXpYY = sqrt(XXpYY);
  66   double p = XXpYY*ra2;
  67   double q = Z*Z*(1-e2)*ra2;
  68   double r = 1/6.0*(p+q-e4);
  69   double s = e4*p*q/(4*r*r*r);
  70   double t = pow(1+s+sqrt(s*(2+s)), 1/3.0);
  71   double u = r*(1+t+1/t);
  72   double v = sqrt(u*u+e4*q);
  73   double w = e2*(u+v-q)/(2*v);
  74   double k = sqrt(u+v+w*w)-w;
  75   double D = k*sqrtXXpYY/(k+e2);
  76   geod.setLongitudeRad(2*atan2(Y, X+sqrtXXpYY));
  77   double sqrtDDpZZ = sqrt(D*D+Z*Z);
  78   geod.setLatitudeRad(2*atan2(Z, D+sqrtDDpZZ));
  79   geod.setElevationM((k+e2-1)*sqrtDDpZZ/k);
  80 }
  81
  82 void
  83 SGGeodesy::SGGeodToCart(const SGGeod& geod, SGVec3<double>& cart)
  84 {
  85   // according to
  86   // H. Vermeille,
  87   // Direct transformation from geocentric to geodetic ccordinates,
  88   // Journal of Geodesy (2002) 76:451-454
  89   double lambda = geod.getLongitudeRad();
  90   double phi = geod.getLatitudeRad();
  91   double h = geod.getElevationM();
  92   double sphi = sin(phi);
  93   double n = a/sqrt(1-e2*sphi*sphi);
  94   double cphi = cos(phi);
  95   double slambda = sin(lambda);
  96   double clambda = cos(lambda);
  97   cart(0) = (h+n)*cphi*clambda;
  98   cart(1) = (h+n)*cphi*slambda;
  99   cart(2) = (h+n-e2*n)*sphi;
 100 }
 101
 102 double
 103 SGGeodesy::SGGeodToSeaLevelRadius(const SGGeod& geod)
 104 {
 105   // this is just a simplified version of the SGGeodToCart function above,
 106   // substitute h = 0, take the 2-norm of the cartesian vector and simplify
 107   double phi = geod.getLatitudeRad();
 108   double sphi = sin(phi);
 109   double sphi2 = sphi*sphi;
 110   return a*sqrt((1 + (e4 - 2*e2)*sphi2)/(1 - e2*sphi2));
 111 }
 112
 113 void
 114 SGGeodesy::SGCartToGeoc(const SGVec3<double>& cart, SGGeoc& geoc)
 115 {
 116   double minVal = SGLimits<double>::min();
 117   if (fabs(cart(0)) < minVal && fabs(cart(1)) < minVal)
 118     geoc.setLongitudeRad(0);
 119   else
 120     geoc.setLongitudeRad(atan2(cart(1), cart(0)));
 121
 122   double nxy = sqrt(cart(0)*cart(0) + cart(1)*cart(1));
 123   if (fabs(nxy) < minVal && fabs(cart(2)) < minVal)
 124     geoc.setLatitudeRad(0);
 125   else
 126     geoc.setLatitudeRad(atan2(cart(2), nxy));
 127
 128   geoc.setRadiusM(norm(cart));
 129 }
 130
 131 void
 132 SGGeodesy::SGGeocToCart(const SGGeoc& geoc, SGVec3<double>& cart)
 133 {
 134   double lat = geoc.getLatitudeRad();
 135   double lon = geoc.getLongitudeRad();
 136   double slat = sin(lat);
 137   double clat = cos(lat);
 138   double slon = sin(lon);
 139   double clon = cos(lon);
 140   cart = geoc.getRadiusM()*SGVec3<double>(clat*clon, clat*slon, slat);
 141 }