GeographicLib  1.21
Geocentric.cpp
Go to the documentation of this file.
00001 /**
00002  * \file Geocentric.cpp
00003  * \brief Implementation for GeographicLib::Geocentric class
00004  *
00005  * Copyright (c) Charles Karney (2008-2011) <charles@karney.com> and licensed
00006  * under the MIT/X11 License.  For more information, see
00007  * http://geographiclib.sourceforge.net/
00008  **********************************************************************/
00009 
00010 #include <GeographicLib/Geocentric.hpp>
00011 
00012 #define GEOGRAPHICLIB_GEOCENTRIC_CPP \
00013   "$Id: b5135e8042dbbbcddfd5894c66b729bf5c43cab9 $"
00014 
00015 RCSID_DECL(GEOGRAPHICLIB_GEOCENTRIC_CPP)
00016 RCSID_DECL(GEOGRAPHICLIB_GEOCENTRIC_HPP)
00017 
00018 namespace GeographicLib {
00019 
00020   using namespace std;
00021 
00022   Geocentric::Geocentric(real a, real f)
00023     : _a(a)
00024     , _f(f <= 1 ? f : 1/f)
00025     , _e2(_f * (2 - _f))
00026     , _e2m(Math::sq(1 - _f))          // 1 - _e2
00027     , _e2a(abs(_e2))
00028     , _e4a(Math::sq(_e2))
00029     , _maxrad(2 * _a / numeric_limits<real>::epsilon())
00030   {
00031     if (!(Math::isfinite(_a) && _a > 0))
00032       throw GeographicErr("Major radius is not positive");
00033     if (!(Math::isfinite(_f) && _f < 1))
00034       throw GeographicErr("Minor radius is not positive");
00035   }
00036 
00037   const Geocentric Geocentric::WGS84(Constants::WGS84_a<real>(),
00038                                      Constants::WGS84_f<real>());
00039 
00040   void Geocentric::IntForward(real lat, real lon, real h,
00041                               real& X, real& Y, real& Z,
00042                               real M[dim2_]) const throw() {
00043     lon = lon >= 180 ? lon - 360 : (lon < -180 ? lon + 360 : lon);
00044     real
00045       phi = lat * Math::degree<real>(),
00046       lam = lon * Math::degree<real>(),
00047       sphi = sin(phi),
00048       cphi = abs(lat) == 90 ? 0 : cos(phi),
00049       n = _a/sqrt(1 - _e2 * Math::sq(sphi)),
00050       slam = lon == -180 ? 0 : sin(lam),
00051       clam = abs(lon) == 90 ? 0 : cos(lam);
00052     Z = ( _e2m * n + h) * sphi;
00053     X = (n + h) * cphi;
00054     Y = X * slam;
00055     X *= clam;
00056     if (M)
00057       Rotation(sphi, cphi, slam, clam, M);
00058   }
00059 
00060   void Geocentric::IntReverse(real X, real Y, real Z,
00061                               real& lat, real& lon, real& h,
00062                               real M[dim2_]) const throw() {
00063     real
00064       R = Math::hypot(X, Y),
00065       slam = R ? Y / R : 0,
00066       clam = R ? X / R : 1;
00067     h = Math::hypot(R, Z);      // Distance to center of earth
00068     real sphi, cphi;
00069     if (h > _maxrad) {
00070       // We really far away (> 12 million light years); treat the earth as a
00071       // point and h, above, is an acceptable approximation to the height.
00072       // This avoids overflow, e.g., in the computation of disc below.  It's
00073       // possible that h has overflowed to inf; but that's OK.
00074       //
00075       // Treat the case X, Y finite, but R overflows to +inf by scaling by 2.
00076       R = Math::hypot(X/2, Y/2);
00077       slam = R ? (Y/2) / R : 0;
00078       clam = R ? (X/2) / R : 1;
00079       real H = Math::hypot(Z/2, R);
00080       sphi = (Z/2) / H;
00081       cphi = R / H;
00082     } else if (_e4a == 0) {
00083       // Treat the spherical case.  Dealing with underflow in the general case
00084       // with _e2 = 0 is difficult.  Origin maps to N pole same as with
00085       // ellipsoid.
00086       real H = Math::hypot(h == 0 ? 1 : Z, R);
00087       sphi = (h == 0 ? 1 : Z) / H;
00088       cphi = R / H;
00089       h -= _a;
00090     } else {
00091       // Treat prolate spheroids by swapping R and Z here and by switching
00092       // the arguments to phi = atan2(...) at the end.
00093       real
00094         p = Math::sq(R / _a),
00095         q = _e2m * Math::sq(Z / _a),
00096         r = (p + q - _e4a) / 6;
00097       if (_f < 0) swap(p, q);
00098       if ( !(_e4a * q == 0 && r <= 0) ) {
00099         real
00100           // Avoid possible division by zero when r = 0 by multiplying
00101           // equations for s and t by r^3 and r, resp.
00102           S = _e4a * p * q / 4, // S = r^3 * s
00103           r2 = Math::sq(r),
00104           r3 = r * r2,
00105           disc = S * (2 * r3 + S);
00106         real u = r;
00107         if (disc >= 0) {
00108           real T3 = S + r3;
00109           // Pick the sign on the sqrt to maximize abs(T3).  This minimizes
00110           // loss of precision due to cancellation.  The result is unchanged
00111           // because of the way the T is used in definition of u.
00112           T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc); // T3 = (r * t)^3
00113           // N.B. cbrt always returns the real root.  cbrt(-8) = -2.
00114           real T = Math::cbrt(T3); // T = r * t
00115           // T can be zero; but then r2 / T -> 0.
00116           u += T + (T != 0 ? r2 / T : 0);
00117         } else {
00118           // T is complex, but the way u is defined the result is real.
00119           real ang = atan2(sqrt(-disc), -(S + r3));
00120           // There are three possible cube roots.  We choose the root which
00121           // avoids cancellation.  Note that disc < 0 implies that r < 0.
00122           u += 2 * r * cos(ang / 3);
00123         }
00124         real
00125           v = sqrt(Math::sq(u) + _e4a * q), // guaranteed positive
00126           // Avoid loss of accuracy when u < 0.  Underflow doesn't occur in
00127           // e4 * q / (v - u) because u ~ e^4 when q is small and u < 0.
00128           uv = u < 0 ? _e4a * q / (v - u) : u + v, // u+v, guaranteed positive
00129           // Need to guard against w going negative due to roundoff in uv - q.
00130           w = max(real(0), _e2a * (uv - q) / (2 * v)),
00131           // Rearrange expression for k to avoid loss of accuracy due to
00132           // subtraction.  Division by 0 not possible because uv > 0, w >= 0.
00133           k = uv / (sqrt(uv + Math::sq(w)) + w),
00134           k1 = _f >= 0 ? k : k - _e2,
00135           k2 = _f >= 0 ? k + _e2 : k,
00136           d = k1 * R / k2,
00137           H = Math::hypot(Z/k1, R/k2);
00138         sphi = (Z/k1) / H;
00139         cphi = (R/k2) / H;
00140         h = (1 - _e2m/k1) * Math::hypot(d, Z);
00141       } else {                  // e4 * q == 0 && r <= 0
00142         // This leads to k = 0 (oblate, equatorial plane) and k + e^2 = 0
00143         // (prolate, rotation axis) and the generation of 0/0 in the general
00144         // formulas for phi and h.  using the general formula and division by 0
00145         // in formula for h.  So handle this case by taking the limits:
00146         // f > 0: z -> 0, k      ->   e2 * sqrt(q)/sqrt(e4 - p)
00147         // f < 0: R -> 0, k + e2 -> - e2 * sqrt(q)/sqrt(e4 - p)
00148         real
00149           zz = sqrt((_f >= 0 ? _e4a - p : p) / _e2m),
00150           xx = sqrt( _f <  0 ? _e4a - p : p        ),
00151           H = Math::hypot(zz, xx);
00152         sphi = zz / H;
00153         cphi = xx / H;
00154         if (Z < 0) sphi = -sphi; // for tiny negative Z (not for prolate)
00155         h = - _a * (_f >= 0 ? _e2m : 1) * H / _e2a;
00156       }
00157     }
00158     lat = atan2(sphi, cphi) / Math::degree<real>();
00159     // Negative signs return lon in [-180, 180).
00160     lon = -atan2(-slam, clam) / Math::degree<real>();
00161     if (M)
00162       Rotation(sphi, cphi, slam, clam, M);
00163   }
00164 
00165   void Geocentric::Rotation(real sphi, real cphi, real slam, real clam,
00166                             real M[dim2_]) throw() {
00167     // This rotation matrix is given by the following quaternion operations
00168     // qrot(lam, [0,0,1]) * qrot(phi, [0,-1,0]) * [1,1,1,1]/2
00169     // or
00170     // qrot(pi/2 + lam, [0,0,1]) * qrot(-pi/2 + phi , [-1,0,0])
00171     // where
00172     // qrot(t,v) = [cos(t/2), sin(t/2)*v[1], sin(t/2)*v[2], sin(t/2)*v[3]]
00173 
00174     // Local X axis (east) in geocentric coords
00175     M[0] = -slam;        M[3] =  clam;        M[6] = 0;
00176     // Local Y axis (north) in geocentric coords
00177     M[1] = -clam * sphi; M[4] = -slam * sphi; M[7] = cphi;
00178     // Local Z axis (up) in geocentric coords
00179     M[2] =  clam * cphi; M[5] =  slam * cphi; M[8] = sphi;
00180   }
00181 
00182 } // namespace GeographicLib