00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #include <GeographicLib/NormalGravity.hpp>
00011
00012 #if defined(_MSC_VER)
00013
00014 # pragma warning (disable: 4127)
00015 #endif
00016
00017 namespace GeographicLib {
00018
00019 using namespace std;
00020
00021 NormalGravity::NormalGravity(real a, real GM, real omega, real f, real J2)
00022 : _a(a)
00023 , _GM(GM)
00024 , _omega(omega)
00025 , _f(f)
00026 , _J2(J2)
00027 , _omega2(Math::sq(_omega))
00028 , _aomega2(Math::sq(_omega * _a))
00029 {
00030 if (!(Math::isfinite(_a) && _a > 0))
00031 throw GeographicErr("Major radius is not positive");
00032 if (!(Math::isfinite(_GM) && _GM > 0))
00033 throw GeographicErr("Gravitational constants is not positive");
00034 if (!(_omega == 0 && _f == 0 && _J2 == 0)) {
00035 bool flatp = _f > 0 && Math::isfinite(_f);
00036 if (_J2 > 0 && Math::isfinite(_J2) && flatp)
00037 throw GeographicErr("Cannot specify both f and J2");
00038 if (!(_J2 > 0 && Math::isfinite(_J2)) && !flatp)
00039 throw GeographicErr("Must specify one of f and J2");
00040 if (!(Math::isfinite(_omega) && _omega != 0))
00041 throw GeographicErr("Angular velocity is not non-zero");
00042 if (flatp)
00043 _J2 = FlatteningToJ2(a, GM, omega, f);
00044 else
00045 _f = J2ToFlattening(a, GM, omega, J2);
00046 }
00047 _e2 = _f * (2 - _f);
00048 _ep2 = _e2 / (1 - _e2);
00049 _q0 = qf(_ep2);
00050 _earth = Geocentric(_a, _f);
00051 _b = _a * (1 - _f);
00052 _E = a * sqrt(_e2);
00053 _U0 = _GM / _E * atan(sqrt(_ep2)) + _aomega2 / 3;
00054
00055
00056 _m = _aomega2 * _b / _GM;
00057 real
00058 Q = _m * sqrt(_ep2) * qpf(_ep2) / (3 * _q0),
00059 G = (1 - _m - Q / 2);
00060 _gammae = _GM / (_a * _b) * G;
00061 _gammap = _GM / (_a * _a) * (1 + Q);
00062
00063 _k = (_m + 3 * Q / 2 - _e2 * (1 + Q)) / G;
00064
00065 _fstar = (_m + 3 * Q / 2 - _f * (1 + Q)) / G;
00066 }
00067
00068 const NormalGravity& NormalGravity::WGS84() {
00069 static const NormalGravity wgs84(Constants::WGS84_a(),
00070 Constants::WGS84_GM(),
00071 Constants::WGS84_omega(),
00072 Constants::WGS84_f(), 0);
00073 return wgs84;
00074 }
00075
00076 const NormalGravity& NormalGravity::GRS80() {
00077 static const NormalGravity grs80(Constants::GRS80_a(),
00078 Constants::GRS80_GM(),
00079 Constants::GRS80_omega(),
00080 0, Constants::GRS80_J2());
00081 return grs80;
00082 }
00083
00084
00085 Math::real NormalGravity::atan7(real x) {
00086 if (abs(x) >= real(0.5)) {
00087 real y = sqrt(abs(x)), x2 = Math::sq(x);
00088 return ((x > 0 ? atan(y) : Math::atanh(y))- y * (1 - x / 3 + x2 / 5)) /
00089 (x * x2 * y);
00090 } else {
00091 real xn = -1, q = 0;
00092 for (int n = 7; ; n += 2) {
00093 real qn = q + xn / n;
00094 if (qn == q)
00095 break;
00096 q = qn;
00097 xn *= -x;
00098 }
00099 return q;
00100 }
00101 }
00102
00103
00104 Math::real NormalGravity::atan5(real x)
00105 { return 1/real(5) + x * atan7(x); }
00106
00107 Math::real NormalGravity::qf(real ep2) {
00108
00109
00110
00111
00112
00113
00114
00115 return sqrt(ep2) * ep2 * (3 * (3 + ep2) * atan5(ep2) - 1) / 6;
00116 }
00117
00118 Math::real NormalGravity::dq(real ep2) {
00119
00120
00121 return sqrt(ep2) * (5 - 3 * (1 + ep2) * (1 + 5 * ep2 * atan7(ep2))) /
00122 (10 * (1 + ep2));
00123 }
00124
00125 Math::real NormalGravity::qpf(real ep2) {
00126
00127
00128
00129
00130
00131
00132
00133 return ep2 * (1 - 3 * (1 + ep2) * atan5(ep2));
00134 }
00135
00136 Math::real NormalGravity::Jn(int n) const {
00137
00138 if (n & 1 || n < 0)
00139 return 0;
00140 n /= 2;
00141 real e2n = 1;
00142 for (int j = n; j--;)
00143 e2n *= -_e2;
00144 return
00145 -3 * e2n * ((1 - n) + 5 * n * _J2 / _e2) / ((2 * n + 1) * (2 * n + 3));
00146 }
00147
00148 Math::real NormalGravity::SurfaceGravity(real lat) const {
00149 real
00150 phi = lat * Math::degree(),
00151 sphi2 = abs(lat) == 90 ? 1 : Math::sq(sin(phi));
00152
00153 return _gammae * (1 + _k * sphi2) / sqrt(1 - _e2 * sphi2);
00154 }
00155
00156 Math::real NormalGravity::V0(real X, real Y, real Z,
00157 real& GammaX, real& GammaY, real& GammaZ)
00158 const {
00159
00160 real
00161 p = Math::hypot(X, Y),
00162 clam = p ? X/p : 1,
00163 slam = p ? Y/p : 0,
00164 r = Math::hypot(p, Z),
00165 Q = Math::sq(r) - Math::sq(_E),
00166 t2 = Math::sq(2 * _E * Z),
00167 disc = sqrt(Math::sq(Q) + t2),
00168
00169
00170 u = sqrt((Q >= 0 ? (Q + disc) : t2 / (disc - Q)) / 2),
00171 uE = Math::hypot(u, _E),
00172
00173 sbet = Z * uE,
00174 cbet = p * u,
00175 s = Math::hypot(cbet, sbet);
00176 cbet = s ? cbet/s : 0;
00177 sbet = s ? sbet/s : 1;
00178 real
00179 invw = uE / Math::hypot(u, _E * sbet),
00180 ep = _E/u,
00181 ep2 = Math::sq(ep),
00182 q = qf(ep2) / _q0,
00183 qp = qpf(ep2) / _q0,
00184
00185 Vres = (_GM / _E * atan(_E / u)
00186 + _aomega2 * q * (Math::sq(sbet) - 1/real(3)) / 2),
00187
00188 gamu = - invw * (_GM
00189 + (_aomega2 * _E * qp
00190 * (Math::sq(sbet) - 1/real(3)) / 2)) / Math::sq(uE),
00191 gamb = _aomega2 * q * sbet * cbet * invw / uE,
00192 t = u * invw / uE;
00193
00194 GammaX = t * cbet * gamu - invw * sbet * gamb;
00195 GammaY = GammaX * slam;
00196 GammaX *= clam;
00197 GammaZ = invw * sbet * gamu + t * cbet * gamb;
00198 return Vres;
00199 }
00200
00201 Math::real NormalGravity::Phi(real X, real Y, real& fX, real& fY)
00202 const {
00203 fX = _omega2 * X;
00204 fY = _omega2 * Y;
00205
00206 return _omega2 * (Math::sq(X) + Math::sq(Y)) / 2;
00207 }
00208
00209 Math::real NormalGravity::U(real X, real Y, real Z,
00210 real& gammaX, real& gammaY, real& gammaZ)
00211 const {
00212 real fX, fY;
00213 real Ures = V0(X, Y, Z, gammaX, gammaY, gammaZ) + Phi(X, Y, fX, fY);
00214 gammaX += fX;
00215 gammaY += fY;
00216 return Ures;
00217 }
00218
00219 Math::real NormalGravity::Gravity(real lat, real h,
00220 real& gammay, real& gammaz)
00221 const {
00222 real X, Y, Z;
00223 real M[Geocentric::dim2_];
00224 _earth.IntForward(lat, 0, h, X, Y, Z, M);
00225 real gammaX, gammaY, gammaZ,
00226 Ures = U(X, Y, Z, gammaX, gammaY, gammaZ);
00227
00228 gammay = M[1] * gammaX + M[4] * gammaY + M[7] * gammaZ;
00229 gammaz = M[2] * gammaX + M[5] * gammaY + M[8] * gammaZ;
00230 return Ures;
00231 }
00232
00233 Math::real NormalGravity::J2ToFlattening(real a, real GM,
00234 real omega, real J2) {
00235 real
00236 K = 2 * Math::sq(a * omega) * a / (15 * GM),
00237 e2 = 3 * J2;
00238
00239 for (int j = 0; j < maxit_ || GEOGRAPHICLIB_PANIC; ++j) {
00240 real e2a = e2,
00241 ep2 = e2 / (1 - e2),
00242 q0 = qf(ep2),
00243 dq0 = dq(ep2) / Math::sq(1 - e2),
00244 h = e2 * (1 - sqrt(e2) * K / q0) - 3 * J2,
00245 dh = 1 - sqrt(e2) * K * (3 * q0 - 2 * e2 * dq0) / (2 * Math::sq(q0)),
00246 de2 = - h / dh;
00247 e2 = e2a + de2;
00248 if (e2 == e2a)
00249 break;
00250 }
00251 return e2 / (1 + sqrt(1 - e2));
00252 }
00253
00254 Math::real NormalGravity::FlatteningToJ2(real a, real GM,
00255 real omega, real f) {
00256 real
00257 K = 2 * Math::sq(a * omega) * a / (15 * GM),
00258 e2 = f * (2 - f),
00259 q0 = qf(e2 / (1 - e2));
00260 return e2 * (1 - K * sqrt(e2) / q0) / 3;
00261 }
00262
00263 }