simgear/math/SGGeodesy.cxx

   1 // Copyright (C) 2006  Mathias Froehlich - Mathias.Froehlich@web.de
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Library General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2 of the License, or (at your option) any later version.
   7 //
   8 // This library is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // Library General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU General Public License
  14 // along with this program; if not, write to the Free Software
  15 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  16 //
  17
  18 #ifdef HAVE_CONFIG_H
  19 #  include <simgear_config.h>
  20 #endif
  21
  22 #include <cmath>
  23
  24 #include "SGMath.hxx"
  25
  26 // These are hard numbers from the WGS84 standard.  DON'T MODIFY
  27 // unless you want to change the datum.
  28 #define _EQURAD 6378137.0
  29 #define _FLATTENING 298.257223563
  30
  31 // These are derived quantities more useful to the code:
  32 #if 0
  33 #define _SQUASH (1 - 1/_FLATTENING)
  34 #define _STRETCH (1/_SQUASH)
  35 #define _POLRAD (EQURAD * _SQUASH)
  36 #else
  37 // High-precision versions of the above produced with an arbitrary
  38 // precision calculator (the compiler might lose a few bits in the FPU
  39 // operations).  These are specified to 81 bits of mantissa, which is
  40 // higher than any FPU known to me:
  41 #define _SQUASH    0.9966471893352525192801545
  42 #define _STRETCH   1.0033640898209764189003079
  43 #define _POLRAD    6356752.3142451794975639668
  44 #endif
  45
  46 // The constants from the WGS84 standard
  47 const double SGGeodesy::EQURAD = _EQURAD;
  48 const double SGGeodesy::iFLATTENING = _FLATTENING;
  49 const double SGGeodesy::SQUASH = _SQUASH;
  50 const double SGGeodesy::STRETCH = _STRETCH;
  51 const double SGGeodesy::POLRAD = _POLRAD;
  52
  53 // additional derived and precomputable ones
  54 // for the geodetic conversion algorithm
  55
  56 #define E2 fabs(1 - _SQUASH*_SQUASH)
  57 static double a = _EQURAD;
  58 static double ra2 = 1/(_EQURAD*_EQURAD);
  59 static double e = sqrt(E2);
  60 static double e2 = E2;
  61 static double e4 = E2*E2;
  62
  63 #undef _EQURAD
  64 #undef _FLATTENING
  65 #undef _SQUASH
  66 #undef _STRETCH
  67 #undef _POLRAD
  68 #undef E2
  69
  70 void
  71 SGGeodesy::SGCartToGeod(const SGVec3<double>& cart, SGGeod& geod)
  72 {
  73   // according to
  74   // H. Vermeille,
  75   // Direct transformation from geocentric to geodetic ccordinates,
  76   // Journal of Geodesy (2002) 76:451-454
  77   double X = cart(0);
  78   double Y = cart(1);
  79   double Z = cart(2);
  80   double XXpYY = X*X+Y*Y;
  81   double sqrtXXpYY = sqrt(XXpYY);
  82   double p = XXpYY*ra2;
  83   double q = Z*Z*(1-e2)*ra2;
  84   double r = 1/6.0*(p+q-e4);
  85   double s = e4*p*q/(4*r*r*r);
  86   double t = pow(1+s+sqrt(s*(2+s)), 1/3.0);
  87   double u = r*(1+t+1/t);
  88   double v = sqrt(u*u+e4*q);
  89   double w = e2*(u+v-q)/(2*v);
  90   double k = sqrt(u+v+w*w)-w;
  91   double D = k*sqrtXXpYY/(k+e2);
  92   geod.setLongitudeRad(2*atan2(Y, X+sqrtXXpYY));
  93   double sqrtDDpZZ = sqrt(D*D+Z*Z);
  94   geod.setLatitudeRad(2*atan2(Z, D+sqrtDDpZZ));
  95   geod.setElevationM((k+e2-1)*sqrtDDpZZ/k);
  96 }
  97
  98 void
  99 SGGeodesy::SGGeodToCart(const SGGeod& geod, SGVec3<double>& cart)
 100 {
 101   // according to
 102   // H. Vermeille,
 103   // Direct transformation from geocentric to geodetic ccordinates,
 104   // Journal of Geodesy (2002) 76:451-454
 105   double lambda = geod.getLongitudeRad();
 106   double phi = geod.getLatitudeRad();
 107   double h = geod.getElevationM();
 108   double sphi = sin(phi);
 109   double n = a/sqrt(1-e2*sphi*sphi);
 110   double cphi = cos(phi);
 111   double slambda = sin(lambda);
 112   double clambda = cos(lambda);
 113   cart(0) = (h+n)*cphi*clambda;
 114   cart(1) = (h+n)*cphi*slambda;
 115   cart(2) = (h+n-e2*n)*sphi;
 116 }
 117
 118 double
 119 SGGeodesy::SGGeodToSeaLevelRadius(const SGGeod& geod)
 120 {
 121   // this is just a simplified version of the SGGeodToCart function above,
 122   // substitute h = 0, take the 2-norm of the cartesian vector and simplify
 123   double phi = geod.getLatitudeRad();
 124   double sphi = sin(phi);
 125   double sphi2 = sphi*sphi;
 126   return a*sqrt((1 + (e4 - 2*e2)*sphi2)/(1 - e2*sphi2));
 127 }
 128
 129 void
 130 SGGeodesy::SGCartToGeoc(const SGVec3<double>& cart, SGGeoc& geoc)
 131 {
 132   double minVal = SGLimits<double>::min();
 133   if (fabs(cart(0)) < minVal && fabs(cart(1)) < minVal)
 134     geoc.setLongitudeRad(0);
 135   else
 136     geoc.setLongitudeRad(atan2(cart(1), cart(0)));
 137
 138   double nxy = sqrt(cart(0)*cart(0) + cart(1)*cart(1));
 139   if (fabs(nxy) < minVal && fabs(cart(2)) < minVal)
 140     geoc.setLatitudeRad(0);
 141   else
 142     geoc.setLatitudeRad(atan2(cart(2), nxy));
 143
 144   geoc.setRadiusM(norm(cart));
 145 }
 146
 147 void
 148 SGGeodesy::SGGeocToCart(const SGGeoc& geoc, SGVec3<double>& cart)
 149 {
 150   double lat = geoc.getLatitudeRad();
 151   double lon = geoc.getLongitudeRad();
 152   double slat = sin(lat);
 153   double clat = cos(lat);
 154   double slon = sin(lon);
 155   double clon = cos(lon);
 156   cart = geoc.getRadiusM()*SGVec3<double>(clat*clon, clat*slon, slat);
 157 }