simgear/math/SGGeodesy.cxx

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