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 }