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