00001
00002
00003
00004
00005
00006
00007
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))
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);
00057 real phi;
00058 if (h > _maxrad)
00059
00060
00061
00062
00063
00064
00065 phi = atan2(z/2, Math::hypot(x/2, y/2));
00066 else if (_e4a == 0) {
00067
00068
00069
00070 phi = atan2(h != 0 ? z : 1, R);
00071 h -= _a;
00072 } else {
00073
00074
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
00083
00084 S = _e4a * p * q / 4,
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
00092
00093
00094 T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc);
00095
00096 real T = Math::cbrt(T3);
00097
00098 u += T + (T != 0 ? r2 / T : 0);
00099 } else {
00100
00101 real ang = atan2(sqrt(-disc), -(S + r3));
00102
00103
00104 u += 2 * r * cos(ang / 3);
00105 }
00106 real
00107 v = sqrt(sq(u) + _e4a * q),
00108
00109
00110 uv = u < 0 ? _e4a * q / (v - u) : u + v,
00111
00112 w = max(real(0), _e2a * (uv - q) / (2 * v)),
00113
00114
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 {
00122
00123
00124
00125
00126
00127
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;
00133 h = - _a * (_f >= 0 ? _e2m : 1) * Math::hypot(xx, zz) / _e2a;
00134 }
00135 }
00136 lat = phi / Constants::degree();
00137
00138 lon = -atan2(-y, x) / Constants::degree();
00139 }
00140
00141 }