INSTINCT Code Coverage Report

Directory: src/
File: Navigation/Gravity/internal/AssociatedLegendre.hpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 29 29 100.0%
Functions: 1 1 100.0%
Branches: 38 64 59.4%
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 // This Source Code Form is subject to the terms of the Mozilla Public
6 // License, v. 2.0. If a copy of the MPL was not distributed with this
7 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
9 /// @file AssociatedLegendre.hpp
10 /// @brief Associated Legendre Polynomials for EGM96
11 /// @author M. Maier (marcel.maier@ins.uni-stuttgart.de)
12 /// @date 2021-05-21
13
14 #pragma once
15
16 #include <cmath>
17 #include <array>
18 #include <numbers>
19 #include "Navigation/Constants.hpp"
20
21 #include "util/Eigen.hpp"
22 #include "util/Logger.hpp"
23
24 namespace NAV::internal
25 {
26 /// @brief Calculates the associated Legendre Polynomial coefficients necessary for the EGM96
27 /// @param[in] theta Elevation angle (spherical coordinates) [rad]
28 /// @param[in] ndegree Degree of associated Legendre polynomials
29 /// @return Matrix of associated Legendre polynomial coefficients P and its derivative Pd
30 ///
31 /// @note See 'GUT User Guide' (2018) chapter 4.2, equations (4.2.2), (4.2.3) and (4.2.6)
32 template<typename T>
33 49997 [[nodiscard]] std::pair<Eigen::MatrixX<T>, Eigen::MatrixX<T>> associatedLegendre(const T& theta, size_t ndegree = 10)
34 {
35 // Index needed for the calculation of all necessary 'Pd' entries up to 'ndegree' (--> Pd(n = ndegree, m = ndegree) is dependent on P(n = ndegree, m = ndegree + 1))
36 49997 int N = static_cast<int>(ndegree + 2);
37
38
2/4
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
 49997 Eigen::MatrixX<T> P = Eigen::MatrixX<T>::Zero(N, N);
39
2/4
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
 49997 Eigen::MatrixX<T> Pd = Eigen::MatrixX<T>::Zero(N, N);
40
41 #if defined(__GNUC__) || defined(__clang__)
42 #pragma GCC diagnostic push
43 #pragma GCC diagnostic ignored "-Wnull-dereference"
44 #endif
45 // Recurrence relations for the normalized associated Legendre functions (see 'GUT User Guide' eq. 4.2.2 and eq. 4.2.3)
46
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 P(0, 0) = T(1.0);
47
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 P(1, 0) = std::numbers::sqrt3 * std::cos(theta);
48
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 P(1, 1) = std::numbers::sqrt3 * std::sin(theta);
49 #if defined(__GNUC__) || defined(__clang__)
50 #pragma GCC diagnostic pop
51 #endif
52
53
2/2
✓ Branch 0 taken 499970 times.
✓ Branch 1 taken 49997 times.
 549967 for (int n = 2; n <= N - 1; n++)
54 {
55 499970 auto nd = static_cast<double>(n);
56
2/4
✓ Branch 1 taken 499970 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 499970 times.
✗ Branch 5 not taken.
 499970 P(n, n) = std::sqrt((2.0 * nd + 1.0) / (2.0 * nd)) * std::sin(theta) * P(n - 1, n - 1);
57
58
2/2
✓ Branch 0 taken 3749775 times.
✓ Branch 1 taken 499970 times.
 4249745 for (int m = 0; m <= n; m++)
59 {
60 3749775 auto md = static_cast<double>(m);
61
62
2/2
✓ Branch 0 taken 499970 times.
✓ Branch 1 taken 3249805 times.
 3749775 if (n == m + 1)
63 {
64
2/4
✓ Branch 1 taken 499970 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 499970 times.
✗ Branch 5 not taken.
 499970 P(n, m) = std::sqrt(((2.0 * nd + 1.0) * (2.0 * nd - 1.0)) / ((nd + md) * (nd - md))) * std::cos(theta) * P(n - 1, m);
65 }
66
2/2
✓ Branch 0 taken 2749835 times.
✓ Branch 1 taken 499970 times.
 3249805 else if (n > m + 1)
67 {
68
1/2
✓ Branch 1 taken 2749835 times.
✗ Branch 2 not taken.
 5499670 P(n, m) = std::sqrt(((2.0 * nd + 1.0) * (2.0 * nd - 1.0)) / ((nd + md) * (nd - md))) * std::cos(theta) * P(n - 1, m)
69
2/4
✓ Branch 1 taken 2749835 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2749835 times.
✗ Branch 5 not taken.
 2749835 - std::sqrt(((2.0 * nd + 1.0) * (nd + md - 1.0) * (nd - md - 1.0)) / ((2.0 * nd - 3.0) * (nd + md) * (nd - md))) * std::cos(theta) * P(n - 2, m);
70 }
71 }
72 }
73
74 // Recurrence relations for the derivative of the normalized associated Legendre functions (see 'GUT User Guide' eq. 4.2.6)
75
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 Pd(0, 0) = T(0.0);
76
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 Pd(1, 0) = -std::numbers::sqrt3 * std::sin(theta);
77
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 Pd(1, 1) = std::numbers::sqrt3 * std::cos(theta);
78
79
2/2
✓ Branch 0 taken 499970 times.
✓ Branch 1 taken 49997 times.
 549967 for (int n = 2; n <= N - 1; n++) // 2nd for-loop is necessary, because for the calculation of 'Pd', all coefficients of 'P' must be available
80 {
81 499970 auto nd = static_cast<double>(n);
82
83
2/4
✓ Branch 1 taken 499970 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 499970 times.
✗ Branch 5 not taken.
 499970 Pd(n, 0) = -0.5 * std::sqrt(2.0 * nd * (nd + 1.0)) * P(n, 1);
84
3/6
✓ Branch 1 taken 499970 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 499970 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 499970 times.
✗ Branch 8 not taken.
 499970 Pd(n, 1) = 0.5 * (std::sqrt(2.0 * nd * (nd + 1.0)) * P(n, 0) - std::sqrt((nd - 1.0) * (nd + 2.0)) * std::pow(P(n, 2), 2.0));
85
86
2/2
✓ Branch 0 taken 2249865 times.
✓ Branch 1 taken 499970 times.
 2749835 for (int m = 2; m <= n - 1; m++)
87 {
88 2249865 auto md = static_cast<double>(m);
89
90 // else if ((m > 1) && (m < N - 1))
91
3/6
✓ Branch 1 taken 2249865 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2249865 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2249865 times.
✗ Branch 8 not taken.
 2249865 Pd(n, m) = 0.5 * (std::sqrt((nd + md) * (nd - md + 1.0)) * P(n, m - 1) - std::sqrt((nd - md) * (nd + md + 1.0)) * P(n, m + 1));
92 }
93 }
94
95
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 99994 return std::make_pair(P, Pd);
96 49997 }
97
98 } // namespace NAV::internal
99