00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 #include "GeographicLib/Geocentric.hpp"
00011 
00012 #define GEOGRAPHICLIB_GEOCENTRIC_CPP "$Id: Geocentric.cpp 6952 2011-02-14 20:26:44Z karney $"
00013 
00014 RCSID_DECL(GEOGRAPHICLIB_GEOCENTRIC_CPP)
00015 RCSID_DECL(GEOGRAPHICLIB_GEOCENTRIC_HPP)
00016 
00017 #if defined(_MSC_VER)
00018 
00019 #pragma warning (disable: 4996)
00020 #endif
00021 
00022 namespace GeographicLib {
00023 
00024   using namespace std;
00025 
00026   Geocentric::Geocentric(real a, real r)
00027     : _a(a)
00028     , _r(r)
00029     , _f(_r != 0 ? 1 / _r : 0)
00030     , _e2(_f * (2 - _f))
00031     , _e2m(sq(1 - _f))          
00032     , _e2a(abs(_e2))
00033     , _e4a(sq(_e2))
00034     , _maxrad(2 * _a / numeric_limits<real>::epsilon())
00035   {
00036     if (!(_a > 0))
00037       throw GeographicErr("Major radius is not positive");
00038     if (!(_f < 1))
00039       throw GeographicErr("Minor radius is not positive");
00040   }
00041 
00042   const Geocentric Geocentric::WGS84(Constants::WGS84_a<real>(),
00043                                      Constants::WGS84_r<real>());
00044   
00045 
00046 
00047 
00048 
00049 
00050 
00051 
00052 
00053 
00054 
00055 
00056 
00057 
00058 
00059 
00060 
00061 
00062   void Geocentric::IntForward(real lat, real lon, real h,
00063                               real& x, real& y, real& z,
00064                               real M[dim2]) const throw() {
00065     lon = lon >= 180 ? lon - 360 : lon < -180 ? lon + 360 : lon;
00066     real
00067       phi = lat * Math::degree<real>(),
00068       lam = lon * Math::degree<real>(),
00069       sphi = sin(phi),
00070       cphi = abs(lat) == 90 ? 0 : cos(phi),
00071       n = _a/sqrt(1 - _e2 * sq(sphi)),
00072       slam = lon == -180 ? 0 : sin(lam),
00073       clam = abs(lon) == 90 ? 0 : cos(lam);
00074     z = ( _e2m * n + h) * sphi;
00075     x = (n + h) * cphi;
00076     y = x * slam;
00077     x *= clam;
00078     if (M)
00079       Rotation(sphi, cphi, slam, clam, M);
00080   }
00081 
00082   void Geocentric::IntReverse(real x, real y, real z,
00083                               real& lat, real& lon, real& h,
00084                               real M[dim2]) const throw() {
00085     real
00086       R = Math::hypot(x, y),
00087       slam = R ? y / R : 0,
00088       clam = R ? x / R : 1;
00089     h = Math::hypot(R, z);      
00090     real sphi, cphi;
00091     if (h > _maxrad) {
00092       
00093       
00094       
00095       
00096       
00097       
00098       R = Math::hypot(x/2, y/2);
00099       slam = R ? (y/2) / R : 0;
00100       clam = R ? (x/2) / R : 1;
00101       real H = Math::hypot(z/2, R);
00102       sphi = (z/2) / H;
00103       cphi = R / H;
00104     } else if (_e4a == 0) {
00105       
00106       
00107       
00108       real H = Math::hypot(h == 0 ? 1 : z, R);
00109       sphi = (h == 0 ? 1 : z) / H;
00110       cphi = R / H;
00111       h -= _a;
00112     } else {
00113       
00114       
00115       real
00116         p = sq(R / _a),
00117         q = _e2m * sq(z / _a),
00118         r = (p + q - _e4a) / 6;
00119       if (_f < 0) swap(p, q);
00120       if ( !(_e4a * q == 0 && r <= 0) ) {
00121         real
00122           
00123           
00124           S = _e4a * p * q / 4, 
00125           r2 = sq(r),
00126           r3 = r * r2,
00127           disc =  S * (2 * r3 + S);
00128         real u = r;
00129         if (disc >= 0) {
00130           real T3 = r3 + S;
00131           
00132           
00133           
00134           T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc); 
00135           
00136           real T = Math::cbrt(T3); 
00137           
00138           u += T + (T != 0 ? r2 / T : 0);
00139         } else {
00140           
00141           real ang = atan2(sqrt(-disc), -(S + r3));
00142           
00143           
00144           u += 2 * r * cos(ang / 3);
00145         }
00146         real
00147           v = sqrt(sq(u) + _e4a * q), 
00148           
00149           
00150           uv = u < 0 ? _e4a * q / (v - u) : u + v, 
00151           
00152           w = max(real(0), _e2a * (uv - q) / (2 * v)),
00153           
00154           
00155           k = uv / (sqrt(uv + sq(w)) + w),
00156           k1 = _f >= 0 ? k : k - _e2,
00157           k2 = _f >= 0 ? k + _e2 : k,
00158           d = k1 * R / k2,
00159           H = Math::hypot(z/k1, R/k2);
00160         sphi = (z/k1) / H;
00161         cphi = (R/k2) / H;
00162         h = (1 - _e2m/k1) * Math::hypot(d, z);
00163       } else {                  
00164         
00165         
00166         
00167         
00168         
00169         
00170         real
00171           zz = sqrt((_f >= 0 ? _e4a - p : p) / _e2m),
00172           xx = sqrt( _f <  0 ? _e4a - p : p        ),
00173           H = Math::hypot(zz, xx);
00174         sphi = zz / H;
00175         cphi = xx / H;
00176         if (z < 0) sphi = -sphi; 
00177         h = - _a * (_f >= 0 ? _e2m : 1) * H / _e2a;
00178       }
00179     }
00180     lat = atan2(sphi, cphi) / Math::degree<real>();
00181     
00182     lon = -atan2(-slam, clam) / Math::degree<real>();
00183     if (M)
00184       Rotation(sphi, cphi, slam, clam, M);
00185   }
00186 
00187   void Geocentric::Rotation(real sphi, real cphi, real slam, real clam,
00188                              real M[dim2]) const throw() {
00189     
00190     
00191     
00192     
00193     
00194     
00195 
00196     
00197     M[0] = -slam;        M[3] =  clam;        M[6] = 0;
00198     
00199     M[1] = -clam * sphi; M[4] = -slam * sphi; M[7] = cphi;
00200     
00201     M[2] =  clam * cphi; M[5] =  slam * cphi; M[8] = sphi;
00202   }
00203 
00204 }