src/NormalGravity.cpp
Go to the documentation of this file.
1 
11 
12 #if defined(_MSC_VER)
13 // Squelch warnings about constant conditional expressions
14 # pragma warning (disable: 4127)
15 #endif
16 
17 namespace GeographicLib {
18 
19  using namespace std;
20 
22  bool geometricp) {
23  _a = a;
24  if (!(Math::isfinite(_a) && _a > 0))
25  throw GeographicErr("Equatorial radius is not positive");
26  _GM = GM;
27  if (!Math::isfinite(_GM))
28  throw GeographicErr("Gravitational constant is not finite");
29  _omega = omega;
30  _omega2 = Math::sq(_omega);
31  _aomega2 = Math::sq(_omega * _a);
32  if (!(Math::isfinite(_omega2) && Math::isfinite(_aomega2)))
33  throw GeographicErr("Rotation velocity is not finite");
34  _f = geometricp ? f_J2 : J2ToFlattening(_a, _GM, _omega, f_J2);
35  _b = _a * (1 - _f);
36  if (!(Math::isfinite(_b) && _b > 0))
37  throw GeographicErr("Polar semi-axis is not positive");
38  _J2 = geometricp ? FlatteningToJ2(_a, _GM, _omega, f_J2) : f_J2;
39  _e2 = _f * (2 - _f);
40  _ep2 = _e2 / (1 - _e2);
41  real ex2 = _f < 0 ? -_e2 : _ep2;
42  _Q0 = Qf(ex2, _f < 0);
43  _earth = Geocentric(_a, _f);
44  _E = _a * sqrt(abs(_e2)); // H+M, Eq 2-54
45  // H+M, Eq 2-61
46  _U0 = _GM * atanzz(ex2, _f < 0) / _b + _aomega2 / 3;
47  real P = Hf(ex2, _f < 0) / (6 * _Q0);
48  // H+M, Eq 2-73
49  _gammae = _GM / (_a * _b) - (1 + P) * _a * _omega2;
50  // H+M, Eq 2-74
51  _gammap = _GM / (_a * _a) + 2 * P * _b * _omega2;
52  // k = gammae * (b * gammap / (a * gammae) - 1)
53  // = (b * gammap - a * gammae) / a
54  _k = -_e2 * _GM / (_a * _b) +
55  _omega2 * (P * (_a + 2 * _b * (1 - _f)) + _a);
56  // f* = (gammap - gammae) / gammae
57  _fstar = (-_f * _GM / (_a * _b) + _omega2 * (P * (_a + 2 * _b) + _a)) /
58  _gammae;
59  }
60 
62  bool geometricp) {
63  Initialize(a, GM, omega, f_J2, geometricp);
64  }
65 
67  if (!(Math::isfinite(GM) && GM > 0))
68  throw GeographicErr("Gravitational constant is not positive");
69  bool geometricp;
70  if (!(omega == 0 && f == 0 && J2 == 0)) {
71  geometricp = f > 0 && Math::isfinite(f);
72  if (J2 > 0 && Math::isfinite(J2) && geometricp)
73  throw GeographicErr("Cannot specify both f and J2");
74  if (!(J2 > 0 && Math::isfinite(J2)) && !geometricp)
75  throw GeographicErr("Must specify one of f and J2");
76  if (!(Math::isfinite(omega) && omega != 0))
77  throw GeographicErr("Angular velocity is not non-zero");
78  } else
79  geometricp = true;
80  Initialize(a, GM, omega, geometricp ? f : J2, geometricp);
81  }
82 
84  static const NormalGravity wgs84(Constants::WGS84_a(),
87  Constants::WGS84_f(), true);
88  return wgs84;
89  }
90 
92  static const NormalGravity grs80(Constants::GRS80_a(),
95  Constants::GRS80_J2(), false);
96  return grs80;
97  }
98 
100  // compute -sum( (-x)^n/(2*n+7), n, 0, inf)
101  // = -1/7 + x/9 - x^2/11 + x^3/13 ...
102  // = (atan(sqrt(x))/sqrt(x)-(1-x/3+x^2/5)) / x^3 (x > 0)
103  // = (atanh(sqrt(-x))/sqrt(-x)-(1-x/3+x^2/5)) / x^3 (x < 0)
104  // require abs(x) < 1/2, but better to restrict calls to abs(x) < 1/4
105  static const real lg2eps_ =
107  int e;
108  frexp(x, &e);
109  e = max(-e, 1); // Here's where abs(x) < 1/2 is assumed
110  // x = [0.5,1) * 2^(-e)
111  // estimate n s.t. x^n/n < 1/7 * epsilon/2
112  // a stronger condition is x^n < epsilon/2
113  // taking log2 of both sides, a stronger condition is n*(-e) < -lg2eps;
114  // or n*e > lg2eps or n > ceiling(lg2eps/e)
115  int n = int(ceil(lg2eps_ / e));
116  Math::real v = 0;
117  while (n--) // iterating from n-1 down to 0
118  v = - x * v - 1/Math::real(2*n + 7);
119  return v;
120  }
121 
123  // Compute Taylor series approximations to
124  // (atan(z)-(z-z^3/3))/z^5,
125  // z = sqrt(x)
126  // require abs(x) < 1/2, but better to restrict calls to abs(x) < 1/4
127  return 1/real(5) + x * atan7series(x);
128  }
129 
131  // Compute
132  // Q(z) = (((1 + 3/z^2) * atan(z) - 3/z)/2) / z^3
133  // = q(z)/z^3 with q(z) defined by H+M, Eq 2-57 with z = E/u
134  // z = sqrt(x)
135  real y = alt ? -x / (1 + x) : x;
136  return !(4 * abs(y) < 1) ? // Backwards test to allow NaNs through
137  ((1 + 3/y) * atanzz(x, alt) - 3/y) / (2 * y) :
138  (3 * (3 + y) * atan5series(y) - 1) / 6;
139  }
140 
142  // z = sqrt(x)
143  // Compute
144  // H(z) = (3*Q(z)+z*diff(Q(z),z))*(1+z^2)
145  // = (3 * (1 + 1/z^2) * (1 - atan(z)/z) - 1) / z^2
146  // = q'(z)/z^2, with q'(z) defined by H+M, Eq 2-67, with z = E/u
147  real y = alt ? -x / (1 + x) : x;
148  return !(4 * abs(y) < 1) ? // Backwards test to allow NaNs through
149  (3 * (1 + 1/y) * (1 - atanzz(x, alt)) - 1) / y :
150  1 - 3 * (1 + y) * atan5series(y);
151  }
152 
154  // z = sqrt(x)
155  // (Q(z) - H(z)/3) / z^2
156  // = - (1+z^2)/(3*z) * d(Q(z))/dz - Q(z)
157  // = ((15+9*z^2)*atan(z)-4*z^3-15*z)/(6*z^7)
158  // = ((25+15*z^2)*atan7+3)/10
159  real y = alt ? -x / (1 + x) : x;
160  return !(4 * abs(y) < 1) ? // Backwards test to allow NaNs through
161  ((9 + 15/y) * atanzz(x, alt) - 4 - 15/y) / (6 * Math::sq(y)) :
162  ((25 + 15*y) * atan7series(y) + 3)/10;
163  }
164 
166  // Note Jn(0) = -1; Jn(2) = _J2; Jn(odd) = 0
167  if (n & 1 || n < 0)
168  return 0;
169  n /= 2;
170  real e2n = 1; // Perhaps this should just be e2n = pow(-_e2, n);
171  for (int j = n; j--;)
172  e2n *= -_e2;
173  return // H+M, Eq 2-92
174  -3 * e2n * ((1 - n) + 5 * n * _J2 / _e2) / ((2 * n + 1) * (2 * n + 3));
175  }
176 
178  real sphi = Math::sind(Math::LatFix(lat));
179  // H+M, Eq 2-78
180  return (_gammae + _k * Math::sq(sphi)) / sqrt(1 - _e2 * Math::sq(sphi));
181  }
182 
184  real& GammaX, real& GammaY, real& GammaZ) const
185  {
186  // See H+M, Sec 6-2
187  real
188  p = Math::hypot(X, Y),
189  clam = p != 0 ? X/p : 1,
190  slam = p != 0 ? Y/p : 0,
191  r = Math::hypot(p, Z);
192  if (_f < 0) swap(p, Z);
193  real
194  Q = Math::sq(r) - Math::sq(_E),
195  t2 = Math::sq(2 * _E * Z),
196  disc = sqrt(Math::sq(Q) + t2),
197  // This is H+M, Eq 6-8a, but generalized to deal with Q negative
198  // accurately.
199  u = sqrt((Q >= 0 ? (Q + disc) : t2 / (disc - Q)) / 2),
200  uE = Math::hypot(u, _E),
201  // H+M, Eq 6-8b
202  sbet = u != 0 ? Z * uE : Math::copysign(sqrt(-Q), Z),
203  cbet = u != 0 ? p * u : p,
204  s = Math::hypot(cbet, sbet);
205  sbet = s != 0 ? sbet/s : 1;
206  cbet = s != 0 ? cbet/s : 0;
207  real
208  z = _E/u,
209  z2 = Math::sq(z),
210  den = Math::hypot(u, _E * sbet);
211  if (_f < 0) {
212  swap(sbet, cbet);
213  swap(u, uE);
214  }
215  real
216  invw = uE / den, // H+M, Eq 2-63
217  bu = _b / (u != 0 || _f < 0 ? u : _E),
218  // Qf(z2->inf, false) = pi/(4*z^3)
219  q = ((u != 0 || _f < 0 ? Qf(z2, _f < 0) : Math::pi() / 4) / _Q0) *
220  bu * Math::sq(bu),
221  qp = _b * Math::sq(bu) * (u != 0 || _f < 0 ? Hf(z2, _f < 0) : 2) / _Q0,
222  ang = (Math::sq(sbet) - 1/real(3)) / 2,
223  // H+M, Eqs 2-62 + 6-9, but omitting last (rotational) term.
224  Vres = _GM * (u != 0 || _f < 0 ?
225  atanzz(z2, _f < 0) / u :
226  Math::pi() / (2 * _E)) + _aomega2 * q * ang,
227  // H+M, Eq 6-10
228  gamu = - (_GM + (_aomega2 * qp * ang)) * invw / Math::sq(uE),
229  gamb = _aomega2 * q * sbet * cbet * invw / uE,
230  t = u * invw / uE,
231  gamp = t * cbet * gamu - invw * sbet * gamb;
232  // H+M, Eq 6-12
233  GammaX = gamp * clam;
234  GammaY = gamp * slam;
235  GammaZ = invw * sbet * gamu + t * cbet * gamb;
236  return Vres;
237  }
238 
240  fX = _omega2 * X;
241  fY = _omega2 * Y;
242  // N.B. fZ = 0;
243  return _omega2 * (Math::sq(X) + Math::sq(Y)) / 2;
244  }
245 
247  real& gammaX, real& gammaY, real& gammaZ) const {
248  real fX, fY;
249  real Ures = V0(X, Y, Z, gammaX, gammaY, gammaZ) + Phi(X, Y, fX, fY);
250  gammaX += fX;
251  gammaY += fY;
252  return Ures;
253  }
254 
256  real& gammay, real& gammaz) const {
257  real X, Y, Z;
259  _earth.IntForward(lat, 0, h, X, Y, Z, M);
260  real gammaX, gammaY, gammaZ,
261  Ures = U(X, Y, Z, gammaX, gammaY, gammaZ);
262  // gammax = M[0] * gammaX + M[3] * gammaY + M[6] * gammaZ;
263  gammay = M[1] * gammaX + M[4] * gammaY + M[7] * gammaZ;
264  gammaz = M[2] * gammaX + M[5] * gammaY + M[8] * gammaZ;
265  return Ures;
266  }
267 
269  real omega, real J2) {
270  // Solve
271  // f = e^2 * (1 - K * e/q0) - 3 * J2 = 0
272  // for e^2 using Newton's method
273  static const real maxe_ = 1 - numeric_limits<real>::epsilon();
274  static const real eps2_ = sqrt(numeric_limits<real>::epsilon()) / 100;
275  real
276  K = 2 * Math::sq(a * omega) * a / (15 * GM),
277  J0 = (1 - 4 * K / Math::pi()) / 3;
278  if (!(GM > 0 && Math::isfinite(K) && K >= 0))
279  return Math::NaN();
280  if (!(Math::isfinite(J2) && J2 <= J0)) return Math::NaN();
281  if (J2 == J0) return 1;
282  // Solve e2 - f1 * f2 * K / Q0 - 3 * J2 = 0 for J2 close to J0;
283  // subst e2 = ep2/(1+ep2), f2 = 1/(1+ep2), f1 = 1/sqrt(1+ep2), J2 = J0-dJ2,
284  // Q0 = pi/(4*z^3) - 2/z^4 + (3*pi)/(4*z^5), z = sqrt(ep2), and balance two
285  // leading terms to give
286  real
287  ep2 = max(Math::sq(32 * K / (3 * Math::sq(Math::pi()) * (J0 - J2))),
288  -maxe_),
289  e2 = min(ep2 / (1 + ep2), maxe_);
290  for (int j = 0; j < maxit_ || GEOGRAPHICLIB_PANIC; ++j) {
291  real
292  e2a = e2, ep2a = ep2,
293  f2 = 1 - e2, // (1 - f)^2
294  f1 = sqrt(f2), // (1 - f)
295  Q0 = Qf(e2 < 0 ? -e2 : ep2, e2 < 0),
296  h = e2 - f1 * f2 * K / Q0 - 3 * J2,
297  dh = 1 - 3 * f1 * K * QH3f(e2 < 0 ? -e2 : ep2, e2 < 0) /
298  (2 * Math::sq(Q0));
299  e2 = min(e2a - h / dh, maxe_);
300  ep2 = max(e2 / (1 - e2), -maxe_);
301  if (abs(h) < eps2_ || e2 == e2a || ep2 == ep2a)
302  break;
303  }
304  return e2 / (1 + sqrt(1 - e2));
305  }
306 
308  real omega, real f) {
309  real
310  K = 2 * Math::sq(a * omega) * a / (15 * GM),
311  f1 = 1 - f,
312  f2 = Math::sq(f1),
313  e2 = f * (2 - f);
314  // H+M, Eq 2-90 + 2-92'
315  return (e2 - K * f1 * f2 / Qf(f < 0 ? -e2 : e2 / f2, f < 0)) / 3;
316  }
317 
318 } // namespace GeographicLib
gtsam.examples.DogLegOptimizerExample.int
int
Definition: DogLegOptimizerExample.py:111
GeographicLib::Constants::GRS80_a
static T GRS80_a()
Definition: Constants.hpp:205
initial::V0
const Vector3 V0(0, 0, 0)
GeographicLib::Math::sind
static T sind(T x)
Definition: Math.hpp:609
GeographicLib::Math::NaN
static T NaN()
Definition: Math.hpp:830
GeographicLib::NormalGravity::Hf
static real Hf(real x, bool alt)
Definition: src/NormalGravity.cpp:141
test_constructor::f1
auto f1
Definition: testHybridNonlinearFactor.cpp:56
Q0
static double Q0[8]
Definition: ndtri.c:67
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
GeographicLib::Math::copysign
static T copysign(T x, T y)
Definition: Math.hpp:751
GeographicLib
Namespace for GeographicLib.
Definition: JacobiConformal.hpp:15
gtsam::Y
GaussianFactorGraphValuePair Y
Definition: HybridGaussianProductFactor.cpp:29
GeographicLib::Math::isfinite
static bool isfinite(T x)
Definition: Math.hpp:806
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
GeographicLib::NormalGravity::atan7series
static real atan7series(real x)
Definition: src/NormalGravity.cpp:99
f2
double f2(const Vector2 &x)
Definition: testNumericalDerivative.cpp:56
real
float real
Definition: datatypes.h:10
log
const EIGEN_DEVICE_FUNC LogReturnType log() const
Definition: ArrayCwiseUnaryOps.h:128
X
#define X
Definition: icosphere.cpp:20
h
const double h
Definition: testSimpleHelicopter.cpp:19
GeographicLib::GeographicErr
Exception handling for GeographicLib.
Definition: Constants.hpp:389
GeographicLib::Geocentric
Geocentric coordinates
Definition: Geocentric.hpp:67
biased_x_rotation::omega
const double omega
Definition: testPreintegratedRotation.cpp:32
GeographicLib::NormalGravity::atan5series
static real atan5series(real x)
Definition: src/NormalGravity.cpp:122
n
int n
Definition: BiCGSTAB_simple.cpp:1
epsilon
static double epsilon
Definition: testRot3.cpp:37
GeographicLib::Math::real
double real
Definition: Math.hpp:129
GeographicLib::Constants::WGS84_omega
static T WGS84_omega()
Definition: Constants.hpp:195
GeographicLib::NormalGravity
The normal gravity of the earth.
Definition: NormalGravity.hpp:79
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
GEOGRAPHICLIB_PANIC
#define GEOGRAPHICLIB_PANIC
Definition: Math.hpp:87
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
GeographicLib::Constants::WGS84_GM
static T WGS84_GM()
Definition: Constants.hpp:184
GeographicLib::NormalGravity::J2ToFlattening
static Math::real J2ToFlattening(real a, real GM, real omega, real J2)
Definition: src/NormalGravity.cpp:268
GeographicLib::NormalGravity::Jn
real Jn(int n) const
Definition: src/NormalGravity.cpp:165
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
GeographicLib::Math::LatFix
static T LatFix(T x)
Definition: Math.hpp:467
GeographicLib::NormalGravity::WGS84
static const NormalGravity & WGS84()
Definition: src/NormalGravity.cpp:83
y
Scalar * y
Definition: level1_cplx_impl.h:124
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
GeographicLib::Geocentric::dim2_
static const size_t dim2_
Definition: Geocentric.hpp:77
GeographicLib::NormalGravity::Initialize
void Initialize(real a, real GM, real omega, real f_J2, bool geometricp)
Definition: src/NormalGravity.cpp:21
gtsam::GM
@ GM
Definition: GncParams.h:37
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
GeographicLib::NormalGravity::Phi
Math::real Phi(real X, real Y, real &fX, real &fY) const
Definition: src/NormalGravity.cpp:239
GeographicLib::Math::pi
static T pi()
Definition: Math.hpp:202
GeographicLib::Constants::WGS84_a
static T WGS84_a()
Definition: Constants.hpp:159
GeographicLib::NormalGravity::GRS80
static const NormalGravity & GRS80()
Definition: src/NormalGravity.cpp:91
K
#define K
Definition: igam.h:8
GeographicLib::NormalGravity::QH3f
static real QH3f(real x, bool alt)
Definition: src/NormalGravity.cpp:153
GeographicLib::Constants::GRS80_J2
static T GRS80_J2()
Definition: Constants.hpp:245
std
Definition: BFloat16.h:88
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
P
static double P[]
Definition: ellpe.c:68
min
#define min(a, b)
Definition: datatypes.h:19
U
@ U
Definition: testDecisionTree.cpp:349
ceil
const EIGEN_DEVICE_FUNC CeilReturnType ceil() const
Definition: ArrayCwiseUnaryOps.h:495
GeographicLib::Constants::WGS84_f
static T WGS84_f()
Definition: Constants.hpp:169
abs
#define abs(x)
Definition: datatypes.h:17
NormalGravity.hpp
Header for GeographicLib::NormalGravity class.
GeographicLib::Constants::GRS80_GM
static T GRS80_GM()
Definition: Constants.hpp:216
GeographicLib::NormalGravity::Qf
static real Qf(real x, bool alt)
Definition: src/NormalGravity.cpp:130
GeographicLib::NormalGravity::SurfaceGravity
Math::real SurfaceGravity(real lat) const
Definition: src/NormalGravity.cpp:177
GeographicLib::Constants::GRS80_omega
static T GRS80_omega()
Definition: Constants.hpp:234
GeographicLib::Math::sq
static T sq(T x)
Definition: Math.hpp:232
align_3::t
Point2 t(10, 10)
GeographicLib::NormalGravity::V0
Math::real V0(real X, real Y, real Z, real &GammaX, real &GammaY, real &GammaZ) const
Definition: src/NormalGravity.cpp:183
GeographicLib::NormalGravity::FlatteningToJ2
static Math::real FlatteningToJ2(real a, real GM, real omega, real f)
Definition: src/NormalGravity.cpp:307
GeographicLib::NormalGravity::NormalGravity
NormalGravity()
Definition: NormalGravity.hpp:180
max
#define max(a, b)
Definition: datatypes.h:20
Z
#define Z
Definition: icosphere.cpp:21
swap
int EIGEN_BLAS_FUNC() swap(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
Definition: level1_impl.h:130
GeographicLib::Math::hypot
static T hypot(T x, T y)
Definition: Math.hpp:243
real
Definition: main.h:100
GeographicLib::NormalGravity::Gravity
Math::real Gravity(real lat, real h, real &gammay, real &gammaz) const
Definition: src/NormalGravity.cpp:255
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
GeographicLib::NormalGravity::U
Math::real U(real X, real Y, real Z, real &gammaX, real &gammaY, real &gammaZ) const
Definition: src/NormalGravity.cpp:246
z2
static const Unit3 z2
Definition: testRotateFactor.cpp:43
lat
static const double lat
Definition: testGeographicLib.cpp:34
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:51