INSTINCT Code Coverage Report

Directory: src/
File: Navigation/Math/Math.cpp
Date: 2025-07-19 10:51:51
Exec Total Coverage
Lines: 41 51 80.4%
Functions: 2 3 66.7%
Branches: 10 20 50.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 // 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 #include "Math.hpp"
10
11 namespace NAV::math
12 {
13
14 25776 uint64_t factorial(uint64_t n)
15 {
16 // uint64_t is required to calculate factorials of n > 12 (Limit of uint32_t). The limit of uint64_t is at n = 20
17 25776 constexpr std::array factorials = {
18 uint64_t(1), // 0
19 uint64_t(1), // 1
20 uint64_t(2), // 2
21 uint64_t(6), // 3
22 uint64_t(24), // 4
23 uint64_t(120), // 5
24 uint64_t(720), // 6
25 uint64_t(5040), // 7
26 uint64_t(40320), // 8
27 uint64_t(362880), // 9
28 uint64_t(3628800), // 10
29 uint64_t(39916800), // 11
30 uint64_t(479001600), // 12
31 uint64_t(6227020800), // 13
32 uint64_t(87178291200), // 14
33 uint64_t(1307674368000), // 15
34 uint64_t(20922789888000), // 16
35 uint64_t(355687428096000), // 17
36 uint64_t(6402373705728000), // 18
37 uint64_t(121645100408832000), // 19
38 uint64_t(2432902008176640000), // 20
39 };
40
41
1/2
✓ Branch 0 taken 25776 times.
✗ Branch 1 not taken.
 25776 if (n < factorials.size())
42 {
43
1/2
✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
 25776 return factorials.at(n);
44 }
45 ✗ return n * factorial(n - 1);
46 }
47
48 ✗ double normalCDF(double value)
49 {
50 ✗ return 0.5 * std::erfc(-value * M_SQRT1_2);
51 }
52
53 1343753 double calcEllipticalIntegral(double phi, double m)
54 {
55 1343753 double origM = m;
56 1343753 double scale = 0.0;
57
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1343753 times.
 1343753 if (m > 1.0)
58 {
59 ✗ scale = std::sqrt(m);
60 ✗ phi = std::asin(scale * std::sin(phi));
61 ✗ m = 1.0 / m;
62 }
63
1/2
✓ Branch 0 taken 1343753 times.
✗ Branch 1 not taken.
 1343753 else if (m < 0.0)
64 {
65 1343753 scale = std::sqrt(1.0 - m);
66 1343753 phi = 0.5 * M_PI - phi;
67 1343753 m /= m - 1.0;
68 }
69 1343753 double a = 0.0;
70 1343753 double eok = 0.0;
71 1343753 double f = 0.0;
72
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1343753 times.
 1343753 if (m == 1.0)
73 {
74 ✗ double per = std::floor((phi + 0.5 * M_PI) / M_PI);
75 ✗ phi = std::sin(phi) * math::sgn(0.5 - std::abs(std::fmod(per, 2.0))) + 2 * per;
76 ✗ a = 0.0;
77 }
78 else
79 { // compute using arithmetic-geometric mean
80 1343753 double sgn = math::sgn(phi);
81 1343753 phi = std::abs(phi),
82 1343753 a = 1.0;
83 1343753 double b = std::sqrt(1.0 - m);
84 1343753 eok = 1.0 - 0.5 * m;
85 1343753 double cs = 0.0;
86 1343753 double twon = 1.0;
87 1343753 double pi2 = 0.5 * M_PI;
88 while (true)
89 { // maximum of 8 passes for 64-bit double
90 6718765 double c = 0.5 * (a - b);
91
2/2
✓ Branch 2 taken 1343753 times.
✓ Branch 3 taken 5375012 times.
 6718765 if (std::abs(c) <= std::numeric_limits<double>::epsilon() * c) { break; }
92 5375012 phi += std::atan((b / a) * std::tan(phi)) + M_PI * std::floor((phi + pi2) / M_PI);
93 5375012 cs += c * std::sin(phi);
94 5375012 eok -= twon * c * c;
95 5375012 twon *= 2.0;
96 5375012 double am = a - c;
97
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 5375012 times.
 5375012 if (am == a) { break; }
98 5375012 b = std::sqrt(a * b);
99 5375012 a = am;
100 5375012 }
101 1343753 double f = sgn * phi / (twon * a);
102 1343753 phi = eok * f + sgn * cs;
103 }
104
105
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1343753 times.
 1343753 if (origM > 1.0)
106 {
107 ✗ phi = (phi - (1.0 - m) * f) / scale;
108 }
109
1/2
✓ Branch 0 taken 1343753 times.
✗ Branch 1 not taken.
 1343753 else if (origM < 0.0)
110 {
111 1343753 phi = (eok * 0.5 * M_PI / a - phi) * scale;
112 }
113
114 1343753 return phi;
115 }
116 } // namespace NAV::math
117