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, 2009, 2010) <charles@karney.com>
00006  * and licensed under the LGPL.  For more information, see
00007  * http://geographiclib.sourceforge.net/
00008  **********************************************************************/
00009 
00010 #include "GeographicLib/Geocentric.hpp"
00011 
00012 #define GEOGRAPHICLIB_GEOCENTRIC_CPP "$Id: Geocentric.cpp 6827 2010-05-20 19:56:18Z karney $"
00013 
00014 RCSID_DECL(GEOGRAPHICLIB_GEOCENTRIC_CPP)
00015 RCSID_DECL(GEOGRAPHICLIB_GEOCENTRIC_HPP)
00016 
00017 namespace GeographicLib {
00018 
00019   using namespace std;
00020 
00021   Geocentric::Geocentric(real a, real r)
00022     : _a(a)
00023     , _r(r)
00024     , _f(_r != 0 ? 1 / _r : 0)
00025     , _e2(_f * (2 - _f))
00026     , _e2m(sq(1 - _f))          // 1 - _e2
00027     , _e2a(abs(_e2))
00028     , _e4a(sq(_e2))
00029     , _maxrad(2 * _a / numeric_limits<real>::epsilon())
00030   {
00031     if (!(_a > 0))
00032       throw GeographicErr("Major radius is not positive");
00033   }
00034 
00035   const Geocentric Geocentric::WGS84(Constants::WGS84_a(),
00036                                      Constants::WGS84_r());
00037 
00038   void Geocentric::Forward(real lat, real lon, real h,
00039                            real& x, real& y, real& z) const throw() {
00040     lon = lon >= 180 ? lon - 360 : lon < -180 ? lon + 360 : lon;
00041     real
00042       phi = lat * Constants::degree(),
00043       lam = lon * Constants::degree(),
00044       sphi = sin(phi),
00045       cphi = abs(lat) == 90 ? 0 : cos(phi),
00046       n = _a/sqrt(1 - _e2 * sq(sphi));
00047     z = ( _e2m * n + h) * sphi;
00048     x = (n + h) * cphi;
00049     y = x * (lon == -180 ? 0 : sin(lam));
00050     x *= (abs(lon) == 90 ? 0 : cos(lam));
00051   }
00052 
00053   void Geocentric::Reverse(real x, real y, real z,
00054                            real& lat, real& lon, real& h) const throw() {
00055     real R = Math::hypot(x, y);
00056     h = Math::hypot(R, z);      // Distance to center of earth
00057     real phi;
00058     if (h > _maxrad)
00059       // We really far away (> 12 million light years); treat the earth as a
00060       // point and h, above, is an acceptable approximation to the height.
00061       // This avoids overflow, e.g., in the computation of disc below.  It's
00062       // possible that h has overflowed to inf; but that's OK.
00063       //
00064       // Treat the case x, y finite, but R overflows to +inf by scaling by 2.
00065       phi = atan2(z/2, Math::hypot(x/2, y/2));
00066     else if (_e4a == 0) {
00067       // Treat the spherical case.  Dealing with underflow in the general case
00068       // with _e2 = 0 is difficult.  Origin maps to N pole same as an
00069       // ellipsoid.
00070       phi = atan2(h != 0 ? z : 1, R);
00071       h -= _a;
00072     } else {
00073       // Treat prolate spheroids by swapping R and z here and by switching
00074       // the arguments to phi = atan2(...) at the end.
00075       real
00076         p = sq(R / _a),
00077         q = _e2m * sq(z / _a),
00078         r = (p + q - _e4a) / 6;
00079       if (_f < 0) swap(p, q);
00080       if ( !(_e4a * q == 0 && r <= 0) ) {
00081         real
00082           // Avoid possible division by zero when r = 0 by multiplying
00083           // equations for s and t by r^3 and r, resp.
00084           S = _e4a * p * q / 4, // S = r^3 * s
00085           r2 = sq(r),
00086           r3 = r * r2,
00087           disc =  S * (2 * r3 + S);
00088         real u = r;
00089         if (disc >= 0) {
00090           real T3 = r3 + S;
00091           // Pick the sign on the sqrt to maximize abs(T3).  This minimizes
00092           // loss of precision due to cancellation.  The result is unchanged
00093           // because of the way the T is used in definition of u.
00094           T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc); // T3 = (r * t)^3
00095           // N.B. cbrt always returns the real root.  cbrt(-8) = -2.
00096           real T = Math::cbrt(T3); // T = r * t
00097           // T can be zero; but then r2 / T -> 0.
00098           u += T + (T != 0 ? r2 / T : 0);
00099         } else {
00100           // T is complex, but the way u is defined the result is real.
00101           real ang = atan2(sqrt(-disc), -(S + r3));
00102           // There are three possible cube roots.  We choose the root which
00103           // avoids cancellation.  Note that disc < 0 implies that r < 0.
00104           u += 2 * r * cos(ang / 3);
00105         }
00106         real
00107           v = sqrt(sq(u) + _e4a * q), // guaranteed positive
00108           // Avoid loss of accuracy when u < 0.  Underflow doesn't occur in
00109           // e4 * q / (v - u) because u ~ e^4 when q is small and u < 0.
00110           uv = u < 0 ? _e4a * q / (v - u) : u + v, // u+v, guaranteed positive
00111           // Need to guard against w going negative due to roundoff in uv - q.
00112           w = max(real(0), _e2a * (uv - q) / (2 * v)),
00113           // Rearrange expression for k to avoid loss of accuracy due to
00114           // subtraction.  Division by 0 not possible because uv > 0, w >= 0.
00115           k = uv / (sqrt(uv + sq(w)) + w),
00116           k1 = _f >= 0 ? k : k - _e2,
00117           k2 = _f >= 0 ? k + _e2 : k,
00118           d = k1 * R / k2;
00119         phi = atan2(z/k1, R/k2);
00120         h = (1 - _e2m/k1) * Math::hypot(d, z);
00121       } else {                  // e4 * q == 0 && r <= 0
00122         // This leads to k = 0 (oblate, equatorial plane) and k + e^2 = 0
00123         // (prolate, rotation axis) and the generation of 0/0 in the general
00124         // formulas for phi and h.  using the general formula and division by 0
00125         // in formula for h.  So handle this case by taking the limits:
00126         // f > 0: z -> 0, k      ->   e2 * sqrt(q)/sqrt(e4 - p)
00127         // f < 0: R -> 0, k + e2 -> - e2 * sqrt(q)/sqrt(e4 - p)
00128         real
00129           zz = sqrt((_f >= 0 ? _e4a - p : p) / _e2m),
00130           xx = sqrt( _f <  0 ? _e4a - p : p        );
00131         phi = atan2(zz, xx);
00132         if (z < 0) phi = -phi; // for tiny negative z (not for prolate)
00133         h = - _a * (_f >= 0 ? _e2m : 1) * Math::hypot(xx, zz) / _e2a;
00134       }
00135     }
00136     lat = phi / Constants::degree();
00137     // Negative signs return lon in [-180, 180).  Assume atan2(0,0) = 0.
00138     lon = -atan2(-y, x) / Constants::degree();
00139   }
00140 
00141 } // namespace GeographicLib

Generated on 21 May 2010 for GeographicLib by  doxygen 1.6.1