INSTINCT Code Coverage Report

Directory: src/
File: Navigation/Geoid/EGM96.cpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 0 100 0.0%
Functions: 0 6 0.0%
Branches: 0 33 0.0%
Line Branch Exec Source
1 // This file is part of INSTINCT, the INS Toolkit for Integrated
2 // Navigation Concepts and Training by the Institute of Navigation of
3 // the University of Stuttgart, Germany.
4
5 /// @file EGM96.hpp
6 /// @brief EGM96 geoid model
7 /// @author T. Topp (topp@ins.uni-stuttgart.de)
8 /// @date 2024-07-05
9 ///
10 /// Source code: https://github.com/emericg/EGM96/blob/master/EGM96.c
11 /// Copyright (c) 2006 D.Ineiev <ineiev@yahoo.co.uk>
12 /// Copyright (c) 2020 Emeric Grange <emeric.grange@gmail.com>
13 ///
14 /// This software is provided 'as-is', without any express or implied warranty.
15 /// In no event will the authors be held liable for any damages arising from
16 /// the use of this software.
17 ///
18 /// Permission is granted to anyone to use this software for any purpose,
19 /// including commercial applications, and to alter it and redistribute it
20 /// freely, subject to the following restrictions:
21 ///
22 /// 1. The origin of this software must not be misrepresented; you must not
23 /// claim that you wrote the original software. If you use this software
24 /// in a product, an acknowledgment in the product documentation would be
25 /// appreciated but is not required.
26 /// 2. Altered source versions must be plainly marked as such, and must not be
27 /// misrepresented as being the original software.
28 /// 3. This notice may not be removed or altered from any source distribution.
29
30 /*
31 * This program is designed for the calculation of a geoid undulation at a point
32 * whose latitude and longitude is specified.
33 *
34 * This program is designed to be used with the constants of EGM96 and those of
35 * the WGS84(g873) system. The undulation will refer to the WGS84 ellipsoid.
36 *
37 * It's designed to use the potential coefficient model EGM96 and a set of
38 * spherical harmonic coefficients of a correction term.
39 * The correction term is composed of several different components, the primary
40 * one being the conversion of a height anomaly to a geoid undulation.
41 * The principles of this procedure were initially described in the paper:
42 * - use of potential coefficient models for geoid undulation determination using
43 * a spherical harmonic representation of the height anomaly/geoid undulation
44 * difference by R.H. Rapp, Journal of Geodesy, 1996.
45 *
46 * This program is a modification of the program described in the following report:
47 * - a fortran program for the computation of gravimetric quantities from high
48 * degree spherical harmonic expansions, Richard H. Rapp, report 334, Department
49 * of Geodetic Science and Surveying, the Ohio State University, Columbus, 1982
50 */
51 /*
52 * Changes:
53 * - Auto-formatting
54 * - Namespace NAV added
55 * - Added "NOLINT" instructions to disable clang-tidy
56 */
57
58 #include "EGM96.hpp"
59 #include "internal/EGM96_Data.hpp"
60 #include <cmath>
61
62 /* ************************************************************************** */
63 namespace NAV
64 {
65 #define _coeffs (65341) //!< Size of correction and harmonic coefficients arrays (361*181)
66 #define _nmax (360) //!< Maximum degree and orders of harmonic coefficients.
67 #define _361 (361)
68
69 /* ************************************************************************** */
70
71 ✗ double hundu(double p[_coeffs + 1], // NOLINT
72 double sinml[_361 + 1], double cosml[_361 + 1], // NOLINT
73 double gr, double re)
74 {
75 // WGS 84 gravitational constant in m³/s² (mass of Earth’s atmosphere included)
76 ✗ const double GM = 0.3986004418e15;
77 // WGS 84 datum surface equatorial radius
78 ✗ const double ae = 6378137.0;
79
80 ✗ double ar = ae / re;
81 ✗ double arn = ar;
82 ✗ double ac = 0;
83 ✗ double a = 0;
84
85 ✗ unsigned k = 3;
86 ✗ for (unsigned n = 2; n <= _nmax; n++)
87 {
88 ✗ arn *= ar;
89 ✗ k++;
90 ✗ double sum = p[k] * Geoid::EGM96::internal::egm96_data[k][2]; // NOLINT
91 ✗ double sumc = p[k] * Geoid::EGM96::internal::egm96_data[k][0]; // NOLINT
92
93 ✗ for (unsigned m = 1; m <= n; m++)
94 {
95 ✗ k++;
96 ✗ double tempc = Geoid::EGM96::internal::egm96_data[k][0] * cosml[m] + Geoid::EGM96::internal::egm96_data[k][1] * sinml[m]; // NOLINT
97 ✗ double temp = Geoid::EGM96::internal::egm96_data[k][2] * cosml[m] + Geoid::EGM96::internal::egm96_data[k][3] * sinml[m]; // NOLINT
98 ✗ sumc += p[k] * tempc;
99 ✗ sum += p[k] * temp;
100 }
101 ✗ ac += sumc;
102 ✗ a += sum * arn;
103 }
104 ✗ ac += Geoid::EGM96::internal::egm96_data[1][0] + (p[2] * Geoid::EGM96::internal::egm96_data[2][0]) + (p[3] * (Geoid::EGM96::internal::egm96_data[3][0] * cosml[1] + Geoid::EGM96::internal::egm96_data[3][1] * sinml[1]));
105
106 // Add haco = ac/100 to convert height anomaly on the ellipsoid to the undulation
107 // Add -0.53m to make undulation refer to the WGS84 ellipsoid
108
109 ✗ return ((a * GM) / (gr * re)) + (ac / 100.0) - 0.53;
110 }
111
112 ✗ void dscml(double rlon, double sinml[_361 + 1], double cosml[_361 + 1]) // NOLINT
113 {
114 ✗ double a = sin(rlon);
115 ✗ double b = cos(rlon);
116
117 ✗ sinml[1] = a;
118 ✗ cosml[1] = b;
119 ✗ sinml[2] = 2 * b * a;
120 ✗ cosml[2] = 2 * b * b - 1;
121
122 ✗ for (unsigned m = 3; m <= _nmax; m++)
123 {
124 ✗ sinml[m] = 2 * b * sinml[m - 1] - sinml[m - 2];
125 ✗ cosml[m] = 2 * b * cosml[m - 1] - cosml[m - 2];
126 }
127 ✗ }
128
129 /*!
130 * \param m : order.
131 * \param theta : Colatitude (radians).
132 * \param rleg : Normalized legendre function.
133 *
134 * This subroutine computes all normalized legendre function in 'rleg'.
135 * The dimensions of array 'rleg' must be at least equal to nmax+1.
136 * All calculations are in double precision.
137 *
138 * Original programmer: Oscar L. Colombo, Dept. of Geodetic Science the Ohio State University, August 1980.
139 * ineiev: I removed the derivatives, for they are never computed here.
140 */
141 ✗ void legfdn(unsigned m, double theta, double rleg[_361 + 1]) // NOLINT
142 {
143 static double drts[1301], dirt[1301], cothet, sithet, rlnn[_361 + 1]; // NOLINT
144 static int ir; // TODO 'ir' must be set to zero before the first call to this sub.
145
146 ✗ unsigned nmax1 = _nmax + 1;
147 ✗ unsigned nmax2p = (2 * _nmax) + 1;
148 ✗ unsigned m1 = m + 1;
149 ✗ unsigned m2 = m + 2;
150 ✗ unsigned m3 = m + 3;
151 unsigned n, n1, n2; // NOLINT
152
153 ✗ if (!ir)
154 {
155 ✗ ir = 1;
156 ✗ for (n = 1; n <= nmax2p; n++)
157 {
158 ✗ drts[n] = sqrt(n); // NOLINT
159 ✗ dirt[n] = 1 / drts[n]; // NOLINT
160 }
161 }
162
163 ✗ cothet = cos(theta);
164 ✗ sithet = sin(theta);
165
166 // compute the legendre functions
167 ✗ rlnn[1] = 1;
168 ✗ rlnn[2] = sithet * drts[3];
169 ✗ for (n1 = 3; n1 <= m1; n1++)
170 {
171 ✗ n = n1 - 1;
172 ✗ n2 = 2 * n;
173 ✗ rlnn[n1] = drts[n2 + 1] * dirt[n2] * sithet * rlnn[n]; // NOLINT
174 }
175
176 ✗ switch (m) // NOLINT
177 {
178 ✗ case 1:
179 ✗ rleg[2] = rlnn[2];
180 ✗ rleg[3] = drts[5] * cothet * rleg[2];
181 ✗ break;
182 ✗ case 0:
183 ✗ rleg[1] = 1;
184 ✗ rleg[2] = cothet * drts[3];
185 ✗ break;
186 }
187 ✗ rleg[m1] = rlnn[m1]; // NOLINT
188
189 ✗ if (m2 <= nmax1)
190 {
191 ✗ rleg[m2] = drts[m1 * 2 + 1] * cothet * rleg[m1]; // NOLINT
192 ✗ if (m3 <= nmax1)
193 {
194 ✗ for (n1 = m3; n1 <= nmax1; n1++)
195 {
196 ✗ n = n1 - 1;
197 ✗ if ((!m && n < 2) || (m == 1 && n < 3)) continue; // NOLINT
198 ✗ n2 = 2 * n;
199 ✗ rleg[n1] = drts[n2 + 1] * dirt[n + m] * dirt[n - m] * (drts[n2 - 1] * cothet * rleg[n1 - 1] - drts[n + m - 1] * drts[n - m - 1] * dirt[n2 - 3] * rleg[n1 - 2]); // NOLINT
200 }
201 }
202 }
203 ✗ }
204
205 namespace
206 {
207
208 /*!
209 * \param lat : Latitude in radians.
210 * \param lon : Longitude in radians.
211 * \param re : Geocentric radius.
212 * \param rlat : Geocentric latitude.
213 * \param gr : Normal gravity (m/sec²).
214 *
215 * This subroutine computes geocentric distance to the point, the geocentric
216 * latitude, and an approximate value of normal gravity at the point based the
217 * constants of the WGS84(g873) system are used.
218 */
219 ✗ void radgra(double lat, double lon, double* rlat, double* gr, double* re)
220 {
221 ✗ const double a = 6378137.0;
222 ✗ const double e2 = 0.00669437999013;
223 ✗ const double geqt = 9.7803253359;
224 ✗ const double k = 0.00193185265246;
225 ✗ double t1 = sin(lat) * sin(lat);
226 ✗ double n = a / sqrt(1.0 - (e2 * t1));
227 ✗ double t2 = n * cos(lat);
228 ✗ double x = t2 * cos(lon);
229 ✗ double y = t2 * sin(lon);
230 ✗ double z = (n * (1 - e2)) * sin(lat);
231
232 ✗ *re = sqrt((x * x) + (y * y) + (z * z)); // compute the geocentric radius
233 ✗ *rlat = atan(z / sqrt((x * x) + (y * y))); // compute the geocentric latitude
234 ✗ *gr = geqt * (1 + (k * t1)) / sqrt(1 - (e2 * t1)); // compute normal gravity (m/sec²)
235 ✗ }
236
237 /*!
238 * \brief Compute the geoid undulation from the EGM96 potential coefficient model, for a given latitude and longitude.
239 * \param lat : Latitude in radians.
240 * \param lon : Longitude in radians.
241 * \return The geoid undulation / altitude offset (in meters).
242 */
243 ✗ double undulation(double lat, double lon)
244 {
245 double p[_coeffs + 1], sinml[_361 + 1], cosml[_361 + 1], rleg[_361 + 1]; // NOLINT
246
247 double rlat, gr, re; // NOLINT
248 ✗ unsigned nmax1 = _nmax + 1;
249
250 // compute the geocentric latitude, geocentric radius, normal gravity
251 ✗ radgra(lat, lon, &rlat, &gr, &re);
252 ✗ rlat = (M_PI / 2) - rlat;
253
254 ✗ for (unsigned j = 1; j <= nmax1; j++)
255 {
256 ✗ unsigned m = j - 1;
257 ✗ legfdn(m, rlat, rleg);
258 ✗ for (unsigned i = j; i <= nmax1; i++)
259 {
260 ✗ p[(((i - 1) * i) / 2) + m + 1] = rleg[i]; // NOLINT
261 }
262 }
263 ✗ dscml(lon, sinml, cosml);
264
265 ✗ return hundu(p, sinml, cosml, gr, re);
266 }
267
268 } // namespace
269
270 /* ************************************************************************** */
271
272 ✗ double egm96_compute_altitude_offset(double lat, double lon)
273 {
274 ✗ return undulation(lat, lon);
275 }
276
277 /* ************************************************************************** */
278 } // namespace NAV
279