00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #include <GeographicLib/Geocentric.hpp>
00011
00012 namespace GeographicLib {
00013
00014 using namespace std;
00015
00016 Geocentric::Geocentric(real a, real f)
00017 : _a(a)
00018 , _f(f <= 1 ? f : 1/f)
00019 , _e2(_f * (2 - _f))
00020 , _e2m(Math::sq(1 - _f))
00021 , _e2a(abs(_e2))
00022 , _e4a(Math::sq(_e2))
00023 , _maxrad(2 * _a / numeric_limits<real>::epsilon())
00024 {
00025 if (!(Math::isfinite(_a) && _a > 0))
00026 throw GeographicErr("Major radius is not positive");
00027 if (!(Math::isfinite(_f) && _f < 1))
00028 throw GeographicErr("Minor radius is not positive");
00029 }
00030
00031 const Geocentric& Geocentric::WGS84() {
00032 static const Geocentric wgs84(Constants::WGS84_a(), Constants::WGS84_f());
00033 return wgs84;
00034 }
00035
00036 void Geocentric::IntForward(real lat, real lon, real h,
00037 real& X, real& Y, real& Z,
00038 real M[dim2_]) const {
00039 lon = Math::AngNormalize(lon);
00040 real
00041 phi = lat * Math::degree(),
00042 lam = lon * Math::degree(),
00043 sphi = sin(phi),
00044 cphi = abs(lat) == 90 ? 0 : cos(phi),
00045 n = _a/sqrt(1 - _e2 * Math::sq(sphi)),
00046 slam = lon == -180 ? 0 : sin(lam),
00047 clam = abs(lon) == 90 ? 0 : cos(lam);
00048 Z = ( _e2m * n + h) * sphi;
00049 X = (n + h) * cphi;
00050 Y = X * slam;
00051 X *= clam;
00052 if (M)
00053 Rotation(sphi, cphi, slam, clam, M);
00054 }
00055
00056 void Geocentric::IntReverse(real X, real Y, real Z,
00057 real& lat, real& lon, real& h,
00058 real M[dim2_]) const {
00059 real
00060 R = Math::hypot(X, Y),
00061 slam = R ? Y / R : 0,
00062 clam = R ? X / R : 1;
00063 h = Math::hypot(R, Z);
00064 real sphi, cphi;
00065 if (h > _maxrad) {
00066
00067
00068
00069
00070
00071
00072 R = Math::hypot(X/2, Y/2);
00073 slam = R ? (Y/2) / R : 0;
00074 clam = R ? (X/2) / R : 1;
00075 real H = Math::hypot(Z/2, R);
00076 sphi = (Z/2) / H;
00077 cphi = R / H;
00078 } else if (_e4a == 0) {
00079
00080
00081
00082 real H = Math::hypot(h == 0 ? 1 : Z, R);
00083 sphi = (h == 0 ? 1 : Z) / H;
00084 cphi = R / H;
00085 h -= _a;
00086 } else {
00087
00088
00089 real
00090 p = Math::sq(R / _a),
00091 q = _e2m * Math::sq(Z / _a),
00092 r = (p + q - _e4a) / 6;
00093 if (_f < 0) swap(p, q);
00094 if ( !(_e4a * q == 0 && r <= 0) ) {
00095 real
00096
00097
00098 S = _e4a * p * q / 4,
00099 r2 = Math::sq(r),
00100 r3 = r * r2,
00101 disc = S * (2 * r3 + S);
00102 real u = r;
00103 if (disc >= 0) {
00104 real T3 = S + r3;
00105
00106
00107
00108 T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc);
00109
00110 real T = Math::cbrt(T3);
00111
00112 u += T + (T ? r2 / T : 0);
00113 } else {
00114
00115 real ang = atan2(sqrt(-disc), -(S + r3));
00116
00117
00118 u += 2 * r * cos(ang / 3);
00119 }
00120 real
00121 v = sqrt(Math::sq(u) + _e4a * q),
00122
00123
00124 uv = u < 0 ? _e4a * q / (v - u) : u + v,
00125
00126 w = max(real(0), _e2a * (uv - q) / (2 * v)),
00127
00128
00129 k = uv / (sqrt(uv + Math::sq(w)) + w),
00130 k1 = _f >= 0 ? k : k - _e2,
00131 k2 = _f >= 0 ? k + _e2 : k,
00132 d = k1 * R / k2,
00133 H = Math::hypot(Z/k1, R/k2);
00134 sphi = (Z/k1) / H;
00135 cphi = (R/k2) / H;
00136 h = (1 - _e2m/k1) * Math::hypot(d, Z);
00137 } else {
00138
00139
00140
00141
00142
00143
00144 real
00145 zz = sqrt((_f >= 0 ? _e4a - p : p) / _e2m),
00146 xx = sqrt( _f < 0 ? _e4a - p : p ),
00147 H = Math::hypot(zz, xx);
00148 sphi = zz / H;
00149 cphi = xx / H;
00150 if (Z < 0) sphi = -sphi;
00151 h = - _a * (_f >= 0 ? _e2m : 1) * H / _e2a;
00152 }
00153 }
00154 lat = atan2(sphi, cphi) / Math::degree();
00155
00156 lon = -atan2(-slam, clam) / Math::degree();
00157 if (M)
00158 Rotation(sphi, cphi, slam, clam, M);
00159 }
00160
00161 void Geocentric::Rotation(real sphi, real cphi, real slam, real clam,
00162 real M[dim2_]) {
00163
00164
00165
00166
00167
00168
00169
00170
00171 M[0] = -slam; M[3] = clam; M[6] = 0;
00172
00173 M[1] = -clam * sphi; M[4] = -slam * sphi; M[7] = cphi;
00174
00175 M[2] = clam * cphi; M[5] = slam * cphi; M[8] = sphi;
00176 }
00177
00178 }