GeographicLib
1.21
|
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