src/GravityModel.cpp
Go to the documentation of this file.
1 
11 #include <fstream>
15 
16 #if !defined(GEOGRAPHICLIB_DATA)
17 # if defined(_WIN32)
18 # define GEOGRAPHICLIB_DATA "C:/ProgramData/GeographicLib"
19 # else
20 # define GEOGRAPHICLIB_DATA "/usr/local/share/GeographicLib"
21 # endif
22 #endif
23 
24 #if !defined(GEOGRAPHICLIB_GRAVITY_DEFAULT_NAME)
25 # define GEOGRAPHICLIB_GRAVITY_DEFAULT_NAME "egm96"
26 #endif
27 
28 #if defined(_MSC_VER)
29 // Squelch warnings about unsafe use of getenv
30 # pragma warning (disable: 4996)
31 #endif
32 
33 namespace GeographicLib {
34 
35  using namespace std;
36 
37  GravityModel::GravityModel(const std::string& name,const std::string& path)
38  : _name(name)
39  , _dir(path)
40  , _description("NONE")
41  , _date("UNKNOWN")
42  , _amodel(Math::NaN())
43  , _GMmodel(Math::NaN())
44  , _zeta0(0)
45  , _corrmult(1)
46  , _norm(SphericalHarmonic::FULL)
47  {
48  if (_dir.empty())
51  {
52  string coeff = _filename + ".cof";
53  ifstream coeffstr(coeff.c_str(), ios::binary);
54  if (!coeffstr.good())
55  throw GeographicErr("Error opening " + coeff);
56  char id[idlength_ + 1];
57  coeffstr.read(id, idlength_);
58  if (!coeffstr.good())
59  throw GeographicErr("No header in " + coeff);
60  id[idlength_] = '\0';
61  if (_id != string(id))
62  throw GeographicErr("ID mismatch: " + _id + " vs " + id);
63  int N, M;
64  SphericalEngine::coeff::readcoeffs(coeffstr, N, M, _Cx, _Sx);
65  if (!(N >= 0 && M >= 0))
66  throw GeographicErr("Degree and order must be at least 0");
67  if (_Cx[0] != 0)
68  throw GeographicErr("A degree 0 term should be zero");
69  _Cx[0] = 1; // Include the 1/r term in the sum
71  SphericalEngine::coeff::readcoeffs(coeffstr, N, M, _CC, _CS);
72  if (N < 0) {
73  N = M = 0;
74  _CC.resize(1, real(0));
75  }
76  _CC[0] += _zeta0 / _corrmult;
77  _correction = SphericalHarmonic(_CC, _CS, N, N, M, real(1), _norm);
78  int pos = int(coeffstr.tellg());
79  coeffstr.seekg(0, ios::end);
80  if (pos != coeffstr.tellg())
81  throw GeographicErr("Extra data in " + coeff);
82  }
83  int nmx = _gravitational.Coefficients().nmx();
84  // Adjust the normalization of the normal potential to match the model.
85  real mult = _earth._GM / _GMmodel;
86  real amult = Math::sq(_earth._a / _amodel);
87  // The 0th term in _zonal should be is 1 + _dzonal0. Instead set it to 1
88  // to give exact cancellation with the (0,0) term in the model and account
89  // for _dzonal0 separately.
90  _zonal.clear(); _zonal.push_back(1);
92  for (int n = 2; n <= nmx; n += 2) {
93  // Only include as many normal zonal terms as matter. Figuring the limit
94  // in this way works because the coefficients of the normal potential
95  // (which is smooth) decay much more rapidly that the corresponding
96  // coefficient of the model potential (which is bumpy). Typically this
97  // goes out to n = 18.
98  mult *= amult;
99  real
100  r = _Cx[n], // the model term
101  s = - mult * _earth.Jn(n) / sqrt(real(2 * n + 1)), // the normal term
102  t = r - s; // the difference
103  if (t == r) // the normal term is negligible
104  break;
105  _zonal.push_back(0); // index = n - 1; the odd terms are 0
106  _zonal.push_back(s);
107  }
108  int nmx1 = int(_zonal.size()) - 1;
112  _zonal,
113  _zonal, // This is not accessed!
114  nmx1, nmx1, 0,
115  _amodel,
117  }
118 
119  void GravityModel::ReadMetadata(const std::string& name) {
120  const char* spaces = " \t\n\v\f\r";
121  _filename = _dir + "/" + name + ".egm";
122  ifstream metastr(_filename.c_str());
123  if (!metastr.good())
124  throw GeographicErr("Cannot open " + _filename);
125  string line;
126  getline(metastr, line);
127  if (!(line.size() >= 6 && line.substr(0,5) == "EGMF-"))
128  throw GeographicErr(_filename + " does not contain EGMF-n signature");
129  string::size_type n = line.find_first_of(spaces, 5);
130  if (n != string::npos)
131  n -= 5;
132  string version(line, 5, n);
133  if (version != "1")
134  throw GeographicErr("Unknown version in " + _filename + ": " + version);
135  string key, val;
136  real a = Math::NaN(), GM = a, omega = a, f = a, J2 = a;
137  while (getline(metastr, line)) {
138  if (!Utility::ParseLine(line, key, val))
139  continue;
140  // Process key words
141  if (key == "Name")
142  _name = val;
143  else if (key == "Description")
144  _description = val;
145  else if (key == "ReleaseDate")
146  _date = val;
147  else if (key == "ModelRadius")
148  _amodel = Utility::val<real>(val);
149  else if (key == "ModelMass")
150  _GMmodel = Utility::val<real>(val);
151  else if (key == "AngularVelocity")
152  omega = Utility::val<real>(val);
153  else if (key == "ReferenceRadius")
154  a = Utility::val<real>(val);
155  else if (key == "ReferenceMass")
156  GM = Utility::val<real>(val);
157  else if (key == "Flattening")
158  f = Utility::fract<real>(val);
159  else if (key == "DynamicalFormFactor")
160  J2 = Utility::fract<real>(val);
161  else if (key == "HeightOffset")
162  _zeta0 = Utility::fract<real>(val);
163  else if (key == "CorrectionMultiplier")
164  _corrmult = Utility::fract<real>(val);
165  else if (key == "Normalization") {
166  if (val == "FULL" || val == "Full" || val == "full")
168  else if (val == "SCHMIDT" || val == "Schmidt" || val == "schmidt")
170  else
171  throw GeographicErr("Unknown normalization " + val);
172  } else if (key == "ByteOrder") {
173  if (val == "Big" || val == "big")
174  throw GeographicErr("Only little-endian ordering is supported");
175  else if (!(val == "Little" || val == "little"))
176  throw GeographicErr("Unknown byte ordering " + val);
177  } else if (key == "ID")
178  _id = val;
179  // else unrecognized keywords are skipped
180  }
181  // Check values
182  if (!(Math::isfinite(_amodel) && _amodel > 0))
183  throw GeographicErr("Model radius must be positive");
184  if (!(Math::isfinite(_GMmodel) && _GMmodel > 0))
185  throw GeographicErr("Model mass constant must be positive");
186  if (!(Math::isfinite(_corrmult) && _corrmult > 0))
187  throw GeographicErr("Correction multiplier must be positive");
188  if (!(Math::isfinite(_zeta0)))
189  throw GeographicErr("Height offset must be finite");
190  if (int(_id.size()) != idlength_)
191  throw GeographicErr("Invalid ID");
192  if (Math::isfinite(f) && Math::isfinite(J2))
193  throw GeographicErr("Cannot specify both f and J2");
194  _earth = NormalGravity(a, GM, omega,
195  Math::isfinite(f) ? f : J2, Math::isfinite(f));
196  }
197 
199  real& deltaX, real& deltaY, real& deltaZ,
200  bool gradp, bool correct) const {
201  // If correct, then produce the correct T = W - U. Otherwise, neglect the
202  // n = 0 term (which is proportial to the difference in the model and
203  // reference values of GM).
204  if (_dzonal0 == 0)
205  // No need to do the correction
206  correct = false;
207  real T, invR = correct ? 1 / Math::hypot(Math::hypot(X, Y), Z) : 1;
208  if (gradp) {
209  // initial values to suppress warnings
210  deltaX = deltaY = deltaZ = 0;
211  T = _disturbing(-1, X, Y, Z, deltaX, deltaY, deltaZ);
212  real f = _GMmodel / _amodel;
213  deltaX *= f;
214  deltaY *= f;
215  deltaZ *= f;
216  if (correct) {
217  invR = _GMmodel * _dzonal0 * invR * invR * invR;
218  deltaX += X * invR;
219  deltaY += Y * invR;
220  deltaZ += Z * invR;
221  }
222  } else
223  T = _disturbing(-1, X, Y, Z);
224  T = (T / _amodel - (correct ? _dzonal0 : 0) * invR) * _GMmodel;
225  return T;
226  }
227 
229  real& GX, real& GY, real& GZ) const {
230  real
231  Vres = _gravitational(X, Y, Z, GX, GY, GZ),
232  f = _GMmodel / _amodel;
233  Vres *= f;
234  GX *= f;
235  GY *= f;
236  GZ *= f;
237  return Vres;
238  }
239 
241  real& gX, real& gY, real& gZ) const {
242  real fX, fY,
243  Wres = V(X, Y, Z, gX, gY, gZ) + _earth.Phi(X, Y, fX, fY);
244  gX += fX;
245  gY += fY;
246  return Wres;
247  }
248 
250  real& Dg01, real& xi, real& eta) const {
251  real X, Y, Z, M[Geocentric::dim2_];
252  _earth.Earth().IntForward(lat, lon, h, X, Y, Z, M);
253  real
254  deltax, deltay, deltaz,
255  T = InternalT(X, Y, Z, deltax, deltay, deltaz, true, false),
256  clam = M[3], slam = -M[0],
257  P = Math::hypot(X, Y),
258  R = Math::hypot(P, Z),
259  // psi is geocentric latitude
260  cpsi = R != 0 ? P / R : M[7],
261  spsi = R != 0 ? Z / R : M[8];
262  // Rotate cartesian into spherical coordinates
264  Geocentric::Rotation(spsi, cpsi, slam, clam, MC);
265  Geocentric::Unrotate(MC, deltax, deltay, deltaz, deltax, deltay, deltaz);
266  // H+M, Eq 2-151c
267  Dg01 = - deltaz - 2 * T / R;
268  real gammaX, gammaY, gammaZ;
269  _earth.U(X, Y, Z, gammaX, gammaY, gammaZ);
270  real gamma = Math::hypot( Math::hypot(gammaX, gammaY), gammaZ);
271  xi = -(deltay/gamma) / Math::degree();
272  eta = -(deltax/gamma) / Math::degree();
273  }
274 
276  {
277  real X, Y, Z;
278  _earth.Earth().IntForward(lat, lon, 0, X, Y, Z, NULL);
279  real
280  gamma0 = _earth.SurfaceGravity(lat),
281  dummy,
282  T = InternalT(X, Y, Z, dummy, dummy, dummy, false, false),
283  invR = 1 / Math::hypot(Math::hypot(X, Y), Z),
284  correction = _corrmult * _correction(invR * X, invR * Y, invR * Z);
285  // _zeta0 has been included in _correction
286  return T/gamma0 + correction;
287  }
288 
290  real& gx, real& gy, real& gz) const {
291  real X, Y, Z, M[Geocentric::dim2_];
292  _earth.Earth().IntForward(lat, lon, h, X, Y, Z, M);
293  real Wres = W(X, Y, Z, gx, gy, gz);
294  Geocentric::Unrotate(M, gx, gy, gz, gx, gy, gz);
295  return Wres;
296  }
298  real& deltax, real& deltay,
299  real& deltaz) const {
300  real X, Y, Z, M[Geocentric::dim2_];
301  _earth.Earth().IntForward(lat, lon, h, X, Y, Z, M);
302  real Tres = InternalT(X, Y, Z, deltax, deltay, deltaz, true, true);
303  Geocentric::Unrotate(M, deltax, deltay, deltaz, deltax, deltay, deltaz);
304  return Tres;
305  }
306 
307  GravityCircle GravityModel::Circle(real lat, real h, unsigned caps) const {
308  if (h != 0)
309  // Disallow invoking GeoidHeight unless h is zero.
310  caps &= ~(CAP_GAMMA0 | CAP_C);
311  real X, Y, Z, M[Geocentric::dim2_];
312  _earth.Earth().IntForward(lat, 0, h, X, Y, Z, M);
313  // Y = 0, cphi = M[7], sphi = M[8];
314  real
315  invR = 1 / Math::hypot(X, Z),
316  gamma0 = (caps & CAP_GAMMA0 ?_earth.SurfaceGravity(lat)
317  : Math::NaN()),
318  fx, fy, fz, gamma;
319  if (caps & CAP_GAMMA) {
320  _earth.U(X, Y, Z, fx, fy, fz); // fy = 0
321  gamma = Math::hypot(fx, fz);
322  } else
323  gamma = Math::NaN();
324  _earth.Phi(X, Y, fx, fy);
325  return GravityCircle(GravityCircle::mask(caps),
326  _earth._a, _earth._f, lat, h, Z, X, M[7], M[8],
328  gamma0, gamma, fx,
329  caps & CAP_G ?
330  _gravitational.Circle(X, Z, true) :
331  CircularEngine(),
332  // N.B. If CAP_DELTA is set then CAP_T should be too.
333  caps & CAP_T ?
334  _disturbing.Circle(-1, X, Z, (caps&CAP_DELTA) != 0) :
335  CircularEngine(),
336  caps & CAP_C ?
337  _correction.Circle(invR * X, invR * Z, false) :
338  CircularEngine());
339  }
340 
342  string path;
343  char* gravitypath = getenv("GEOGRAPHICLIB_GRAVITY_PATH");
344  if (gravitypath)
345  path = string(gravitypath);
346  if (!path.empty())
347  return path;
348  char* datapath = getenv("GEOGRAPHICLIB_DATA");
349  if (datapath)
350  path = string(datapath);
351  return (!path.empty() ? path : string(GEOGRAPHICLIB_DATA)) + "/gravity";
352  }
353 
355  string name;
356  char* gravityname = getenv("GEOGRAPHICLIB_GRAVITY_NAME");
357  if (gravityname)
358  name = string(gravityname);
359  return !name.empty() ? name : string(GEOGRAPHICLIB_GRAVITY_DEFAULT_NAME);
360  }
361 
362 } // namespace GeographicLib
const gtsam::Symbol key('X', 0)
static T NaN()
Definition: Math.hpp:830
const char Y
Math::real T(real X, real Y, real Z, real &deltaX, real &deltaY, real &deltaZ) const
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
SphericalHarmonic::normalization _norm
static const double lat
CircularEngine Circle(real tau, real p, real z, bool gradp) const
void ReadMetadata(const std::string &name)
Header for GeographicLib::Utility class.
The normal gravity of the earth.
static bool isfinite(T x)
Definition: Math.hpp:806
int n
Rot2 R(Rot2::fromAngle(0.1))
SphericalHarmonic _correction
Mathematical functions needed by GeographicLib.
Definition: Math.hpp:102
Definition: BFloat16.h:88
Header for GeographicLib::GravityModel class.
const SphericalEngine::coeff & Coefficients() const
#define N
Definition: gksort.c:12
Math::real InternalT(real X, real Y, real Z, real &deltaX, real &deltaY, real &deltaZ, bool gradp, bool correct) const
static void readcoeffs(std::istream &stream, int &N, int &M, std::vector< real > &C, std::vector< real > &S)
std::vector< real > _Cx
#define Z
Definition: icosphere.cpp:21
Math::real MassConstant() const
const double fy
#define GEOGRAPHICLIB_DATA
std::vector< real > _CC
static T hypot(T x, T y)
Definition: Math.hpp:243
Math::real Gravity(real lat, real lon, real h, real &gx, real &gy, real &gz) const
static T sq(T x)
Definition: Math.hpp:232
CircularEngine Circle(real p, real z, bool gradp) const
Definition: main.h:100
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Namespace for GeographicLib.
void IntForward(real lat, real lon, real h, real &X, real &Y, real &Z, real M[dim2_]) const
Vector::Scalar omega(const Vector &t, const Vector &s, RealScalar angle)
Definition: IDRS.h:36
RealScalar s
Math::real Disturbance(real lat, real lon, real h, real &deltax, real &deltay, real &deltaz) const
#define NULL
Definition: ccolamd.c:609
static T degree()
Definition: Math.hpp:216
Vector xi
Definition: testPose2.cpp:148
Spherical harmonic sums for a circle.
Math::real SurfaceGravity(real lat) const
std::vector< real > _CS
std::vector< real > _zonal
const double h
GravityModel(const GravityModel &)
static std::string DefaultGravityName()
GravityCircle Circle(real lat, real h, unsigned caps=ALL) const
static const size_t dim2_
Definition: Geocentric.hpp:77
Math::real V(real X, real Y, real Z, real &GX, real &GY, real &GZ) const
Exception handling for GeographicLib.
Definition: Constants.hpp:389
static std::string DefaultGravityPath()
static void Unrotate(real M[dim2_], real X, real Y, real Z, real &x, real &y, real &z)
Definition: Geocentric.hpp:89
std::vector< real > _Sx
static EIGEN_DEPRECATED const end_t end
static void Rotation(real sphi, real cphi, real slam, real clam, real M[dim2_])
Spherical harmonic series with a correction to the coefficients.
static const int idlength_
SphericalHarmonic1 _disturbing
static const double lon
SphericalHarmonic _gravitational
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Annotation for function names.
Definition: attr.h:48
const double fx
#define X
Definition: icosphere.cpp:20
Spherical harmonic series.
Math::real W(real X, real Y, real Z, real &gX, real &gY, real &gZ) const
Header for GeographicLib::GravityCircle class.
Math::real Phi(real X, real Y, real &fX, real &fY) const
static bool ParseLine(const std::string &line, std::string &key, std::string &val)
Definition: Utility.cpp:22
Header for GeographicLib::SphericalEngine class.
#define GEOGRAPHICLIB_GRAVITY_DEFAULT_NAME
void SphericalAnomaly(real lat, real lon, real h, real &Dg01, real &xi, real &eta) const
const Geocentric & Earth() const
Math::real U(real X, real Y, real Z, real &gammaX, real &gammaY, real &gammaZ) const
Point2 t(10, 10)
Gravity on a circle of latitude.
Math::real GeoidHeight(real lat, real lon) const


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:19