GeographicLib: NormalGravity.cpp Source File

GeographicLib 2.5.2

Loading...
Searching...
No Matches
NormalGravity.cpp
Go to the documentation of this file.
1/**
2 * \file NormalGravity.cpp
3 * \brief Implementation for GeographicLib::NormalGravity class
4 *
5 * Copyright (c) Charles Karney (2011-2022) <karney@alum.mit.edu> and licensed
6 * under the MIT/X11 License. For more information, see
7 * https://geographiclib.sourceforge.io/
8 **********************************************************************/
9
10#include <GeographicLib/NormalGravity.hpp>
11
12namespace GeographicLib {
13
14 using namespace std;
15
16 void NormalGravity::Initialize(real a, real GM, real omega, real f_J2,
17 bool geometricp) {
18 _a = a;
19 if (!(isfinite(_a) && _a > 0))
20 throw GeographicErr("Equatorial radius is not positive");
21 _gGM = GM;
22 if (!isfinite(_gGM))
23 throw GeographicErr("Gravitational constant is not finite");
24 _omega = omega;
25 _omega2 = Math::sq(_omega);
26 _aomega2 = Math::sq(_omega * _a);
27 if (!(isfinite(_omega2) && isfinite(_aomega2)))
28 throw GeographicErr("Rotation velocity is not finite");
29 _f = geometricp ? f_J2 : J2ToFlattening(_a, _gGM, _omega, f_J2);
30 _b = _a * (1 - _f);
31 if (!(isfinite(_b) && _b > 0))
32 throw GeographicErr("Polar semi-axis is not positive");
33 _jJ2 = geometricp ? FlatteningToJ2(_a, _gGM, _omega, f_J2) : f_J2;
34 _e2 = _f * (2 - _f);
35 _ep2 = _e2 / (1 - _e2);
36 real ex2 = _f < 0 ? -_e2 : _ep2;
37 _qQ0 = Qf(ex2, _f < 0);
38 _earth = Geocentric(_a, _f);
39 _eE = _a * sqrt(fabs(_e2)); // H+M, Eq 2-54
40 // H+M, Eq 2-61
41 _uU0 = _gGM * atanzz(ex2, _f < 0) / _b + _aomega2 / 3;
42 real P = Hf(ex2, _f < 0) / (6 * _qQ0);
43 // H+M, Eq 2-73
44 _gammae = _gGM / (_a * _b) - (1 + P) * _a * _omega2;
45 // H+M, Eq 2-74
46 _gammap = _gGM / (_a * _a) + 2 * P * _b * _omega2;
47 // k = gammae * (b * gammap / (a * gammae) - 1)
48 // = (b * gammap - a * gammae) / a
49 _k = -_e2 * _gGM / (_a * _b) +
50 _omega2 * (P * (_a + 2 * _b * (1 - _f)) + _a);
51 // f* = (gammap - gammae) / gammae
52 _fstar = (-_f * _gGM / (_a * _b) + _omega2 * (P * (_a + 2 * _b) + _a)) /
53 _gammae;
54 }
55
56 NormalGravity::NormalGravity(real a, real GM, real omega, real f_J2,
57 bool geometricp) {
58 Initialize(a, GM, omega, f_J2, geometricp);
59 }
60
61 const NormalGravity& NormalGravity::WGS84() {
62 static const NormalGravity wgs84(Constants::WGS84_a(),
63 Constants::WGS84_GM(),
64 Constants::WGS84_omega(),
65 Constants::WGS84_f(), true);
66 return wgs84;
67 }
68
69 const NormalGravity& NormalGravity::GRS80() {
70 static const NormalGravity grs80(Constants::GRS80_a(),
71 Constants::GRS80_GM(),
72 Constants::GRS80_omega(),
73 Constants::GRS80_J2(), false);
74 return grs80;
75 }
76
77 Math::real NormalGravity::atan7series(real x) {
78 // compute -sum( (-x)^n/(2*n+7), n, 0, inf)
79 // = -1/7 + x/9 - x^2/11 + x^3/13 ...
80 // = (atan(sqrt(x))/sqrt(x)-(1-x/3+x^2/5)) / x^3 (x > 0)
81 // = (atanh(sqrt(-x))/sqrt(-x)-(1-x/3+x^2/5)) / x^3 (x < 0)
82 // require abs(x) < 1/2, but better to restrict calls to abs(x) < 1/4
83 static const real lg2eps_ = -log2(numeric_limits<real>::epsilon() / 2);
84 int e;
85 (void) frexp(x, &e);
86 e = max(-e, 1); // Here's where abs(x) < 1/2 is assumed
87 // x = [0.5,1) * 2^(-e)
88 // estimate n s.t. x^n/n < 1/7 * epsilon/2
89 // a stronger condition is x^n < epsilon/2
90 // taking log2 of both sides, a stronger condition is n*(-e) < -lg2eps;
91 // or n*e > lg2eps or n > ceiling(lg2eps/e)
92 int n = x == 0 ? 1 : int(ceil(lg2eps_ / e));
93 Math::real v = 0;
94 while (n--) // iterating from n-1 down to 0
95 v = - x * v - 1/Math::real(2*n + 7);
96 return v;
97 }
98
99 Math::real NormalGravity::atan5series(real x) {
100 // Compute Taylor series approximations to
101 // (atan(z)-(z-z^3/3))/z^5,
102 // z = sqrt(x)
103 // require abs(x) < 1/2, but better to restrict calls to abs(x) < 1/4
104 return 1/real(5) + x * atan7series(x);
105 }
106
107 Math::real NormalGravity::Qf(real x, bool alt) {
108 // Compute
109 // Q(z) = (((1 + 3/z^2) * atan(z) - 3/z)/2) / z^3
110 // = q(z)/z^3 with q(z) defined by H+M, Eq 2-57 with z = E/u
111 // z = sqrt(x)
112 real y = alt ? -x / (1 + x) : x;
113 return !(4 * fabs(y) < 1) ? // Backwards test to allow NaNs through
114 ((1 + 3/y) * atanzz(x, alt) - 3/y) / (2 * y) :
115 (3 * (3 + y) * atan5series(y) - 1) / 6;
116 }
117
118 Math::real NormalGravity::Hf(real x, bool alt) {
119 // z = sqrt(x)
120 // Compute
121 // H(z) = (3*Q(z)+z*diff(Q(z),z))*(1+z^2)
122 // = (3 * (1 + 1/z^2) * (1 - atan(z)/z) - 1) / z^2
123 // = q'(z)/z^2, with q'(z) defined by H+M, Eq 2-67, with z = E/u
124 real y = alt ? -x / (1 + x) : x;
125 return !(4 * fabs(y) < 1) ? // Backwards test to allow NaNs through
126 (3 * (1 + 1/y) * (1 - atanzz(x, alt)) - 1) / y :
127 1 - 3 * (1 + y) * atan5series(y);
128 }
129
130 Math::real NormalGravity::QH3f(real x, bool alt) {
131 // z = sqrt(x)
132 // (Q(z) - H(z)/3) / z^2
133 // = - (1+z^2)/(3*z) * d(Q(z))/dz - Q(z)
134 // = ((15+9*z^2)*atan(z)-4*z^3-15*z)/(6*z^7)
135 // = ((25+15*z^2)*atan7+3)/10
136 real y = alt ? -x / (1 + x) : x;
137 return !(4 * fabs(y) < 1) ? // Backwards test to allow NaNs through
138 ((9 + 15/y) * atanzz(x, alt) - 4 - 15/y) / (6 * Math::sq(y)) :
139 ((25 + 15*y) * atan7series(y) + 3)/10;
140 }
141
142 Math::real NormalGravity::Jn(int n) const {
143 // Note Jn(0) = -1; Jn(2) = _jJ2; Jn(odd) = 0
144 if (n & 1 || n < 0)
145 return 0;
146 n /= 2;
147 real e2n = 1; // Perhaps this should just be e2n = pow(-_e2, n);
148 for (int j = n; j--;)
149 e2n *= -_e2;
150 return // H+M, Eq 2-92
151 -3 * e2n * ((1 - n) + 5 * n * _jJ2 / _e2) / ((2 * n + 1) * (2 * n + 3));
152 }
153
154 Math::real NormalGravity::SurfaceGravity(real lat) const {
155 real sphi = Math::sind(Math::LatFix(lat));
156 // H+M, Eq 2-78
157 return (_gammae + _k * Math::sq(sphi)) / sqrt(1 - _e2 * Math::sq(sphi));
158 }
159
160 Math::real NormalGravity::V0(real X, real Y, real Z,
161 real& GammaX, real& GammaY, real& GammaZ) const
162 {
163 // See H+M, Sec 6-2
164 real
165 p = hypot(X, Y),
166 clam = p != 0 ? X/p : 1,
167 slam = p != 0 ? Y/p : 0,
168 r = hypot(p, Z);
169 if (_f < 0) swap(p, Z);
170 real
171 Q = Math::sq(r) - Math::sq(_eE),
172 t2 = Math::sq(2 * _eE * Z),
173 disc = sqrt(Math::sq(Q) + t2),
174 // This is H+M, Eq 6-8a, but generalized to deal with Q negative
175 // accurately.
176 u = sqrt((Q >= 0 ? (Q + disc) : t2 / (disc - Q)) / 2),
177 uE = hypot(u, _eE),
178 // H+M, Eq 6-8b
179 sbet = u != 0 ? Z * uE : copysign(sqrt(-Q), Z),
180 cbet = u != 0 ? p * u : p,
181 s = hypot(cbet, sbet);
182 sbet = s != 0 ? sbet/s : 1;
183 cbet = s != 0 ? cbet/s : 0;
184 real
185 z = _eE/u,
186 z2 = Math::sq(z),
187 den = hypot(u, _eE * sbet);
188 if (_f < 0) {
189 swap(sbet, cbet);
190 swap(u, uE);
191 }
192 real
193 invw = uE / den, // H+M, Eq 2-63
194 bu = _b / (u != 0 || _f < 0 ? u : _eE),
195 // Qf(z2->inf, false) = pi/(4*z^3)
196 q = ((u != 0 || _f < 0 ? Qf(z2, _f < 0) : Math::pi() / 4) / _qQ0) *
197 bu * Math::sq(bu),
198 qp = _b * Math::sq(bu) * (u != 0 || _f < 0 ? Hf(z2, _f < 0) : 2) / _qQ0,
199 ang = (Math::sq(sbet) - 1/real(3)) / 2,
200 // H+M, Eqs 2-62 + 6-9, but omitting last (rotational) term.
201 Vres = _gGM * (u != 0 || _f < 0 ?
202 atanzz(z2, _f < 0) / u :
203 Math::pi() / (2 * _eE)) + _aomega2 * q * ang,
204 // H+M, Eq 6-10
205 gamu = - (_gGM + (_aomega2 * qp * ang)) * invw / Math::sq(uE),
206 gamb = _aomega2 * q * sbet * cbet * invw / uE,
207 t = u * invw / uE,
208 gamp = t * cbet * gamu - invw * sbet * gamb;
209 // H+M, Eq 6-12
210 GammaX = gamp * clam;
211 GammaY = gamp * slam;
212 GammaZ = invw * sbet * gamu + t * cbet * gamb;
213 return Vres;
214 }
215
216 Math::real NormalGravity::Phi(real X, real Y, real& fX, real& fY) const {
217 fX = _omega2 * X;
218 fY = _omega2 * Y;
219 // N.B. fZ = 0;
220 return _omega2 * (Math::sq(X) + Math::sq(Y)) / 2;
221 }
222
223 Math::real NormalGravity::U(real X, real Y, real Z,
224 real& gammaX, real& gammaY, real& gammaZ) const {
225 real fX, fY;
226 real Ures = V0(X, Y, Z, gammaX, gammaY, gammaZ) + Phi(X, Y, fX, fY);
227 gammaX += fX;
228 gammaY += fY;
229 return Ures;
230 }
231
232 Math::real NormalGravity::Gravity(real lat, real h,
233 real& gammay, real& gammaz) const {
234 real X, Y, Z;
235 real M[Geocentric::dim2_];
236 _earth.IntForward(lat, 0, h, X, Y, Z, M);
237 real gammaX, gammaY, gammaZ,
238 Ures = U(X, Y, Z, gammaX, gammaY, gammaZ);
239 // gammax = M[0] * gammaX + M[3] * gammaY + M[6] * gammaZ;
240 gammay = M[1] * gammaX + M[4] * gammaY + M[7] * gammaZ;
241 gammaz = M[2] * gammaX + M[5] * gammaY + M[8] * gammaZ;
242 return Ures;
243 }
244
245 Math::real NormalGravity::J2ToFlattening(real a, real GM,
246 real omega, real J2) {
247 // Solve
248 // f = e^2 * (1 - K * e/q0) - 3 * J2 = 0
249 // for e^2 using Newton's method
250 static const real maxe_ = 1 - numeric_limits<real>::epsilon();
251 static const real eps2_ = sqrt(numeric_limits<real>::epsilon()) / 100;
252 real
253 K = 2 * Math::sq(a * omega) * a / (15 * GM),
254 J0 = (1 - 4 * K / Math::pi()) / 3;
255 if (!(GM > 0 && isfinite(K) && K >= 0))
256 return Math::NaN();
257 if (!(isfinite(J2) && J2 <= J0)) return Math::NaN();
258 if (J2 == J0) return 1;
259 // Solve e2 - f1 * f2 * K / Q0 - 3 * J2 = 0 for J2 close to J0;
260 // subst e2 = ep2/(1+ep2), f2 = 1/(1+ep2), f1 = 1/sqrt(1+ep2), J2 = J0-dJ2,
261 // Q0 = pi/(4*z^3) - 2/z^4 + (3*pi)/(4*z^5), z = sqrt(ep2), and balance two
262 // leading terms to give
263 real
264 ep2 = fmax(Math::sq(32 * K / (3 * Math::sq(Math::pi()) * (J0 - J2))),
265 -maxe_),
266 e2 = fmin(ep2 / (1 + ep2), maxe_);
267 for (int j = 0;
268 j < maxit_ ||
269 GEOGRAPHICLIB_PANIC("Convergence failure in NormalGravity");
270 ++j) {
271 real
272 e2a = e2, ep2a = ep2,
273 f2 = 1 - e2, // (1 - f)^2
274 f1 = sqrt(f2), // (1 - f)
275 Q0 = Qf(e2 < 0 ? -e2 : ep2, e2 < 0),
276 h = e2 - f1 * f2 * K / Q0 - 3 * J2,
277 dh = 1 - 3 * f1 * K * QH3f(e2 < 0 ? -e2 : ep2, e2 < 0) /
278 (2 * Math::sq(Q0));
279 e2 = fmin(e2a - h / dh, maxe_);
280 ep2 = fmax(e2 / (1 - e2), -maxe_);
281 if (fabs(h) < eps2_ || e2 == e2a || ep2 == ep2a)
282 break;
283 }
284 return e2 / (1 + sqrt(1 - e2));
285 }
286
287 Math::real NormalGravity::FlatteningToJ2(real a, real GM,
288 real omega, real f) {
289 real
290 K = 2 * Math::sq(a * omega) * a / (15 * GM),
291 f1 = 1 - f,
292 f2 = Math::sq(f1),
293 e2 = f * (2 - f);
294 // H+M, Eq 2-90 + 2-92'
295 return (e2 - K * f1 * f2 / Qf(f < 0 ? -e2 : e2 / f2, f < 0)) / 3;
296 }
297
298} // namespace GeographicLib
real
GeographicLib::Math::real real
Definition GeodSolve.cpp:28
GEOGRAPHICLIB_PANIC
#define GEOGRAPHICLIB_PANIC(msg)
Definition Math.hpp:67
NormalGravity.hpp
Header for GeographicLib::NormalGravity class.
GeographicLib::Constants::GRS80_J2
static T GRS80_J2()
Definition Constants.hpp:193
GeographicLib::Constants::GRS80_GM
static T GRS80_GM()
Definition Constants.hpp:172
GeographicLib::Constants::WGS84_omega
static T WGS84_omega()
Definition Constants.hpp:159
GeographicLib::Constants::GRS80_a
static T GRS80_a()
Definition Constants.hpp:165
GeographicLib::Constants::GRS80_omega
static T GRS80_omega()
Definition Constants.hpp:186
GeographicLib::Constants::WGS84_f
static T WGS84_f()
Definition Constants.hpp:141
GeographicLib::Constants::WGS84_a
static T WGS84_a()
Definition Constants.hpp:135
GeographicLib::Constants::WGS84_GM
static T WGS84_GM()
Definition Constants.hpp:152
GeographicLib::Math::LatFix
static T LatFix(T x)
Definition Math.hpp:315
GeographicLib::Math::sq
static T sq(T x)
Definition Math.hpp:227
GeographicLib::Math::sind
static T sind(T x)
Definition Math.cpp:160
GeographicLib::Math::pi
static T pi()
Definition Math.hpp:205
GeographicLib::Math::NaN
static T NaN()
Definition Math.cpp:283
GeographicLib::Math::real
double real
Definition Math.hpp:115
GeographicLib::NormalGravity::V0
Math::real V0(real X, real Y, real Z, real &GammaX, real &GammaY, real &GammaZ) const
Definition NormalGravity.cpp:160
GeographicLib::NormalGravity::NormalGravity
NormalGravity()
Definition NormalGravity.hpp:152
GeographicLib::NormalGravity::FlatteningToJ2
static Math::real FlatteningToJ2(real a, real GM, real omega, real f)
Definition NormalGravity.cpp:287
GeographicLib::NormalGravity::Phi
Math::real Phi(real X, real Y, real &fX, real &fY) const
Definition NormalGravity.cpp:216
GeographicLib::NormalGravity::WGS84
static const NormalGravity & WGS84()
Definition NormalGravity.cpp:61
GeographicLib::NormalGravity::J2ToFlattening
static Math::real J2ToFlattening(real a, real GM, real omega, real J2)
Definition NormalGravity.cpp:245
GeographicLib::NormalGravity::NormalGravity
NormalGravity(real a, real GM, real omega, real f_J2, bool geometricp=true)
Definition NormalGravity.cpp:56
GeographicLib::NormalGravity::U
Math::real U(real X, real Y, real Z, real &gammaX, real &gammaY, real &gammaZ) const
Definition NormalGravity.cpp:223
GeographicLib::NormalGravity::SurfaceGravity
Math::real SurfaceGravity(real lat) const
Definition NormalGravity.cpp:154
GeographicLib::NormalGravity::GRS80
static const NormalGravity & GRS80()
Definition NormalGravity.cpp:69
GeographicLib::NormalGravity::Gravity
Math::real Gravity(real lat, real h, real &gammay, real &gammaz) const
Definition NormalGravity.cpp:232
GeographicLib
Namespace for GeographicLib.
Definition Accumulator.cpp:12
std::swap
void swap(GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &a, GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &b)
Definition NearestNeighbor.hpp:819