From 7d2d76071d975ab82d27af1b81748c872ad8b2c2 Mon Sep 17 00:00:00 2001 From: Nino Tarantino Date: Tue, 22 Sep 2026 15:52:33 -0500 Subject: [PATCH 01/13] Create unit tests for protected math functions --- models/utilities/CMakeLists.txt | 4 + .../math_utils/include/math_utils_private.hh | 2 +- .../test/frame_transformations_test.cc | 30 ++++ .../math_utils/test/linear_algebra_test.cc | 6 + .../math_utils/test/numerical_methods_test.cc | 54 ++++++ .../test/protection_methods_test.cc | 164 ++++++++++++++++++ 6 files changed, 259 insertions(+), 1 deletion(-) create mode 100644 models/utilities/math_utils/test/frame_transformations_test.cc create mode 100644 models/utilities/math_utils/test/linear_algebra_test.cc create mode 100644 models/utilities/math_utils/test/numerical_methods_test.cc create mode 100644 models/utilities/math_utils/test/protection_methods_test.cc diff --git a/models/utilities/CMakeLists.txt b/models/utilities/CMakeLists.txt index f946e73e..745123ba 100644 --- a/models/utilities/CMakeLists.txt +++ b/models/utilities/CMakeLists.txt @@ -109,5 +109,9 @@ add_cml_tests( SOURCES cml_message/test/cml_message_test.cc + math_utils/test/frame_transformations_test.cc + math_utils/test/linear_algebra_test.cc + math_utils/test/protection_methods_test.cc + math_utils/test/numerical_methods_test.cc table_interp_cpp/test/table_independent_variable_test.cc ) diff --git a/models/utilities/math_utils/include/math_utils_private.hh b/models/utilities/math_utils/include/math_utils_private.hh index 16e22ee9..b937690a 100644 --- a/models/utilities/math_utils/include/math_utils_private.hh +++ b/models/utilities/math_utils/include/math_utils_private.hh @@ -185,8 +185,8 @@ class MathUtilsPrivate { "Log of negative number.\n", "The log of ", val, " is undefined,\n" "and the result is set as ", failed_val, "."); - res = failed_val; } + res = failed_val; } feenableexcept(fe_prev); // restore the previous settings of fp exceptions diff --git a/models/utilities/math_utils/test/frame_transformations_test.cc b/models/utilities/math_utils/test/frame_transformations_test.cc new file mode 100644 index 00000000..cedcae4d --- /dev/null +++ b/models/utilities/math_utils/test/frame_transformations_test.cc @@ -0,0 +1,30 @@ +#include "../include/math_utils.hh" +#include "mocks/cml/cml_message_mock.hh" + +#include +#include +#include + +TEST(MathUtils, GenerateInertialToLvlh) { + +} + +TEST(MathUtils, GenerateInertialToUvw) { + +} + +TEST(MathUtils, GenerateInertialToReference) { + +} + +TEST(MathUtils, GenerateInertialToVnc) { + +} + +TEST(MathUtils, GenerateTransPfixToEnu) { + +} + +TEST(MathUtils, GenerateQuatEnuToPfix) { + +} diff --git a/models/utilities/math_utils/test/linear_algebra_test.cc b/models/utilities/math_utils/test/linear_algebra_test.cc new file mode 100644 index 00000000..ddc19104 --- /dev/null +++ b/models/utilities/math_utils/test/linear_algebra_test.cc @@ -0,0 +1,6 @@ +#include "../include/math_utils.hh" +#include "mocks/cml/cml_message_mock.hh" + +#include +#include +#include diff --git a/models/utilities/math_utils/test/numerical_methods_test.cc b/models/utilities/math_utils/test/numerical_methods_test.cc new file mode 100644 index 00000000..40b84c5f --- /dev/null +++ b/models/utilities/math_utils/test/numerical_methods_test.cc @@ -0,0 +1,54 @@ +#include "../include/math_utils.hh" +#include "mocks/cml/cml_message_mock.hh" + +#include +#include +#include + +TEST(MathUtils, IsNearEqual) { + +} + +TEST(MathUtils, IsEqual) { + +} + +TEST(MathUtils, HasChangedFrom) { + +} + +TEST(MathUtils, IsWithinRange) { + +} + +TEST(MathUtils, IsWithinAbsoluteTolerance) { + +} + +TEST(MathUtils, IsWithinRelativeTolerance) { + +} + +// Test the function which returns the sign of the input value. +TEST(MathUtils, Sign) { + EXPECT_EQ(MathUtils::sign(-5.0), -1); + EXPECT_EQ(MathUtils::sign(-1.0f), -1); + EXPECT_EQ(MathUtils::sign(-10), -1); + + EXPECT_EQ(MathUtils::sign(5.0), 1); + EXPECT_EQ(MathUtils::sign(1.0f), 1); + EXPECT_EQ(MathUtils::sign(10), 1); + + EXPECT_EQ(MathUtils::sign(0.0), 0); + EXPECT_EQ(MathUtils::sign(-0.0), 0); + EXPECT_EQ(MathUtils::sign(-0.0f), 0); + EXPECT_EQ(MathUtils::sign(0), 0); +} + +TEST(MathUtils, Polynomial) { + +} + +TEST(MathUtils, QuadraticSolver) { + +} diff --git a/models/utilities/math_utils/test/protection_methods_test.cc b/models/utilities/math_utils/test/protection_methods_test.cc new file mode 100644 index 00000000..143a451d --- /dev/null +++ b/models/utilities/math_utils/test/protection_methods_test.cc @@ -0,0 +1,164 @@ +#include "../include/math_utils.hh" +#include "mocks/cml/cml_message_mock.hh" + +#include +#include +#include + +namespace { + +// Floating point comparison tolerances. +inline constexpr double double_tolerance = 1e-12; +inline constexpr float float_tolerance = 1e-4; + +// Test the protected division function. +TEST(MathUtils, DivideProtected) { + using testing::_; + using testing::HasSubstr; + + // Double-precision values. + { + CMLMessage::Mock cml_message_mock; + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("Divided by zero or overflow"))).Times(1); + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Fail, _, _, HasSubstr("Divided by zero or overflow"))).Times(1); + + ASSERT_NEAR(MathUtils::divide_protected(15.0, 5.0), 3.0, double_tolerance); + ASSERT_NEAR(MathUtils::divide_protected(1.0, 0.0, 70.0), 70.0, double_tolerance); + ASSERT_NEAR(MathUtils::divide_protected(1.0e-300, 1.0e300, 2.0), 0.0, double_tolerance); + ASSERT_NEAR(MathUtils::divide_protected(1.0e38, 1.0e-38, 35.0), 1e76, double_tolerance); + ASSERT_NEAR(MathUtils::divide_protected(1.0e300, 1.0e-300, 35.0, true), 35.0, double_tolerance); + } + + // Single-precision values. + { + CMLMessage::Mock cml_message_mock; + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("Divided by zero or overflow"))).Times(1); + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Fail, _, _, HasSubstr("Divided by zero or overflow"))).Times(1); + + ASSERT_NEAR(MathUtils::divide_protected(15.0f, 5.0f), 3.0f, float_tolerance); + ASSERT_NEAR(MathUtils::divide_protected(1.0f, 0.0f, 3.14f), 3.14f, float_tolerance); + ASSERT_NEAR(MathUtils::divide_protected(1.0e-38f, 1.0e38f, 2.0f), 0.0f, float_tolerance); + ASSERT_NEAR(MathUtils::divide_protected(1.0e38f, 1.0e-38f, 5.45678f, true), 5.45678, float_tolerance); + } +} + +// Test the protected arccosine function. +TEST(MathUtils, AcosProtected) { + ASSERT_NEAR(MathUtils::acos_protected(0.877582561890373), 0.5, double_tolerance); + ASSERT_NEAR(MathUtils::acos_protected(-1.1), M_PI, double_tolerance); + ASSERT_NEAR(MathUtils::acos_protected(1.1), 0.0, double_tolerance); + + ASSERT_NEAR(MathUtils::acos_protected(0.877582561890373f), 0.5f, float_tolerance); + ASSERT_NEAR(MathUtils::acos_protected(-1.1f), static_cast(M_PI), float_tolerance); + ASSERT_NEAR(MathUtils::acos_protected(1.1f), 0.0f, float_tolerance); +} + +// Test the protected arcsine function. +TEST(MathUtils, AsinProtected) { + ASSERT_NEAR(MathUtils::asin_protected(0.479425538604203), 0.5, double_tolerance); + ASSERT_NEAR(MathUtils::asin_protected(-1.1), -M_PI_2, double_tolerance); + ASSERT_NEAR(MathUtils::asin_protected(1.1), M_PI_2, double_tolerance); + + ASSERT_NEAR(MathUtils::asin_protected(0.479425538604203f), 0.5f, float_tolerance); + ASSERT_NEAR(MathUtils::asin_protected(-1.1f), static_cast(-M_PI_2), float_tolerance); + ASSERT_NEAR(MathUtils::asin_protected(1.1f), static_cast(M_PI_2), float_tolerance); +} + +// Test the protected square root function. +TEST(MathUtils, SqrtProtected) { + using testing::_; + using testing::HasSubstr; + + CMLMessage::Mock cml_message_mock; + + // Square root of a positive number yields the same result. + ASSERT_NEAR(MathUtils::sqrt_protected(0.0), 0.0, double_tolerance); + ASSERT_NEAR(MathUtils::sqrt_protected(4.0), 2.0, double_tolerance); + ASSERT_NEAR(MathUtils::sqrt_protected(16.0), 4.0, double_tolerance); + + ASSERT_NEAR(MathUtils::sqrt_protected(0.0f), 0.0f, float_tolerance); + ASSERT_NEAR(MathUtils::sqrt_protected(4.0f), 2.0f, float_tolerance); + ASSERT_NEAR(MathUtils::sqrt_protected(16.0f), 4.0f, float_tolerance); + + // Square root of a negative number emits an error and returns 0. + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("Cannot take square root of a negative value"))).Times(2); + ASSERT_EQ(MathUtils::sqrt_protected(-5.0), 0.0); + ASSERT_EQ(MathUtils::sqrt_protected(-5.0f), 0.0f); +} + +// Test the protected logarithm function. +TEST(MathUtils, LogProtected) { + using testing::_; + using testing::HasSubstr; + + // Double-precision values. + { + CMLMessage::Mock cml_message_mock; + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("Log of negative number"))).Times(1); + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Fail, _, _, HasSubstr("Log of negative number"))).Times(1); + + ASSERT_NEAR(MathUtils::log_protected(1.0), 0.0, double_tolerance); + ASSERT_NEAR(MathUtils::log_protected(M_E), 1.0, double_tolerance); + ASSERT_NEAR(MathUtils::log_protected(1.0e-308), -709.1962086421661, double_tolerance); + ASSERT_NEAR(MathUtils::log_protected(1.0e308), 709.1962086421661, double_tolerance); + ASSERT_NEAR(MathUtils::log_protected(-1.0, 5.0), 5.0, double_tolerance); + ASSERT_NEAR(MathUtils::log_protected(-2.0, 0.0, true), 0.0, double_tolerance); + } + + // Single-precision values. + { + CMLMessage::Mock cml_message_mock; + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("Log of negative number"))).Times(1); + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Fail, _, _, HasSubstr("Log of negative number"))).Times(1); + + ASSERT_NEAR(MathUtils::log_protected(1.0f), 0.0f, float_tolerance); + ASSERT_NEAR(MathUtils::log_protected(static_cast(M_E)), 1.0f, float_tolerance); + ASSERT_NEAR(MathUtils::log_protected(1.0e-38f), -87.49823, float_tolerance); + ASSERT_NEAR(MathUtils::log_protected(1.0e38f), 87.49823, float_tolerance); + ASSERT_NEAR(MathUtils::log_protected(-1.0f, 5.0f), 5.0f, float_tolerance); + ASSERT_NEAR(MathUtils::log_protected(-2.0f, 0.0f, true), 0.0f, float_tolerance); + } +} + +// Test the protected base-10 logarithm function. +TEST(MathUtils, Log10Protected) { + using testing::_; + using testing::HasSubstr; + + // Double precision values. + { + CMLMessage::Mock cml_message_mock; + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("Log of negative number"))).Times(1); + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Fail, _, _, HasSubstr("Log of negative number"))).Times(1); + + ASSERT_NEAR(MathUtils::log10_protected(1.0), 0.0, double_tolerance); + ASSERT_NEAR(MathUtils::log10_protected(10.0), 1.0, double_tolerance); + ASSERT_NEAR(MathUtils::log10_protected(1.0e-308), -308.0, double_tolerance); + ASSERT_NEAR(MathUtils::log10_protected(1.0e308), 308.0, double_tolerance); + ASSERT_NEAR(MathUtils::log10_protected(-4.0, 10.0), 10.0, double_tolerance); + ASSERT_NEAR(MathUtils::log10_protected(0.0, 0.0, true), 0.0, double_tolerance); + } + + // Single precision values. + { + ASSERT_NEAR(MathUtils::log10_protected(1.0f), 0.0f, float_tolerance); + ASSERT_NEAR(MathUtils::log10_protected(10.0f), 1.0f, float_tolerance); + ASSERT_NEAR(MathUtils::log10_protected(1.0e-38f), -38.0f, float_tolerance); + ASSERT_NEAR(MathUtils::log10_protected(1.0e38f), 38.0f, float_tolerance); + ASSERT_NEAR(MathUtils::log10_protected(-4.0f, 10.0f), 10.0f, float_tolerance); + ASSERT_NEAR(MathUtils::log10_protected(0.0f, 0.0f, true), 0.0f, float_tolerance); + } +} + +} From 5404bd15f8a29060a2a2d3c1a06b1cd5e2b1ec86 Mon Sep 17 00:00:00 2001 From: Nino Tarantino Date: Tue, 22 Sep 2026 16:43:32 -0500 Subject: [PATCH 02/13] Move quadratic solver tests to GTest --- models/utilities/CMakeLists.txt | 1 + .../math_utils/test/numerical_methods_test.cc | 4 - .../math_utils/test/quadratic_solver_test.cc | 141 ++++++++++++++++++ 3 files changed, 142 insertions(+), 4 deletions(-) create mode 100644 models/utilities/math_utils/test/quadratic_solver_test.cc diff --git a/models/utilities/CMakeLists.txt b/models/utilities/CMakeLists.txt index 745123ba..aca429ae 100644 --- a/models/utilities/CMakeLists.txt +++ b/models/utilities/CMakeLists.txt @@ -112,6 +112,7 @@ add_cml_tests( math_utils/test/frame_transformations_test.cc math_utils/test/linear_algebra_test.cc math_utils/test/protection_methods_test.cc + math_utils/test/quadratic_solver_test.cc math_utils/test/numerical_methods_test.cc table_interp_cpp/test/table_independent_variable_test.cc ) diff --git a/models/utilities/math_utils/test/numerical_methods_test.cc b/models/utilities/math_utils/test/numerical_methods_test.cc index 40b84c5f..260ad96d 100644 --- a/models/utilities/math_utils/test/numerical_methods_test.cc +++ b/models/utilities/math_utils/test/numerical_methods_test.cc @@ -48,7 +48,3 @@ TEST(MathUtils, Sign) { TEST(MathUtils, Polynomial) { } - -TEST(MathUtils, QuadraticSolver) { - -} diff --git a/models/utilities/math_utils/test/quadratic_solver_test.cc b/models/utilities/math_utils/test/quadratic_solver_test.cc new file mode 100644 index 00000000..2d0261f8 --- /dev/null +++ b/models/utilities/math_utils/test/quadratic_solver_test.cc @@ -0,0 +1,141 @@ +#include "../include/quadratic_solver.hh" + +#include + +namespace { + +// Floating point comparison tolerance +inline constexpr double tolerance = 1e-12; + +// Test the quadratic solver's root finding math. +TEST(QuadraticSolver, ComputeRoots) { + // x^2-3x+2=0 + { + QuadraticSolver article(1.0, -3.0, 2.0); + const bool success = article.solve(); + EXPECT_NEAR(article.root1, 1.0, tolerance); + EXPECT_NEAR(article.root2, 2.0, tolerance); + EXPECT_TRUE(article.roots_exist); + EXPECT_TRUE(success); + } + + // x^2+2x+1=0 + { + QuadraticSolver article(1.0, 2.0, 1.0); + const bool success = article.solve(); + EXPECT_NEAR(article.root1, -1.0, tolerance); + EXPECT_NEAR(article.root2, -1.0, tolerance); + EXPECT_TRUE(article.roots_exist); + EXPECT_TRUE(success); + } + + // 2x+1=0 + { + QuadraticSolver article(0.0, 2.0, 1.0); + const bool success = article.solve(); + EXPECT_NEAR(article.root1, -0.5, tolerance); + EXPECT_NEAR(article.root2, -0.5, tolerance); + EXPECT_TRUE(article.roots_exist); + EXPECT_TRUE(success); + } + + // x^2-1=0 + { + QuadraticSolver article(1.0, 0.0, -1.0); + const bool success = article.solve(); + EXPECT_NEAR(article.root1, -1.0, tolerance); + EXPECT_NEAR(article.root2, 1.0, tolerance); + EXPECT_TRUE(article.roots_exist); + EXPECT_TRUE(success); + } + + // x^2+3x=0 + { + QuadraticSolver article(1.0, 3.0, 0.0); + const bool success = article.solve(); + EXPECT_NEAR(article.root1, 0.0, tolerance); + EXPECT_NEAR(article.root2, -3.0, tolerance); + EXPECT_TRUE(article.roots_exist); + EXPECT_TRUE(success); + } + + // 1=0 + { + QuadraticSolver article(0.0, 0.0, 1.0); + const bool success = article.solve(); + EXPECT_NEAR(article.root1, 0.0, tolerance); + EXPECT_NEAR(article.root2, 0.0, tolerance); + EXPECT_FALSE(article.roots_exist); + EXPECT_FALSE(success); + } + + // 3x=0 + { + QuadraticSolver article(0.0, 3.0, 0.0); + const bool success = article.solve(); + EXPECT_NEAR(article.root1, 0.0, tolerance); + EXPECT_NEAR(article.root2, 0.0, tolerance); + EXPECT_TRUE(article.roots_exist); + EXPECT_TRUE(success); + } + + // 3x^2=0 + { + QuadraticSolver article(3.0, 0.0, 0.0); + const bool success = article.solve(); + EXPECT_NEAR(article.root1, 0.0, tolerance); + EXPECT_NEAR(article.root2, 0.0, tolerance); + EXPECT_TRUE(article.roots_exist); + EXPECT_TRUE(success); + } + + // 0=0 + { + QuadraticSolver article(0.0, 0.0, 0.0); + const bool success = article.solve(); + EXPECT_NEAR(article.root1, 0.0, tolerance); + EXPECT_NEAR(article.root2, 0.0, tolerance); + EXPECT_FALSE(article.roots_exist); + EXPECT_FALSE(success); + } + + // x^2+(1e308)x+1=0 + { + QuadraticSolver article(1.0, 1e308, 1.0); + const bool success = article.solve(); + EXPECT_NEAR(article.root1, -1e-308, tolerance); + EXPECT_NEAR(article.root2, -1e308, tolerance); + EXPECT_TRUE(article.roots_exist); + EXPECT_TRUE(success); + } +} + +// Test the quadratic solver's logic for finding whether roots exist, but not +// actually solving for them. +TEST(QuadraticSolver, DontComputeRoots) { + // x^2-3x+2=0 + { + QuadraticSolver article(1.0, 3.0, 2.0, false); + const bool success = article.solve(false); + EXPECT_TRUE(article.roots_exist); + EXPECT_TRUE(success); + + // Roots aren't calculated. They'd be -2 and -1 if we solved for them. + EXPECT_NEAR(article.root1, 0.0, tolerance); + EXPECT_NEAR(article.root2, 0.0, tolerance); + } + + // 2x+1=0 + { + QuadraticSolver article(0.0, 2.0, 1.0, false); + const bool success = article.solve(false); + EXPECT_TRUE(article.roots_exist); + EXPECT_TRUE(success); + + // Roots aren't calculated. They'd be -0.5 and -0.5 if we solved for them. + EXPECT_NEAR(article.root1, 0.0, tolerance); + EXPECT_NEAR(article.root2, 0.0, tolerance); + } +} + +} From a131056a055fd2321640715b462ad24cf7b16e4b Mon Sep 17 00:00:00 2001 From: Nino Tarantino Date: Tue, 22 Sep 2026 17:47:01 -0500 Subject: [PATCH 03/13] Fill out numerical method tests --- .../math_utils/test/numerical_methods_test.cc | 139 +++++++++++++++++- utility/mocks/cml/cml_message_mock.hh | 2 +- 2 files changed, 134 insertions(+), 7 deletions(-) diff --git a/models/utilities/math_utils/test/numerical_methods_test.cc b/models/utilities/math_utils/test/numerical_methods_test.cc index 260ad96d..1d51b159 100644 --- a/models/utilities/math_utils/test/numerical_methods_test.cc +++ b/models/utilities/math_utils/test/numerical_methods_test.cc @@ -4,29 +4,111 @@ #include #include #include +#include -TEST(MathUtils, IsNearEqual) { +namespace { +// Test the is_near_equal function. +TEST(MathUtils, IsNearEqual) { + // Double precision values. + EXPECT_TRUE(MathUtils::is_near_equal(3.45, 3.45)); + EXPECT_TRUE(MathUtils::is_near_equal(1.0000000000000001, 1.0)); + EXPECT_FALSE(MathUtils::is_near_equal(1.000000000000001, 1.0)); + EXPECT_FALSE(MathUtils::is_near_equal(5.4321, 5.4322, 1.0)); + EXPECT_FALSE(MathUtils::is_near_equal(5.4321, 5.4322, 1e7)); + EXPECT_TRUE(MathUtils::is_near_equal(5.4321, 5.4322, 1e15)); + + // Single precision values. + EXPECT_TRUE(MathUtils::is_near_equal(3.45f, 3.45f)); + EXPECT_TRUE(MathUtils::is_near_equal(1.00000001f, 1.0f)); + EXPECT_FALSE(MathUtils::is_near_equal(1.0000001f, 1.0f)); + EXPECT_FALSE(MathUtils::is_near_equal(5.4321f, 5.4322f, 1.0f)); + EXPECT_TRUE(MathUtils::is_near_equal(5.4321f, 5.4322f, 1e7f)); } +// Test the is_equal function. TEST(MathUtils, IsEqual) { + EXPECT_TRUE(MathUtils::is_equal(3, 3)); + EXPECT_FALSE(MathUtils::is_equal(3, 4)); -} + EXPECT_TRUE(MathUtils::is_equal(true, true)); + EXPECT_FALSE(MathUtils::is_equal(true, false)); -TEST(MathUtils, HasChangedFrom) { + EXPECT_TRUE(MathUtils::is_equal(3.45, 3.45)); + EXPECT_FALSE(MathUtils::is_equal(3.45, 3.56)); + EXPECT_TRUE(MathUtils::is_equal(3.45f, 3.45f)); + EXPECT_FALSE(MathUtils::is_equal(3.45f, 3.56f)); } -TEST(MathUtils, IsWithinRange) { +// Test the has_changed_from function. +TEST(MathUtils, HasChangedFrom) { + EXPECT_TRUE(MathUtils::has_changed_from(3.45, 4.56)); + EXPECT_TRUE(MathUtils::has_changed_from(4.56, 3.45)); + EXPECT_FALSE(MathUtils::has_changed_from(3.45, 3.45)); + EXPECT_TRUE(MathUtils::has_changed_from(3.45f, 4.56f)); + EXPECT_TRUE(MathUtils::has_changed_from(4.56f, 3.45f)); + EXPECT_FALSE(MathUtils::has_changed_from(3.45f, 3.45f)); } +// Test the absolute tolerance comparison function. TEST(MathUtils, IsWithinAbsoluteTolerance) { - + using testing::_; + using testing::HasSubstr; + + testing::StrictMock cml_message_mock; + + // Nominal tests. + EXPECT_TRUE(MathUtils::is_within_abs_tolerance(3.45, 3.46, 0.01)); + EXPECT_FALSE(MathUtils::is_within_abs_tolerance(3.45, 3.46, 0.0099)); + EXPECT_TRUE(MathUtils::is_within_abs_tolerance(3, -2, 5)); + EXPECT_TRUE(MathUtils::is_within_abs_tolerance(3U, 2U, 2U)); + EXPECT_FALSE(MathUtils::is_within_abs_tolerance(-2.0f, -2.01f, 0.0099f)); + + // Boolean cases. + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("ambiguous interpretation"))).Times(5); + EXPECT_TRUE(MathUtils::is_within_abs_tolerance(true, true, true)); + EXPECT_TRUE(MathUtils::is_within_abs_tolerance(true, true, false)); + EXPECT_FALSE(MathUtils::is_within_abs_tolerance(true, false, false)); + EXPECT_TRUE(MathUtils::is_within_abs_tolerance(false, false, true)); + EXPECT_TRUE(MathUtils::is_within_abs_tolerance(false, false, false)); + + // A negative absolute tolerance will emit an error. + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("not well-defined when the tolerance is negative"))).Times(1); + EXPECT_FALSE(MathUtils::is_within_abs_tolerance(-2.0f, -2.01f, -1.0f)); } +// Test the relative tolerance comparison function. TEST(MathUtils, IsWithinRelativeTolerance) { - + using testing::_; + using testing::HasSubstr; + + testing::StrictMock cml_message_mock; + + + // Nominal tests. + EXPECT_TRUE(MathUtils::is_within_rel_tolerance(1.65, 1.5, 0.1)); + EXPECT_FALSE(MathUtils::is_within_rel_tolerance(1.65, 1.5, 0.09999)); + EXPECT_TRUE(MathUtils::is_within_rel_tolerance(2, -2, 2.0)); + EXPECT_FALSE(MathUtils::is_within_rel_tolerance(10U, 2U, 2.0)); + EXPECT_TRUE(MathUtils::is_within_rel_tolerance(-1.65f, -1.5f, 0.1f)); + + // Boolean tests. + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("ambiguous interpretation"))).Times(5); + EXPECT_TRUE(MathUtils::is_within_rel_tolerance(true, true, true)); + EXPECT_TRUE(MathUtils::is_within_rel_tolerance(true, true, false)); + EXPECT_FALSE(MathUtils::is_within_rel_tolerance(true, false, false)); + EXPECT_TRUE(MathUtils::is_within_rel_tolerance(false, false, true)); + EXPECT_TRUE(MathUtils::is_within_rel_tolerance(false, false, false)); + + // A negative relative tolerance will emit an error. + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("not well-defined when the tolerance is negative"))).Times(1); + EXPECT_FALSE(MathUtils::is_within_rel_tolerance(-2.0f, -2.01f, -1.0)); } // Test the function which returns the sign of the input value. @@ -45,6 +127,51 @@ TEST(MathUtils, Sign) { EXPECT_EQ(MathUtils::sign(0), 0); } +// Test the polynomial evaluation math. TEST(MathUtils, Polynomial) { + using testing::_; + using testing::HasSubstr; + + // Floating point comparison tolerance. + constexpr double tolerance = 1e-12; + + // Nominal case: y = 5x^2 + 4x + 3, y(2) = 31 + { + const std::vector coeffs {3.0, 4.0, 5.0}; + EXPECT_NEAR(MathUtils::polynomial(2.0, coeffs), 31.0, tolerance); + } + + // Overflow case, use fallback value. + { + CMLMessage::Mock cml_message_mock; + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("The polynomial sum is overflow"))); + + const std::vector coeffs {3.0, 4.0, 5.0}; + EXPECT_NEAR(MathUtils::polynomial(1e308, coeffs, 75.7575), 75.7575, tolerance); + } + + // Test evaluation at a very small value. + { + const std::vector coeffs {0.0, 4.0, 5.0}; + EXPECT_NEAR(MathUtils::polynomial(-1e-308, coeffs), -4e-308, tolerance); + } + + // Higher order polynomial. + { + const std::vector coeffs {0.0, 4.0, 5.0, -1.0, -1.0}; + EXPECT_NEAR(MathUtils::polynomial(-2.0, coeffs), 4.0, tolerance); + } + + // Overflow case with failure enabled. + { + CMLMessage::Mock cml_message_mock; + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Fail, _, _, HasSubstr("The polynomial sum is overflow"))); + + const std::vector coeffs {5.0, 4.0, 3.0}; + EXPECT_NEAR(MathUtils::polynomial(1e308, coeffs, 0.0, true), 0.0, tolerance); + } +} } diff --git a/utility/mocks/cml/cml_message_mock.hh b/utility/mocks/cml/cml_message_mock.hh index 37bf7999..3fa5f9e4 100644 --- a/utility/mocks/cml/cml_message_mock.hh +++ b/utility/mocks/cml/cml_message_mock.hh @@ -9,7 +9,7 @@ namespace CMLMessage { // Mock interface to CML's CMLMessage class -class Mock final { +class Mock { public: Mock(); ~Mock(); From 1e34ee7038b4c8f659ceee401a6b2cc55fb8633d Mon Sep 17 00:00:00 2001 From: Nino Tarantino Date: Wed, 23 Sep 2026 11:19:47 -0500 Subject: [PATCH 04/13] Move frame transformation tests to GTest --- models/utilities/math_utils/src/math_utils.cc | 62 +- .../test/frame_transformations_test.cc | 784 ++++++++++++++++++ 2 files changed, 811 insertions(+), 35 deletions(-) diff --git a/models/utilities/math_utils/src/math_utils.cc b/models/utilities/math_utils/src/math_utils.cc index 0598fc34..a6761f00 100644 --- a/models/utilities/math_utils/src/math_utils.cc +++ b/models/utilities/math_utils/src/math_utils.cc @@ -34,7 +34,7 @@ /******************************************************************************* generate_inertial_to_lvlh -Purpose:( Generates the transfromation matrix from inertial to LVLH given +Purpose:( Generates the transformation matrix from inertial to LVLH given a position and velocity expressed in inertial. LVLH is defined as: X - completes @@ -61,7 +61,7 @@ MathUtils::generate_inertial_to_lvlh( const double position[3], __FILE__,__LINE__,"Invalid arguments\n", "The inertial-to-LVLH method cannot function when\n" "the input arguments are NULL.\n" - "Setting transformation matrix to identitiy.\n"); + "Setting transformation matrix to identity.\n"); jeod::Matrix3x3::identity(T_inrtl_lvlh); return; } @@ -138,11 +138,9 @@ MathUtils::generate_inertial_to_lvlh( const double position[3], jeod::Vector3::normalize(x_unit); - for (unsigned int ii = 0; ii < 3; ++ii) { - T_inrtl_lvlh[0][ii] = x_unit[ii]; - T_inrtl_lvlh[1][ii] = y_unit[ii]; - T_inrtl_lvlh[2][ii] = z_unit[ii]; - } + jeod::Vector3::copy(x_unit, T_inrtl_lvlh[0]); + jeod::Vector3::copy(y_unit, T_inrtl_lvlh[1]); + jeod::Vector3::copy(z_unit, T_inrtl_lvlh[2]); } /******************************************************************************* @@ -174,7 +172,7 @@ MathUtils::generate_inertial_to_uvw( const double position[3], __FILE__,__LINE__,"Invalid arguments\n", "The inertial-to-UVW method cannot function when\n" "the input arguments are NULL.\n" - "Setting transformation matrix to identitiy.\n"); + "Setting transformation matrix to identity.\n"); jeod::Matrix3x3::identity(T_inrtl_uvw); return; } @@ -248,11 +246,9 @@ MathUtils::generate_inertial_to_uvw( const double position[3], jeod::Vector3::normalize( w_unit); } jeod::Vector3::normalize( v_unit); - for (unsigned int ii = 0; ii < 3; ++ii) { - T_inrtl_uvw[0][ii] = u_unit[ii]; - T_inrtl_uvw[1][ii] = v_unit[ii]; - T_inrtl_uvw[2][ii] = w_unit[ii]; - } + jeod::Vector3::copy(u_unit, T_inrtl_uvw[0]); + jeod::Vector3::copy(v_unit, T_inrtl_uvw[1]); + jeod::Vector3::copy(w_unit, T_inrtl_uvw[2]); } /******************************************************************************* @@ -361,11 +357,9 @@ MathUtils::generate_inrtl_to_reference( const double x_axis_inrtl[3], // T_inrtl_reference = [ x0 x1 x2] // [ [ y0 y1 y2] ] // [ z0 z1 z2]] - for (unsigned int ii = 0; ii < 3; ii++) { - T_inrtl_reference[0][ii] = x_unit[ii]; - T_inrtl_reference[1][ii] = y_unit[ii]; - T_inrtl_reference[2][ii] = z_unit[ii]; - } + jeod::Vector3::copy(x_unit, T_inrtl_reference[0]); + jeod::Vector3::copy(y_unit, T_inrtl_reference[1]); + jeod::Vector3::copy(z_unit, T_inrtl_reference[2]); } /******************************************************************************* @@ -435,11 +429,9 @@ MathUtils::generate_inrtl_to_vnc( const double (&position)[3], // T_inrtl_vnc = [ x0 x1 x2] // [ [ y0 y1 y2] ] // [ z0 z1 z2]] - for (unsigned int ii = 0; ii < 3; ii++) { - T_inrtl_vnc[0][ii] = x_unit[ii]; - T_inrtl_vnc[1][ii] = y_unit[ii]; - T_inrtl_vnc[2][ii] = z_unit[ii]; - } + jeod::Vector3::copy(x_unit, T_inrtl_vnc[0]); + jeod::Vector3::copy(y_unit, T_inrtl_vnc[1]); + jeod::Vector3::copy(z_unit, T_inrtl_vnc[2]); } /***************************************************************************** @@ -480,15 +472,15 @@ MathUtils::generate_T_pfix_to_enu( const double position_pfix[3], __FILE__,__LINE__,"Invalid Position\n", "Position vector is NULL.\n" "Cannot generate the ENU frame.\n" - "Setting transformation matrix to identitiy.\n"); + "Setting transformation matrix to identity.\n"); jeod::Matrix3x3::identity(T_pfix_to_enu); return; } // Create 3-arrays to express the 3 axes of ENU in the ECEF frame - double east_hat[3]={0}; - double north_hat[3]={0}; - double up_hat[3]={0}; + double east_hat[3] {}; + double north_hat[3] {}; + double up_hat[3] {}; // Up is the position vector, passed in as an argument. Need to normalize // this vector to get a unit-vector. @@ -502,7 +494,8 @@ MathUtils::generate_T_pfix_to_enu( const double position_pfix[3], "Aligning east with pfix +x\n" " north to complete.\n"); east_hat[0] = 1.0; - north_hat[1] = up_hat[2] = (up_hat[2]>0) ? 1.0 : -1.0; + up_hat[2] = std::copysign(1.0, up_hat[2]); + north_hat[1] = up_hat[2]; } else { // East unit-vector is the normalization of (ECEF-z) x (up). @@ -573,11 +566,10 @@ MathUtils::generate_Q_enu_to_pfix( double longitude, longitude += M_PI; } - jeod::Quaternion Q_enu_to_uen; - Q_enu_to_uen.scalar = - Q_enu_to_uen.vector[0] = - Q_enu_to_uen.vector[1] = + Q_enu_to_uen.scalar = 0.5; + Q_enu_to_uen.vector[0] = 0.5; + Q_enu_to_uen.vector[1] = 0.5; Q_enu_to_uen.vector[2] = 0.5; // equatorial East-y frame (equEy) is the rotation of the UEN frame such that @@ -622,7 +614,7 @@ MathUtils::polynomial( double x, x_to_i *= x; } - if (std::isnan(sum) || std::isinf(sum)) { //chec invalid ops and overflow + if (std::isnan(sum) || std::isinf(sum)) { //check invalid ops and overflow if (failed_flag) { CMLMessage::fail( __FILE__, __LINE__,"Overflow value detected.\n", @@ -848,7 +840,7 @@ MathUtils::cholesky_decomposition ( const std::string & caller_id, // -- the element in row (ii), and // -- the element in this row // for every column to the left of the current column. - // This is analgous to subtracting off the scalar product of the row(ii) + // This is analogous to subtracting off the scalar product of the row(ii) // and this row for elements to the left. // These values have already been computed because values are computed // for all rows as each column is processed, moving to the right. @@ -1023,4 +1015,4 @@ template<> bool MathUtils::is_within_rel_tolerance( bool value, bool expected, double tol) { return is_within_abs_tolerance(value, expected, static_cast(tol)); -} \ No newline at end of file +} diff --git a/models/utilities/math_utils/test/frame_transformations_test.cc b/models/utilities/math_utils/test/frame_transformations_test.cc index cedcae4d..ecdbeddd 100644 --- a/models/utilities/math_utils/test/frame_transformations_test.cc +++ b/models/utilities/math_utils/test/frame_transformations_test.cc @@ -5,26 +5,810 @@ #include #include +namespace { + +// Common input values for each transformation matrix generation test case. For most +// tests, vector1 is position and vector2 is velocity. +inline constexpr double vector1_test_cases[][3] { + {0.0, 0.0, 0.0}, + {200.0, 100.0, 0.0}, + {100.0, 0.0, 0.0}, + {0.0, 0.0, 200.0}, + {200.0, 100.0, 300.0}, + {200.0, 100.0, 300.0}, + {0.0, 0.0, 10.0}, +}; +inline constexpr double vector2_test_cases[][3] { + {10.0, 10.0, 10.0}, + {0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0}, + {10.0, 30.0, 20.0}, + {200.0, 100.0, 300.0}, + {0.0, 0.0, 10.0}, +}; + +// Target to use for identity matrix comparisons. Just to avoid re-creating this +// a bunch of times. +inline constexpr double identity_matrix[3][3] { + {1.0, 0.0, 0.0}, + {0.0, 1.0, 0.0}, + {0.0, 0.0, 1.0} +}; + +// Floating point comparison tolerance. +inline constexpr double tolerance = 1e-12; + +// Function to compare the values between two matrices. +void test_matrices_equal(const double (&lhs)[3][3], const double (&rhs)[3][3]) { + using testing::DoubleNear; + using testing::Pointwise; + for (unsigned int row = 0; row < 3; ++row) { + EXPECT_THAT(lhs[row], Pointwise(DoubleNear(tolerance), rhs[row])); + } +} + +// Test the method which generates an inertial to LVLH frame transformation matrix. TEST(MathUtils, GenerateInertialToLvlh) { + using testing::_; + using testing::HasSubstr; + + testing::StrictMock cml_message_mock; + + // Null pointer cases. + { + const double position[3] {}; + const double velocity[3] {}; + double T_inrtl_lvlh[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("output matrix is NULL"))); + MathUtils::generate_inertial_to_lvlh(position, velocity, nullptr); + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("input arguments are NULL"))).Times(3); + MathUtils::generate_inertial_to_lvlh(nullptr, velocity, T_inrtl_lvlh); + test_matrices_equal(T_inrtl_lvlh, identity_matrix); + + MathUtils::generate_inertial_to_lvlh(position, nullptr, T_inrtl_lvlh); + test_matrices_equal(T_inrtl_lvlh, identity_matrix); + + MathUtils::generate_inertial_to_lvlh(nullptr, nullptr, T_inrtl_lvlh); + test_matrices_equal(T_inrtl_lvlh, identity_matrix); + } + + // Case 1 + { + const double (&position)[3] = vector1_test_cases[0]; + const double (&velocity)[3] = vector2_test_cases[0]; + const double (&expected)[3][3] = identity_matrix; + double T_inrtl_lvlh[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("Aligning LVLH with inertial instead"))); + MathUtils::generate_inertial_to_lvlh(position, velocity, T_inrtl_lvlh); + test_matrices_equal(T_inrtl_lvlh, expected); + } + + // Case 2 + { + const double (&position)[3] = vector1_test_cases[1]; + const double (&velocity)[3] = vector2_test_cases[1]; + constexpr double expected[3][3] { + {0.4472135954999579, -0.8944271909999157, 0.0}, + {0.0, 0.0, 1.0}, + {-0.894427190999916, -0.447213595499958, 0.0} + }; + double T_inrtl_lvlh[3][3] {}; + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("Aligning LVLH-x close to inertial-x"))); + MathUtils::generate_inertial_to_lvlh(position, velocity, T_inrtl_lvlh); + test_matrices_equal(T_inrtl_lvlh, expected); + } + + // Case 3 + { + const double (&position)[3] = vector1_test_cases[2]; + const double (&velocity)[3] = vector2_test_cases[2]; + constexpr double expected[3][3] { + {0.0, 0.0, 1.0}, + {0.0, 1.0, 0.0}, + {-1.0, 0.0, 0.0} + }; + double T_inrtl_lvlh[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("Aligning LVLH-y close to inertial-y"))); + MathUtils::generate_inertial_to_lvlh(position, velocity, T_inrtl_lvlh); + test_matrices_equal(T_inrtl_lvlh, expected); + } + + // Case 4 + { + const double (&position)[3] = vector1_test_cases[3]; + const double (&velocity)[3] = vector2_test_cases[3]; + constexpr double expected[3][3] { + {1.0, 0.0, 0.0}, + {0.0, -1.0, 0.0}, + {0.0, 0.0, -1.0} + }; + double T_inrtl_lvlh[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("Aligning LVLH-x close to inertial-x"))); + MathUtils::generate_inertial_to_lvlh(position, velocity, T_inrtl_lvlh); + test_matrices_equal(T_inrtl_lvlh, expected); + } + + // Case 5 + { + const double (&position)[3] = vector1_test_cases[4]; + const double (&velocity)[3] = vector2_test_cases[4]; + constexpr double expected[3][3] { + {-0.2468853599393471, 0.95668076976497, -0.1543033499620919}, + {0.8082903768654761, 0.1154700538379252, -0.5773502691896257}, + {-0.5345224838248488, -0.2672612419124244, -0.8017837257372732} + }; + double T_inrtl_lvlh[3][3] {}; + + MathUtils::generate_inertial_to_lvlh(position, velocity, T_inrtl_lvlh); + test_matrices_equal(T_inrtl_lvlh, expected); + } + + // Case 6 + { + const double (&position)[3] = vector1_test_cases[5]; + const double (&velocity)[3] = vector2_test_cases[5]; + constexpr double expected[3][3] { + {0.8451542547285165, -0.1690308509457033, -0.5070925528371099}, + {0.0, -0.9486832980505137, 0.3162277660168379}, + {-0.5345224838248488, -0.2672612419124244, -0.8017837257372732} + }; + double T_inrtl_lvlh[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("Aligning LVLH-x close to inertial-x"))); + MathUtils::generate_inertial_to_lvlh(position, velocity, T_inrtl_lvlh); + test_matrices_equal(T_inrtl_lvlh, expected); + } + + // Case 7 + { + const double (&position)[3] = vector1_test_cases[6]; + const double (&velocity)[3] = vector2_test_cases[6]; + constexpr double expected[3][3] { + {1.0, 0.0, 0.0}, + {0.0, -1.0, 0.0}, + {0.0, 0.0, -1.0} + }; + double T_inrtl_lvlh[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("Aligning LVLH-x close to inertial-x"))); + MathUtils::generate_inertial_to_lvlh(position, velocity, T_inrtl_lvlh); + test_matrices_equal(T_inrtl_lvlh, expected); + } } +// Test the method which generates an inertial to UVW frame transformation matrix. TEST(MathUtils, GenerateInertialToUvw) { + using testing::_; + using testing::HasSubstr; + + testing::StrictMock cml_message_mock; + + // Null pointer cases. + { + const double position[3] {}; + const double velocity[3] {}; + double T_inrtl_uvw[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("output matrix is NULL"))); + MathUtils::generate_inertial_to_uvw(position, velocity, nullptr); + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("input arguments are NULL"))).Times(3); + MathUtils::generate_inertial_to_uvw(nullptr, velocity, T_inrtl_uvw); + test_matrices_equal(T_inrtl_uvw, identity_matrix); + MathUtils::generate_inertial_to_uvw(position, nullptr, T_inrtl_uvw); + test_matrices_equal(T_inrtl_uvw, identity_matrix); + + MathUtils::generate_inertial_to_uvw(nullptr, nullptr, T_inrtl_uvw); + test_matrices_equal(T_inrtl_uvw, identity_matrix); + } + + // Case 1 + { + const double (&position)[3] = vector1_test_cases[0]; + const double (&velocity)[3] = vector2_test_cases[0]; + const double (&expected)[3][3] = identity_matrix; + double T_inrtl_uvw[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("Aligning UVW with inertial instead"))); + MathUtils::generate_inertial_to_uvw(position, velocity, T_inrtl_uvw); + test_matrices_equal(T_inrtl_uvw, expected); + } + + // Case 2 + { + const double (&position)[3] = vector1_test_cases[1]; + const double (&velocity)[3] = vector2_test_cases[1]; + const double expected[3][3] { + {0.894427190999916, 0.447213595499958, 0.0}, + {-0.447213595499958, 0.894427190999916, 0.0}, + {0.0, 0.0, 1.0} + }; + double T_inrtl_uvw[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("Aligning UVW-w close to inertial-z"))); + MathUtils::generate_inertial_to_uvw(position, velocity, T_inrtl_uvw); + test_matrices_equal(T_inrtl_uvw, expected); + } + + // Case 3 + { + const double (&position)[3] = vector1_test_cases[2]; + const double (&velocity)[3] = vector2_test_cases[2]; + const double (&expected)[3][3] = identity_matrix; + double T_inrtl_uvw[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("Aligning UVW-w close to inertial-z"))); + MathUtils::generate_inertial_to_uvw(position, velocity, T_inrtl_uvw); + test_matrices_equal(T_inrtl_uvw, expected); + } + + // Case 4 + { + const double (&position)[3] = vector1_test_cases[3]; + const double (&velocity)[3] = vector2_test_cases[3]; + const double expected[3][3] { + {0.0, 0.0, 1.0}, + {0.0, -1.0, 0.0}, + {1.0, 0.0, 0.0} + }; + double T_inrtl_uvw[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("Aligning UVW-w close to inertial-x"))); + MathUtils::generate_inertial_to_uvw(position, velocity, T_inrtl_uvw); + test_matrices_equal(T_inrtl_uvw, expected); + } + + // Case 5 + { + const double (&position)[3] = vector1_test_cases[4]; + const double (&velocity)[3] = vector2_test_cases[4]; + const double expected[3][3] { + {0.5345224838248488, 0.2672612419124244, 0.8017837257372732}, + {-0.2468853599393471, 0.95668076976497, -0.1543033499620919}, + {-0.8082903768654761, -0.1154700538379252, 0.5773502691896257} + }; + double T_inrtl_uvw[3][3] {}; + + MathUtils::generate_inertial_to_uvw(position, velocity, T_inrtl_uvw); + test_matrices_equal(T_inrtl_uvw, expected); + } + + // Case 6 + { + const double (&position)[3] = vector1_test_cases[5]; + const double (&velocity)[3] = vector2_test_cases[5]; + const double expected[3][3] { + {0.5345224838248488, 0.2672612419124244, 0.8017837257372732}, + {-0.447213595499958, 0.894427190999916, 0.0}, + {-0.7171371656006362, -0.3585685828003181, 0.5976143046671969} + }; + double T_inrtl_uvw[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("Aligning UVW-w close to inertial-z"))); + MathUtils::generate_inertial_to_uvw(position, velocity, T_inrtl_uvw); + test_matrices_equal(T_inrtl_uvw, expected); + } + + // Case 7 + { + const double (&position)[3] = vector1_test_cases[6]; + const double (&velocity)[3] = vector2_test_cases[6]; + const double expected[3][3] { + {0.0, 0.0, 1.0}, + {0.0, -1.0, 0.0}, + {1.0, 0.0, 0.0} + }; + double T_inrtl_uvw[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("Aligning UVW-w close to inertial-x"))); + MathUtils::generate_inertial_to_uvw(position, velocity, T_inrtl_uvw); + test_matrices_equal(T_inrtl_uvw, expected); + } } +// Test the method which generates an inertial to arbitrary reference frame transformation matrix. TEST(MathUtils, GenerateInertialToReference) { + using testing::_; + using testing::HasSubstr; + + testing::StrictMock cml_message_mock; + + // Null pointer cases. + { + const double x_axis_inertial[3] {}; + const double position[3] {}; + double T_inrtl_reference[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("output matrix is NULL"))); + MathUtils::generate_inrtl_to_reference(x_axis_inertial, position, nullptr); + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("input arguments are NULL"))).Times(3); + MathUtils::generate_inrtl_to_reference(nullptr, position, T_inrtl_reference); + test_matrices_equal(T_inrtl_reference, identity_matrix); + + MathUtils::generate_inrtl_to_reference(x_axis_inertial, nullptr, T_inrtl_reference); + test_matrices_equal(T_inrtl_reference, identity_matrix); + + MathUtils::generate_inrtl_to_reference(nullptr, nullptr, T_inrtl_reference); + test_matrices_equal(T_inrtl_reference, identity_matrix); + } + + // Case 1 + { + const double (&x_axis_inertial)[3] = vector1_test_cases[0]; + const double (&position)[3] = vector2_test_cases[0]; + const double (&expected)[3][3] = identity_matrix; + double T_inrtl_reference[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("Aligning reference ref-frame with inertial frame instead"))); + MathUtils::generate_inrtl_to_reference(x_axis_inertial, position, T_inrtl_reference); + test_matrices_equal(T_inrtl_reference, expected); + } + + // Case 2 + { + const double (&x_axis_inertial)[3] = vector1_test_cases[1]; + const double (&position)[3] = vector2_test_cases[1]; + const double (&expected)[3][3] = identity_matrix; + double T_inrtl_reference[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("Aligning reference ref-frame with inertial frame instead"))); + MathUtils::generate_inrtl_to_reference(x_axis_inertial, position, T_inrtl_reference); + test_matrices_equal(T_inrtl_reference, expected); + } + + // Case 3 + { + const double (&x_axis_inertial)[3] = vector1_test_cases[2]; + const double (&position)[3] = vector2_test_cases[2]; + const double (&expected)[3][3] = identity_matrix; + double T_inrtl_reference[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("Aligning reference ref-frame with inertial frame instead"))); + MathUtils::generate_inrtl_to_reference(x_axis_inertial, position, T_inrtl_reference); + test_matrices_equal(T_inrtl_reference, expected); + } + + // Case 4 + { + const double (&x_axis_inertial)[3] = vector1_test_cases[3]; + const double (&position)[3] = vector2_test_cases[3]; + const double (&expected)[3][3] = identity_matrix; + double T_inrtl_reference[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("Aligning reference ref-frame with inertial frame instead"))); + MathUtils::generate_inrtl_to_reference(x_axis_inertial, position, T_inrtl_reference); + test_matrices_equal(T_inrtl_reference, expected); + } + // Case 5 + { + const double (&x_axis_inertial)[3] = vector1_test_cases[4]; + const double (&position)[3] = vector2_test_cases[4]; + const double expected[3][3] { + {0.5345224838248488, 0.2672612419124244, 0.8017837257372732}, + {-0.8082903768654761, -0.1154700538379251, 0.5773502691896257}, + {0.246885359939347, -0.95668076976497, 0.1543033499620919} + }; + double T_inrtl_reference[3][3] {}; + + MathUtils::generate_inrtl_to_reference(x_axis_inertial, position, T_inrtl_reference); + test_matrices_equal(T_inrtl_reference, expected); + } + + // Case 6 + { + const double (&x_axis_inertial)[3] = vector1_test_cases[5]; + const double (&position)[3] = vector2_test_cases[5]; + const double expected[3][3] { + {0.5345224838248488, 0.2672612419124244, 0.8017837257372732}, + {0.4472135954999579, -0.8944271909999159, 0.0}, + {0.7171371656006362, 0.3585685828003181, -0.5976143046671968} + }; + double T_inrtl_reference[3][3] {}; + + MathUtils::generate_inrtl_to_reference(x_axis_inertial, position, T_inrtl_reference); + test_matrices_equal(T_inrtl_reference, expected); + } + + // Case 7 + { + const double (&x_axis_inertial)[3] = vector1_test_cases[6]; + const double (&position)[3] = vector2_test_cases[6]; + const double expected[3][3] { + {0.0, 0.0, 1.0}, + {0.0, 1.0, 0.0}, + {-1.0, 0.0, 0.0} + }; + double T_inrtl_reference[3][3] {}; + + MathUtils::generate_inrtl_to_reference(x_axis_inertial, position, T_inrtl_reference); + test_matrices_equal(T_inrtl_reference, expected); + } } +// Test the method which generates an inertial to VNC frame transformation matrix. TEST(MathUtils, GenerateInertialToVnc) { + using testing::_; + using testing::HasSubstr; + + testing::StrictMock cml_message_mock; + + // Case 1 + { + const double (&position)[3] = vector1_test_cases[0]; + const double (&velocity)[3] = vector2_test_cases[0]; + const double (&expected)[3][3] = identity_matrix; + double T_inrtl_vnc[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("either the position or velocity is a zero vector"))); + MathUtils::generate_inrtl_to_vnc(position, velocity, T_inrtl_vnc); + test_matrices_equal(T_inrtl_vnc, expected); + } + + // Case 2 + { + const double (&position)[3] = vector1_test_cases[1]; + const double (&velocity)[3] = vector2_test_cases[1]; + const double (&expected)[3][3] = identity_matrix; + double T_inrtl_vnc[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("either the position or velocity is a zero vector"))); + MathUtils::generate_inrtl_to_vnc(position, velocity, T_inrtl_vnc); + test_matrices_equal(T_inrtl_vnc, expected); + } + + // Case 3 + { + const double (&position)[3] = vector1_test_cases[2]; + const double (&velocity)[3] = vector2_test_cases[2]; + const double (&expected)[3][3] = identity_matrix; + double T_inrtl_vnc[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("either the position or velocity is a zero vector"))); + MathUtils::generate_inrtl_to_vnc(position, velocity, T_inrtl_vnc); + test_matrices_equal(T_inrtl_vnc, expected); + } + + // Case 4 + { + const double (&position)[3] = vector1_test_cases[3]; + const double (&velocity)[3] = vector2_test_cases[3]; + const double (&expected)[3][3] = identity_matrix; + double T_inrtl_vnc[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("either the position or velocity is a zero vector"))); + MathUtils::generate_inrtl_to_vnc(position, velocity, T_inrtl_vnc); + test_matrices_equal(T_inrtl_vnc, expected); + } + + // Case 5 + { + const double (&position)[3] = vector1_test_cases[4]; + const double (&velocity)[3] = vector2_test_cases[4]; + const double expected[3][3] { + {0.2672612419124243, 0.8017837257372731, 0.5345224838248487}, + {-0.8082903768654761, -0.1154700538379252, 0.5773502691896257}, + {0.5246313898711126, -0.5863527298559493, 0.6172133998483676} + }; + double T_inrtl_vnc[3][3] {}; + MathUtils::generate_inrtl_to_vnc(position, velocity, T_inrtl_vnc); + test_matrices_equal(T_inrtl_vnc, expected); + } + + // Case 6 + { + const double (&position)[3] = vector1_test_cases[5]; + const double (&velocity)[3] = vector2_test_cases[5]; + const double (&expected)[3][3] = identity_matrix; + double T_inrtl_vnc[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("position and velocity are aligned"))); + MathUtils::generate_inrtl_to_vnc(position, velocity, T_inrtl_vnc); + test_matrices_equal(T_inrtl_vnc, expected); + } + + // Case 7 + { + const double (&position)[3] = vector1_test_cases[6]; + const double (&velocity)[3] = vector2_test_cases[6]; + const double (&expected)[3][3] = identity_matrix; + double T_inrtl_vnc[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("position and velocity are aligned"))); + MathUtils::generate_inrtl_to_vnc(position, velocity, T_inrtl_vnc); + test_matrices_equal(T_inrtl_vnc, expected); + } } +// Test the method which generates a planet-fixed to topocentric East-North-Up frame transformation matrix. TEST(MathUtils, GenerateTransPfixToEnu) { + using testing::_; + using testing::HasSubstr; + + testing::StrictMock cml_message_mock; + + // Null pointer cases. + { + const double position_pfix[3] {}; + double T_pfix_enu[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("output matrix is NULL"))); + MathUtils::generate_T_pfix_to_enu(position_pfix, nullptr); + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("Position vector is NULL"))); + MathUtils::generate_T_pfix_to_enu(nullptr, T_pfix_enu); + test_matrices_equal(T_pfix_enu, identity_matrix); + } + + // Case 1 + { + const double (&position_pfix)[3] = vector1_test_cases[0]; + const double expected[3][3] = {}; + double T_pfix_enu[3][3] {}; + + MathUtils::generate_T_pfix_to_enu(position_pfix, T_pfix_enu); + test_matrices_equal(T_pfix_enu, expected); + } + + // Case 2 + { + const double (&position_pfix)[3] = vector1_test_cases[1]; + const double expected[3][3] = { + {-0.447213595499958, 0.894427190999916, 0.0}, + {0.0, 0.0, 1.0}, + {0.894427190999916, 0.447213595499958, 0.0} + }; + double T_pfix_enu[3][3] {}; + + MathUtils::generate_T_pfix_to_enu(position_pfix, T_pfix_enu); + test_matrices_equal(T_pfix_enu, expected); + } + + // Case 3 + { + const double (&position_pfix)[3] = vector1_test_cases[2]; + const double expected[3][3] = { + {0.0, 1.0, 0.0}, + {0.0, 0.0, 1.0}, + {1.0, 0.0, 0.0} + }; + double T_pfix_enu[3][3] {}; + + MathUtils::generate_T_pfix_to_enu(position_pfix, T_pfix_enu); + test_matrices_equal(T_pfix_enu, expected); + } + // Case 4 + { + const double (&position_pfix)[3] = vector1_test_cases[3]; + const double (&expected)[3][3] = identity_matrix; + double T_pfix_enu[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("Aligning east with pfix +x"))); + MathUtils::generate_T_pfix_to_enu(position_pfix, T_pfix_enu); + test_matrices_equal(T_pfix_enu, expected); + } + + // Case 5 + { + const double (&position_pfix)[3] = vector1_test_cases[4]; + const double expected[3][3] = { + {-0.4472135954999579, 0.8944271909999159, 0.0}, + {-0.7171371656006362, -0.3585685828003181, 0.5976143046671968}, + {0.5345224838248488, 0.2672612419124244, 0.8017837257372732} + }; + double T_pfix_enu[3][3] {}; + + MathUtils::generate_T_pfix_to_enu(position_pfix, T_pfix_enu); + test_matrices_equal(T_pfix_enu, expected); + } + + // Case 6 + { + const double (&position_pfix)[3] = vector1_test_cases[5]; + const double expected[3][3] = { + {-0.4472135954999579, 0.8944271909999159, 0.0}, + {-0.7171371656006362, -0.3585685828003181, 0.5976143046671968}, + {0.5345224838248488, 0.2672612419124244, 0.8017837257372732} + }; + double T_pfix_enu[3][3] {}; + + MathUtils::generate_T_pfix_to_enu(position_pfix, T_pfix_enu); + test_matrices_equal(T_pfix_enu, expected); + } + + // Case 7 + { + const double (&position_pfix)[3] = vector1_test_cases[6]; + const double (&expected)[3][3] = identity_matrix; + double T_pfix_enu[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("Aligning east with pfix +x"))); + MathUtils::generate_T_pfix_to_enu(position_pfix, T_pfix_enu); + test_matrices_equal(T_pfix_enu, expected); + } } +// Test the method which generates a generic East-North-Up to planet-fixed frame transformation quaternion. TEST(MathUtils, GenerateQuatEnuToPfix) { + using testing::_; + using testing::HasSubstr; + + testing::StrictMock cml_message_mock; + + jeod::Quaternion q_enu_to_pfix; + + // Case 1 + MathUtils::generate_Q_enu_to_pfix(0.0, 0.0, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, 0.5, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], 0.5, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], 0.5, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], 0.5, tolerance); + + // Case 2 + MathUtils::generate_Q_enu_to_pfix(1.570796, 0.0, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, 1.15539443734658e-07, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], 1.15539443734658e-07, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], 0.7071067811865381, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], 0.7071067811865381, tolerance); + + // Case 3 + MathUtils::generate_Q_enu_to_pfix(0.0, 1.570796, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, 0.7071067811865381, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], 1.15539443734658e-07, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], 1.15539443734658e-07, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], 0.7071067811865381, tolerance); + + // Case 4 + MathUtils::generate_Q_enu_to_pfix(1.570796, 1.570796, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, 1.633974483494782e-07, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], 2.670086374223501e-14, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], 1.633974483494782e-07, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], 0.9999999999999732, tolerance); + + // Case 5 + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("Latitude too large"))); + MathUtils::generate_Q_enu_to_pfix(1.570796, 1.570797, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, -0.9999999999999302, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], -3.366025517481663e-07, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], 5.498379529456088e-14, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], 1.633974485437673e-07, tolerance); + + // Case 6 + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, HasSubstr("Latitude too small"))); + MathUtils::generate_Q_enu_to_pfix(1.570796, -1.570797, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, -3.366025517204108e-07, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], -0.9999999999999302, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], 1.633974485715228e-07, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], 5.498379529456088e-14, tolerance); + + // Case 7 + MathUtils::generate_Q_enu_to_pfix(0.523599, 0.0, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, 0.3535533218845634, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], 0.3535533218845634, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], 0.6123724753647819, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], 0.6123724753647819, tolerance); + + // Case 8 + MathUtils::generate_Q_enu_to_pfix(0.0, 0.523599, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, 0.6123724753647819, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], 0.3535533218845634, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], 0.3535533218845634, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], 0.6123724753647819, tolerance); + + // Case 9 + MathUtils::generate_Q_enu_to_pfix(0.523599, 0.523599, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, 0.4330126457917832, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], 0.2499999028312194, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], 0.4330126457917831, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], 0.7500000971687807, tolerance); + + // Case 10 + MathUtils::generate_Q_enu_to_pfix(0.785398, 0.0, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, 0.2705981034453612, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], 0.2705981034453612, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], 0.6532814603306707, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], 0.6532814603306707, tolerance); + + // Case 11 + MathUtils::generate_Q_enu_to_pfix(0.0, 0.785398, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, 0.6532814603306707, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], 0.2705981034453612, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], 0.2705981034453612, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], 0.6532814603306707, tolerance); + + // Case 12 + MathUtils::generate_Q_enu_to_pfix(1.0472, 0.0, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, 0.1830118656101681, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], 0.1830118656101681, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], 0.6830129259727709, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], 0.6830129259727709, tolerance); + + // Case 13 + MathUtils::generate_Q_enu_to_pfix(0.0, 1.0472, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, 0.6830129259727709, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], 0.1830118656101681, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], 0.1830118656101681, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], 0.6830129259727709, tolerance); + + // Case 14 + MathUtils::generate_Q_enu_to_pfix(0.0, -1.0472, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, 0.1830118656101681, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], 0.6830129259727709, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], 0.6830129259727709, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], 0.1830118656101681, tolerance); + + // Case 15 + MathUtils::generate_Q_enu_to_pfix(2.0944, -0.523599, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, -0.1294106800827339, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], -0.2241459310253017, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], 0.836515809039842, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], 0.4829625023868248, tolerance); + + // Case 16 + MathUtils::generate_Q_enu_to_pfix(4.1888, 0.0, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, -0.6830135982082818, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], -0.6830135982082818, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], 0.1830093567623682, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], 0.1830093567623682, tolerance); + + // Case 17 + MathUtils::generate_Q_enu_to_pfix(-2.0944, 0.0, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, 0.6830122537280444, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], 0.6830122537280444, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], -0.1830143744554987, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], -0.1830143744554987, tolerance); + + // Case 18 + MathUtils::generate_Q_enu_to_pfix(10.472, 0.0, q_enu_to_pfix); + EXPECT_NEAR(q_enu_to_pfix.scalar, 0.6830149426516572, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[0], 0.6830149426516572, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[1], -0.1830043390593605, tolerance); + EXPECT_NEAR(q_enu_to_pfix.vector[2], -0.1830043390593605, tolerance); +} } From 55cc7c7e8c87744a75e80b5e684970b169395594 Mon Sep 17 00:00:00 2001 From: Nino Tarantino Date: Wed, 23 Sep 2026 12:04:07 -0500 Subject: [PATCH 05/13] Convert ellipsoid intersection tests to GTest --- models/utilities/CMakeLists.txt | 2 + .../test/cholesky_decomposition_test.cc | 12 +++ .../test/ellipsoid_intersection_test.cc | 98 +++++++++++++++++++ .../math_utils/test/linear_algebra_test.cc | 6 ++ 4 files changed, 118 insertions(+) create mode 100644 models/utilities/math_utils/test/cholesky_decomposition_test.cc create mode 100644 models/utilities/math_utils/test/ellipsoid_intersection_test.cc diff --git a/models/utilities/CMakeLists.txt b/models/utilities/CMakeLists.txt index aca429ae..c8b631d6 100644 --- a/models/utilities/CMakeLists.txt +++ b/models/utilities/CMakeLists.txt @@ -109,6 +109,8 @@ add_cml_tests( SOURCES cml_message/test/cml_message_test.cc + math_utils/test/cholesky_decomposition_test.cc + math_utils/test/ellipsoid_intersection_test.cc math_utils/test/frame_transformations_test.cc math_utils/test/linear_algebra_test.cc math_utils/test/protection_methods_test.cc diff --git a/models/utilities/math_utils/test/cholesky_decomposition_test.cc b/models/utilities/math_utils/test/cholesky_decomposition_test.cc new file mode 100644 index 00000000..49275997 --- /dev/null +++ b/models/utilities/math_utils/test/cholesky_decomposition_test.cc @@ -0,0 +1,12 @@ +#include "../include/math_utils.hh" +#include "mocks/cml/cml_message_mock.hh" + +#include +#include +#include + +namespace { + + + +} diff --git a/models/utilities/math_utils/test/ellipsoid_intersection_test.cc b/models/utilities/math_utils/test/ellipsoid_intersection_test.cc new file mode 100644 index 00000000..59221189 --- /dev/null +++ b/models/utilities/math_utils/test/ellipsoid_intersection_test.cc @@ -0,0 +1,98 @@ +#include "../include/ellipsoid_intersection.hh" + +#include +#include +#include + +namespace { + +// Test several updates of the ellipsoid intersection model. +TEST(EllipsoidIntersection, Update) { + using testing::DoubleNear; + using testing::Pointwise; + + // Floating point comparison tolerance. + constexpr double tolerance = 1e-12; + + // Case 1 + { + const double source_point[3] {5.0, 0.0, 0.0}; + const double target_point[3] {-5.0, 0.0, 0.0}; + + EllipsoidIntersection article(source_point, target_point, 2.0, 3.0, 4.0); + const bool intersection = article.update(); + EXPECT_TRUE(intersection); + EXPECT_THAT(article.root1, Pointwise(DoubleNear(tolerance), {2.0, 0.0, 0.0})); + EXPECT_THAT(article.root2, Pointwise(DoubleNear(tolerance), {-2.0, 0.0, 0.0})); + EXPECT_NEAR(article.get_scaled_root1(), 0.3, tolerance); + EXPECT_NEAR(article.get_scaled_root2(), 0.7, tolerance); + } + + // Case 2 + { + const double source_point[3] {0.0, 5.0, 0.0}; + const double target_point[3] {0.0, -5.0, 0.0}; + + EllipsoidIntersection article(source_point, target_point, 2.0, 3.0, 4.0); + const bool intersection = article.update(); + EXPECT_TRUE(intersection); + EXPECT_THAT(article.root1, Pointwise(DoubleNear(tolerance), {0.0, 3.0, 0.0})); + EXPECT_THAT(article.root2, Pointwise(DoubleNear(tolerance), {0.0, -3.0, 0.0})); + EXPECT_NEAR(article.get_scaled_root1(), 0.2, tolerance); + EXPECT_NEAR(article.get_scaled_root2(), 0.8, tolerance); + } + + // Case 3 + { + const double source_point[3] {0.0, 0.0, 5.0}; + const double target_point[3] {0.0, 0.0, -5.0}; + + EllipsoidIntersection article(source_point, target_point, 2.0, 3.0, 4.0); + const bool intersection = article.update(); + EXPECT_TRUE(intersection); + EXPECT_THAT(article.root1, Pointwise(DoubleNear(tolerance), {0.0, 0.0, 4.0})); + EXPECT_THAT(article.root2, Pointwise(DoubleNear(tolerance), {0.0, 0.0, -4.0})); + EXPECT_NEAR(article.get_scaled_root1(), 0.1, tolerance); + EXPECT_NEAR(article.get_scaled_root2(), 0.9, tolerance); + } + + // Case 4 + { + const double source_point[3] {-1.0, 0.0, 4.0}; + const double target_point[3] {1.0, 0.0, 4.0}; + + EllipsoidIntersection article(source_point, target_point, 2.0, 3.0, 4.0); + const bool intersection = article.update(); + EXPECT_TRUE(intersection); + EXPECT_THAT(article.root1, Pointwise(DoubleNear(tolerance), {0.0, 0.0, 4.0})); + EXPECT_THAT(article.root2, Pointwise(DoubleNear(tolerance), {0.0, 0.0, 4.0})); + EXPECT_NEAR(article.get_scaled_root1(), 0.5, tolerance); + EXPECT_NEAR(article.get_scaled_root2(), 0.5, tolerance); + } + + // Case 5 + { + const double source_point[3] {1.0, 1.0, 1.0}; + const double target_point[3] {0.0, 0.0, 1.0}; + + EllipsoidIntersection article(source_point, target_point, 2.0, 3.0, 4.0); + const bool intersection = article.update(); + EXPECT_TRUE(intersection); + EXPECT_THAT(article.root1, Pointwise(DoubleNear(tolerance), {1.611258466588724, 1.611258466588724, 1.0})); + EXPECT_THAT(article.root2, Pointwise(DoubleNear(tolerance), {-1.611258466588724, -1.611258466588724, 1.0})); + EXPECT_NEAR(article.get_scaled_root1(), -0.61125846658872407, tolerance); + EXPECT_NEAR(article.get_scaled_root2(), 2.611258466588724, tolerance); + } + + // Case 6 + { + const double source_point[3] {1.5, 2.5, 0.0}; + const double target_point[3] {1.5, 2.5, 1.0}; + + EllipsoidIntersection article(source_point, target_point, 2.0, 3.0, 4.0); + const bool intersection = article.update(false); + EXPECT_FALSE(intersection); + } +} + +} diff --git a/models/utilities/math_utils/test/linear_algebra_test.cc b/models/utilities/math_utils/test/linear_algebra_test.cc index ddc19104..49275997 100644 --- a/models/utilities/math_utils/test/linear_algebra_test.cc +++ b/models/utilities/math_utils/test/linear_algebra_test.cc @@ -4,3 +4,9 @@ #include #include #include + +namespace { + + + +} From 558aee17401631b11f0a8a1bc1ab8865fac26828 Mon Sep 17 00:00:00 2001 From: Nino Tarantino Date: Wed, 23 Sep 2026 13:12:32 -0500 Subject: [PATCH 06/13] Rename math utils test files --- models/utilities/CMakeLists.txt | 11 ++++++----- ...t.cc => math_utils_cholesky_decomposition_test.cc} | 0 ...st.cc => math_utils_frame_transformations_test.cc} | 0 ...ebra_test.cc => math_utils_linear_algebra_test.cc} | 0 ...s_test.cc => math_utils_numerical_methods_test.cc} | 0 ..._test.cc => math_utils_protection_methods_test.cc} | 0 .../utilities/math_utils/test/std_array_ops_test.cc | 0 7 files changed, 6 insertions(+), 5 deletions(-) rename models/utilities/math_utils/test/{cholesky_decomposition_test.cc => math_utils_cholesky_decomposition_test.cc} (100%) rename models/utilities/math_utils/test/{frame_transformations_test.cc => math_utils_frame_transformations_test.cc} (100%) rename models/utilities/math_utils/test/{linear_algebra_test.cc => math_utils_linear_algebra_test.cc} (100%) rename models/utilities/math_utils/test/{numerical_methods_test.cc => math_utils_numerical_methods_test.cc} (100%) rename models/utilities/math_utils/test/{protection_methods_test.cc => math_utils_protection_methods_test.cc} (100%) create mode 100644 models/utilities/math_utils/test/std_array_ops_test.cc diff --git a/models/utilities/CMakeLists.txt b/models/utilities/CMakeLists.txt index c8b631d6..09faaf2d 100644 --- a/models/utilities/CMakeLists.txt +++ b/models/utilities/CMakeLists.txt @@ -109,12 +109,13 @@ add_cml_tests( SOURCES cml_message/test/cml_message_test.cc - math_utils/test/cholesky_decomposition_test.cc math_utils/test/ellipsoid_intersection_test.cc - math_utils/test/frame_transformations_test.cc - math_utils/test/linear_algebra_test.cc - math_utils/test/protection_methods_test.cc + math_utils/test/math_utils_cholesky_decomposition_test.cc + math_utils/test/math_utils_frame_transformations_test.cc + math_utils/test/math_utils_linear_algebra_test.cc + math_utils/test/math_utils_numerical_methods_test.cc + math_utils/test/math_utils_protection_methods_test.cc math_utils/test/quadratic_solver_test.cc - math_utils/test/numerical_methods_test.cc + math_utils/test/std_array_ops_test.cc table_interp_cpp/test/table_independent_variable_test.cc ) diff --git a/models/utilities/math_utils/test/cholesky_decomposition_test.cc b/models/utilities/math_utils/test/math_utils_cholesky_decomposition_test.cc similarity index 100% rename from models/utilities/math_utils/test/cholesky_decomposition_test.cc rename to models/utilities/math_utils/test/math_utils_cholesky_decomposition_test.cc diff --git a/models/utilities/math_utils/test/frame_transformations_test.cc b/models/utilities/math_utils/test/math_utils_frame_transformations_test.cc similarity index 100% rename from models/utilities/math_utils/test/frame_transformations_test.cc rename to models/utilities/math_utils/test/math_utils_frame_transformations_test.cc diff --git a/models/utilities/math_utils/test/linear_algebra_test.cc b/models/utilities/math_utils/test/math_utils_linear_algebra_test.cc similarity index 100% rename from models/utilities/math_utils/test/linear_algebra_test.cc rename to models/utilities/math_utils/test/math_utils_linear_algebra_test.cc diff --git a/models/utilities/math_utils/test/numerical_methods_test.cc b/models/utilities/math_utils/test/math_utils_numerical_methods_test.cc similarity index 100% rename from models/utilities/math_utils/test/numerical_methods_test.cc rename to models/utilities/math_utils/test/math_utils_numerical_methods_test.cc diff --git a/models/utilities/math_utils/test/protection_methods_test.cc b/models/utilities/math_utils/test/math_utils_protection_methods_test.cc similarity index 100% rename from models/utilities/math_utils/test/protection_methods_test.cc rename to models/utilities/math_utils/test/math_utils_protection_methods_test.cc diff --git a/models/utilities/math_utils/test/std_array_ops_test.cc b/models/utilities/math_utils/test/std_array_ops_test.cc new file mode 100644 index 00000000..e69de29b From 20eac277fbcb3fd215a8294dd3df897f377993e5 Mon Sep 17 00:00:00 2001 From: Nino Tarantino Date: Wed, 23 Sep 2026 15:19:44 -0500 Subject: [PATCH 07/13] Convert tests for backwards difference and vector derivative to GTest --- models/utilities/math_utils/src/math_utils.cc | 22 +-- .../test/math_utils_numerical_methods_test.cc | 157 ++++++++++++++++++ 2 files changed, 168 insertions(+), 11 deletions(-) diff --git a/models/utilities/math_utils/src/math_utils.cc b/models/utilities/math_utils/src/math_utils.cc index a6761f00..5fc1b94d 100644 --- a/models/utilities/math_utils/src/math_utils.cc +++ b/models/utilities/math_utils/src/math_utils.cc @@ -21,8 +21,10 @@ #include #include #include +#include #include #include +#include #include #include #include "jeod/models/utils/math/include/vector3.hh" @@ -32,6 +34,8 @@ #include "../include/math_utils.hh" +#include + /******************************************************************************* generate_inertial_to_lvlh Purpose:( Generates the transformation matrix from inertial to LVLH given @@ -893,24 +897,20 @@ MathUtils::compute_backward_difference( const std::list & history) return 0.0; } - static const std::array, 5> back_diff_coefficients = + static constexpr std::array, 5> back_diff_coefficients = {{{ 0, 0, 0, 0, 0 }, { 1, -1.0, 0, 0, 0 }, { 1.5, -2.0, 0.5, 0, 0 }, { 11.0/6, -3.0, 1.5, -1.0/3, 0 }, { 25.0/12, -4.0, 3.0, -4.0/3, 0.25}}}; - double derivative = 0.0; - const size_t order = std::min(history.size() - 1, static_cast(4)); + constexpr auto max_order = back_diff_coefficients.size() - 1; - size_t ii = 0; - for (auto it = history.begin(); - it != history.end() && ii < 5; - ++it) { + // Use at most the first 5 items in the history (4th order difference). + const auto order = std::min(history.size() - 1, max_order); + const auto begin = history.begin(); + const auto end = std::next(begin, static_cast(order) + 1); - derivative += (*it) * back_diff_coefficients[order][ii]; - ++ii; - } - return derivative; + return std::inner_product(begin, end, back_diff_coefficients[order].begin(), 0.0); } diff --git a/models/utilities/math_utils/test/math_utils_numerical_methods_test.cc b/models/utilities/math_utils/test/math_utils_numerical_methods_test.cc index 1d51b159..2f84e4f5 100644 --- a/models/utilities/math_utils/test/math_utils_numerical_methods_test.cc +++ b/models/utilities/math_utils/test/math_utils_numerical_methods_test.cc @@ -4,6 +4,7 @@ #include #include #include +#include #include namespace { @@ -174,4 +175,160 @@ TEST(MathUtils, Polynomial) { } } +// Test the backwards difference function. +TEST(MathUtils, BackwardDifference) { + using testing::_; + using testing::HasSubstr; + testing::StrictMock cml_message_mock; + + // Floating point comparison tolerance. + constexpr double tolerance = 1e-12; + + std::list history; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("there is no history for this variable"))); + EXPECT_EQ(MathUtils::compute_backward_difference(history), 0.0); + + history.push_back(436.0); + EXPECT_EQ(MathUtils::compute_backward_difference(history), 0.0); + + history.push_back(217.0); + EXPECT_NEAR(MathUtils::compute_backward_difference(history), 219.0, tolerance); + + history.push_back(88.0); + EXPECT_NEAR(MathUtils::compute_backward_difference(history), 264.0, tolerance); + + history.push_back(25.0); + EXPECT_NEAR(MathUtils::compute_backward_difference(history), 272.0, tolerance); + + history.push_back(4.0); + EXPECT_NEAR(MathUtils::compute_backward_difference(history), 272.0, tolerance); + + history.push_back(999999.0); + EXPECT_NEAR(MathUtils::compute_backward_difference(history), 272.0, tolerance); +} + +// Test the unit vector derivative function. +TEST(MathUtils, UnitVectorDerivative) { + using testing::_; + using testing::DoubleNear; + using testing::HasSubstr; + using testing::Pointwise; + + // Floating point comparison tolerance. + constexpr double tolerance = 1e-12; + + testing::StrictMock cml_message_mock; + + double unit_vector_derivative[3] {}; + + // Case 1 + { + const double vector[3] {}; + const double derivative[3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("A unit vector is not defined for a zero vector"))); + MathUtils::compute_unit_vector_derivative(vector, derivative, unit_vector_derivative); + EXPECT_THAT(unit_vector_derivative, Pointwise(DoubleNear(tolerance), {0.0, 0.0, 0.0})); + } + + // Case 2 + { + const double vector[3] {}; + const double derivative[3] {3.0, 4.0, 0.0}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("A unit vector is not defined for a zero vector"))); + MathUtils::compute_unit_vector_derivative(vector, derivative, unit_vector_derivative); + EXPECT_THAT(unit_vector_derivative, Pointwise(DoubleNear(tolerance), {0.6, 0.8, 0.0})); + } + + // Case 3 + { + const double vector[3] {0.0, 0.0, 1.0}; + const double derivative[3] {0.0, 0.0, 1.0}; + + MathUtils::compute_unit_vector_derivative(vector, derivative, unit_vector_derivative); + EXPECT_THAT(unit_vector_derivative, Pointwise(DoubleNear(tolerance), {0.0, 0.0, 0.0})); + } + + // Case 4 + { + const double vector[3] {0.0, 0.0, 1.0}; + const double derivative[3] {0.0, 1.0, 0.0}; + + MathUtils::compute_unit_vector_derivative(vector, derivative, unit_vector_derivative); + EXPECT_THAT(unit_vector_derivative, Pointwise(DoubleNear(tolerance), {0.0, 1.0, 0.0})); + } + + // Case 5 + { + const double vector[3] {0.0, 0.0, 1.0}; + const double derivative[3] {0.0, 5.0, 0.0}; + + MathUtils::compute_unit_vector_derivative(vector, derivative, unit_vector_derivative); + EXPECT_THAT(unit_vector_derivative, Pointwise(DoubleNear(tolerance), {0.0, 5.0, 0.0})); + } + + // Case 6 + { + const double vector[3] {0.0, 0.0, 5.0}; + const double derivative[3] {0.0, 5.0, 0.0}; + + MathUtils::compute_unit_vector_derivative(vector, derivative, unit_vector_derivative); + EXPECT_THAT(unit_vector_derivative, Pointwise(DoubleNear(tolerance), {0.0, 1.0, 0.0})); + } + + // Case 7 + { + const double vector[3] {1.0, 1.0, 0.0}; + const double derivative[3] {0.0, 0.0, 2.0}; + + MathUtils::compute_unit_vector_derivative(vector, derivative, unit_vector_derivative); + EXPECT_THAT(unit_vector_derivative, Pointwise(DoubleNear(tolerance), {0.0, 0.0, std::sqrt(2.0)})); + } + + // Case 8 + { + const double vector[3] {1.0, 1.0, 0.0}; + const double derivative[3] {0.0, 2.0, 2.0}; + + MathUtils::compute_unit_vector_derivative(vector, derivative, unit_vector_derivative); + const double inv_sqrt2 = 1.0 / std::sqrt(2.0); + EXPECT_THAT(unit_vector_derivative, Pointwise(DoubleNear(tolerance), {-inv_sqrt2, inv_sqrt2, 2.0 * inv_sqrt2})); + } + + // Case 9 + { + const double vector[3] {1.0, 2.0, 3.0}; + const double derivative[3] {3.0, 2.0, 1.0}; + + MathUtils::compute_unit_vector_derivative(vector, derivative, unit_vector_derivative); + const double factor = 2.0 * std::sqrt(2.0) / (7.0 * std::sqrt(7.0)); + EXPECT_THAT(unit_vector_derivative, Pointwise(DoubleNear(tolerance), {factor * 4.0, factor, -factor * 2.0})); + } + + // Case 10 + { + const double vector[3] {1.0, 2.0, 3.0}; + const double derivative[3] {6.0, 4.0, 2.0}; + + MathUtils::compute_unit_vector_derivative(vector, derivative, unit_vector_derivative); + const double factor = 4.0 * std::sqrt(2.0) / (7.0 * std::sqrt(7.0)); + EXPECT_THAT(unit_vector_derivative, Pointwise(DoubleNear(tolerance), {factor * 4.0, factor, -factor * 2.0})); + } + + // Case 11 + { + const double vector[3] {4.0, 8.0, 12.0}; + const double derivative[3] {6.0, 4.0, 2.0}; + + MathUtils::compute_unit_vector_derivative(vector, derivative, unit_vector_derivative); + const double factor = std::sqrt(2.0) / (7.0 * std::sqrt(7.0)); + EXPECT_THAT(unit_vector_derivative, Pointwise(DoubleNear(tolerance), {factor * 4.0, factor, -factor * 2.0})); + } +} + } From 38e66d67b66f38c2f166b0a18fdcd2010ba4a8b3 Mon Sep 17 00:00:00 2001 From: Nino Tarantino Date: Wed, 23 Sep 2026 16:32:56 -0500 Subject: [PATCH 08/13] Create Cholesky decomposition GTests --- .../math_utils_cholesky_decomposition_test.cc | 205 ++++++++++++++++++ 1 file changed, 205 insertions(+) diff --git a/models/utilities/math_utils/test/math_utils_cholesky_decomposition_test.cc b/models/utilities/math_utils/test/math_utils_cholesky_decomposition_test.cc index 49275997..f248bf27 100644 --- a/models/utilities/math_utils/test/math_utils_cholesky_decomposition_test.cc +++ b/models/utilities/math_utils/test/math_utils_cholesky_decomposition_test.cc @@ -2,11 +2,216 @@ #include "mocks/cml/cml_message_mock.hh" #include +#include #include #include namespace { +// Floating point comparison tolerance. +inline constexpr double tolerance = 1e-12; +// Function to compare the values between two matrices. +template +void test_matrices_equal(const double (&lhs)[dimension][dimension], const double (&rhs)[dimension][dimension]) { + using testing::DoubleNear; + using testing::Pointwise; + for (unsigned int row = 0; row < dimension; ++row) { + EXPECT_THAT(lhs[row], Pointwise(DoubleNear(tolerance), rhs[row])); + } +} + +// Test various null pointer failures. +TEST(MathUtils, CholeskyDecompositionNullPointers) { + using testing::_; + using testing::HasSubstr; + + testing::StrictMock cml_message_mock; + + constexpr unsigned int dimension = 10; + const double in_array[dimension][dimension] {}; + double out_array[dimension][dimension] {}; + + // Null input array. + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("the matrix to be decomposed is addressed with a NULL pointer"))); + EXPECT_FALSE(MathUtils::cholesky_decomposition("Test", nullptr, &out_array[0][0], dimension)); + + // Null output array. + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, + HasSubstr("array to which the computed decomposition will be sent is addressed with a NULL pointer"))); + EXPECT_FALSE(MathUtils::cholesky_decomposition("Test", &in_array[0][0], nullptr, dimension)); +} + +// Test calculation of Cholesky decomposition of a 3x3 matrix. +TEST(MathUtils, CholeskyDecomposition3x3) { + testing::StrictMock cml_message_mock; + + constexpr unsigned int dimension = 3; + const double in_array[dimension][dimension] { + {1.0, 0.0, 1.0}, + {0.0, 2.0, 0.0}, + {1.0, 0.0, 3.0} + }; + double out_array[dimension][dimension] {}; + const double expected[dimension][dimension] { + {1.0, 0.0, 0.0}, + {0.0, std::sqrt(2.0), 0.0}, + {1.0, 0.0, std::sqrt(2.0)} + }; + + EXPECT_CALL(cml_message_mock, publish).Times(0); + const bool success = MathUtils::cholesky_decomposition("Test", &in_array[0][0], &out_array[0][0], dimension); + EXPECT_TRUE(success); + test_matrices_equal(out_array, expected); +} + +// Test calculation of Cholesky decomposition of a submatrix within a larger matrix. +TEST(MathUtils, CholeskyDecompositionSubmatrix) { + using testing::_; + using testing::HasSubstr; + + testing::StrictMock cml_message_mock; + + // Decompose a 3x3 submatrix within a 10x10 matrix. + { + // Values outside the upper 3x3 region shouldn't have any effect on the output. + constexpr unsigned int dimension = 10; + const double in_array[dimension][dimension] { + {1.0, 0.0, 1.0, 100.0}, + {0.0, 2.0, 0.0, 200.0}, + {1.0, 0.0, 3.0, 300.0}, + {-50.0, -60.0, 0.0, -1.0} + }; + double out_array[dimension][dimension] {}; + const double expected[dimension][dimension] { + {1.0, 0.0, 0.0}, + {0.0, std::sqrt(2.0), 0.0}, + {1.0, 0.0, std::sqrt(2.0)} + }; + + EXPECT_CALL(cml_message_mock, publish).Times(0); + const bool success = MathUtils::cholesky_decomposition("Test", &in_array[0][0], &out_array[0][0], dimension, 3); + EXPECT_TRUE(success); + test_matrices_equal(out_array, expected); + } + + // Attempt to decompose a submatrix that's larger than the input matrix. + { + constexpr unsigned int dimension = 5; + const double in_array[dimension][dimension] {}; + double out_array[dimension][dimension] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("A sub-matrix must be smaller than the original matrix"))); + const bool success = MathUtils::cholesky_decomposition("Test", &in_array[0][0], &out_array[0][0], dimension, dimension + 1); + EXPECT_FALSE(success); + } +} + +// Test various Cholesky decomposition error conditions +TEST(MathUtils, CholeskyDecompositionErrorConditions) { + using testing::_; + using testing::HasSubstr; + + testing::StrictMock cml_message_mock; + + // Asymmetric matrix. + { + constexpr unsigned int dimension = 3; + const double in_array[dimension][dimension] { + {1.0, -5000.0, 200.0}, + {0.0, 2.0, 4444.0}, + {1.0, 0.0, 3.0} + }; + double out_array[dimension][dimension] {}; + + // The decomposition will proceed with values below the diagonal, treating the + // matrix as though it was symmetric. + const double expected[dimension][dimension] { + {1.0, 0.0, 0.0}, + {0.0, std::sqrt(2.0), 0.0}, + {1.0, 0.0, std::sqrt(2.0)} + }; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("not a symmetric matrix"))); + const bool success = MathUtils::cholesky_decomposition("Test", &in_array[0][0], &out_array[0][0], dimension); + EXPECT_TRUE(success); + test_matrices_equal(out_array, expected); + } + + // Negative determinant. + { + constexpr unsigned int dimension = 3; + const double in_array[dimension][dimension] { + {1.0, 0.0, 1.0}, + {0.0, 2.0, 0.0}, + {1.0, 0.0, 0.0} + }; + double out_array[dimension][dimension] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, + HasSubstr("A negative eigen-value exists, detected in processing col 2"))); + const bool success = MathUtils::cholesky_decomposition("Test", &in_array[0][0], &out_array[0][0], dimension); + EXPECT_FALSE(success); + } + + // Zero determinant. + { + constexpr unsigned int dimension = 3; + const double in_array[dimension][dimension] { + {0.0, 0.0, 1.0}, + {0.0, 2.0, 0.0}, + {1.0, 0.0, 3.0} + }; + double out_array[dimension][dimension] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, + HasSubstr("A negative eigen-value exists\n"))); + const bool success = MathUtils::cholesky_decomposition("Test", &in_array[0][0], &out_array[0][0], dimension); + EXPECT_FALSE(success); + } + + // Ambiguous solution due to determinant of zero. + { + constexpr unsigned int dimension = 10; + const double in_array[dimension][dimension] { + { 1.6641, 3.50751, 0.74175, 0, 1.73118, 2.64321, 0.73788, 1.67184, 0.84108, 1.24485 }, + { 3.50751, 8.013905, 3.187493, 0, 4.57795, 7.662583, 2.00364, 3.573468, 2.857864, 4.637963 }, + { 0.74175, 3.187493, 4.82635, 0, 3.773771, 7.531023, 2.654479, 1.877517, 3.946451, 6.775963 }, + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, + { 1.73118, 4.57795, 3.773771, 0, 9.967224, 14.664142, 10.295208, 12.423878, 9.559074, 9.055285 }, + { 2.64321, 7.662583, 7.531023, 0, 14.664142, 22.981239, 14.016988, 17.331281, 14.986778, 16.304499}, + { 0.73788, 2.00364, 2.654479, 0, 10.295208, 14.016988, 25.62034, 20.296328, 17.423248, 10.169399}, + { 1.67184, 3.573468, 1.877517, 0, 12.423878, 17.331281, 20.296328, 32.862215, 16.117226, 14.139477}, + { 0.84108, 2.857864, 3.946451, 0, 9.559074, 14.986778, 17.423248, 16.117226, 25.962729, 17.506381}, + { 1.24485, 4.637963, 6.775963, 0, 9.055285, 16.304499, 10.169399, 14.139477, 17.506381, 27.0338 } + }; + double out_array[dimension][dimension] {}; + const double expected[dimension][dimension] { + { 1.29, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, + { 2.719, 0.788, 0, 0, 0, 0, 0, 0, 0, 0 }, + { 0.575, 2.061, 0.498, 0, 0, 0, 0, 0, 0, 0 }, + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, + { 1.342, 1.179, 1.149, 0, 2.33581206435792, 0, 0, 0, 0, 0 }, + { 2.049, 2.654, 1.773, 0, 2.888991414578969, 0.4993211456457813, 0, 0, 0, 0 }, + { 0.572, 0.569, 2.315, 0, 2.652952304920735, -0.8692150051890255, 3.437510631630103, 0, 0, 0 }, + { 1.296, 0.063, 2.013, 0, 3.552268663481212, 1.355945637049325, 1.923974545593591, 2.994592079019146, 0, 0 }, + { 0.652, 1.377, 1.473, 0, 2.298185749578112, 1.492434492028816, 2.343866284839132, -0.827036959989062, 2.790170111847167, 0 }, + { 0.965, 2.556, 1.914, 0, 1.090646391836446, 2.001116809004143, 0.7499941599377179, 0.2819358021403084, 1.261754093264061, 2.911711272835517 } + }; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Warning, _, _, + HasSubstr("There are multiple solutions for the decomposition of this matrix"))); + const bool success = MathUtils::cholesky_decomposition("Test", &in_array[0][0], &out_array[0][0], dimension); + EXPECT_TRUE(success); + test_matrices_equal(out_array, expected); + } +} } From f27f7c22dd2608538cc0da115a1f8c720100d832 Mon Sep 17 00:00:00 2001 From: Nino Tarantino Date: Thu, 24 Sep 2026 15:03:01 -0500 Subject: [PATCH 09/13] Fix duplicate include --- models/utilities/math_utils/src/math_utils.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/models/utilities/math_utils/src/math_utils.cc b/models/utilities/math_utils/src/math_utils.cc index 5fc1b94d..aac5f689 100644 --- a/models/utilities/math_utils/src/math_utils.cc +++ b/models/utilities/math_utils/src/math_utils.cc @@ -34,8 +34,6 @@ #include "../include/math_utils.hh" -#include - /******************************************************************************* generate_inertial_to_lvlh Purpose:( Generates the transformation matrix from inertial to LVLH given From db5e529ca6464f8c3abab0b537829e612af98cba Mon Sep 17 00:00:00 2001 From: Nino Tarantino Date: Wed, 30 Sep 2026 17:06:16 -0500 Subject: [PATCH 10/13] Fill out some linear algebra tests --- .../math_utils/include/math_utils.hh | 9 +- .../math_utils/include/std_array_ops.hh | 3 +- .../math_utils_cholesky_decomposition_test.cc | 4 + .../test/math_utils_linear_algebra_test.cc | 142 +++++++++++++++++- .../test/math_utils_numerical_methods_test.cc | 1 - 5 files changed, 147 insertions(+), 12 deletions(-) diff --git a/models/utilities/math_utils/include/math_utils.hh b/models/utilities/math_utils/include/math_utils.hh index 556483c8..ce0b7902 100644 --- a/models/utilities/math_utils/include/math_utils.hh +++ b/models/utilities/math_utils/include/math_utils.hh @@ -516,13 +516,6 @@ Linear Algebra Section return result; } - template - static std::array vector_elementwise_sqrt( - const std::array&& vec) { - for (size_t ii = 0; ii < N; ii++) { vec[ii] = sqrt_protected(vec[ii]); } - return vec; - } - template static std::array vector_elementwise_sqrt(const double (&vec)[N]) { std::array result; @@ -1003,4 +996,4 @@ template<> bool MathUtils::is_equal( double val1, double val2); template<> bool MathUtils::is_within_abs_tolerance( bool val1, bool val2, bool tol); template<> bool MathUtils::is_within_rel_tolerance( bool value, bool expected, double tol); -#endif \ No newline at end of file +#endif diff --git a/models/utilities/math_utils/include/std_array_ops.hh b/models/utilities/math_utils/include/std_array_ops.hh index f9a3d291..8a944a70 100644 --- a/models/utilities/math_utils/include/std_array_ops.hh +++ b/models/utilities/math_utils/include/std_array_ops.hh @@ -11,10 +11,9 @@ PROGRAMMERS: #ifndef SWIG -#include #include #include -#include +#include #include #include "math_utils.hh" diff --git a/models/utilities/math_utils/test/math_utils_cholesky_decomposition_test.cc b/models/utilities/math_utils/test/math_utils_cholesky_decomposition_test.cc index f248bf27..7497c90d 100644 --- a/models/utilities/math_utils/test/math_utils_cholesky_decomposition_test.cc +++ b/models/utilities/math_utils/test/math_utils_cholesky_decomposition_test.cc @@ -214,4 +214,8 @@ TEST(MathUtils, CholeskyDecompositionErrorConditions) { } } +// Test inverting a matrix using Cholesky decomposition +TEST(MathUtils, MatrixInversionUsingCholeskyDecomp) { +} + } diff --git a/models/utilities/math_utils/test/math_utils_linear_algebra_test.cc b/models/utilities/math_utils/test/math_utils_linear_algebra_test.cc index 49275997..f55e23e2 100644 --- a/models/utilities/math_utils/test/math_utils_linear_algebra_test.cc +++ b/models/utilities/math_utils/test/math_utils_linear_algebra_test.cc @@ -1,12 +1,152 @@ #include "../include/math_utils.hh" #include "mocks/cml/cml_message_mock.hh" -#include +#include #include #include namespace { +// Floating point comparison tolerance. +constexpr double tolerance = 1e-12; +// Test addition and subtraction of C-style vectors. +TEST(MathUtils, VectorAlgebra) { + using testing::DoubleNear; + using testing::Pointwise; + + const double lhs[5] {1.0, 2.0, 3.0, 4.0, 5.0}; + const double rhs[5] {-3.0, 6.0, 0.0, 2.0, -1.0}; + const std::array diff = MathUtils::diff(lhs, rhs); + const std::array sum = MathUtils::sum(lhs, rhs); + + EXPECT_THAT(diff, Pointwise(DoubleNear(tolerance), {4.0, -4.0, 3.0, 2.0, 6.0})); + EXPECT_THAT(sum, Pointwise(DoubleNear(tolerance), {-2.0, 8.0, 3.0, 6.0, 4.0})); +} + +// Test vector magnitude functions. +TEST(MathUtils, VectorMagnitude) { + // Test the Pythagorean quintuple, [1, 2, 8, 10, 13]. + const std::array arr4 {-1.0, 2.0, -8.0, -10.0}; + const double c_arr4[4] {-1.0, 2.0, -8.0, -10.0}; + EXPECT_NEAR(MathUtils::vec_mag_sq(arr4), 169.0, tolerance); + EXPECT_NEAR(MathUtils::vec_mag_sq(c_arr4), 169.0, tolerance); + EXPECT_NEAR(MathUtils::vec_mag(arr4), 13.0, tolerance); + EXPECT_NEAR(MathUtils::vec_mag(c_arr4), 13.0, tolerance); + + // Magnitude of vector components using the Euler brick [44, 117, 240]. + const std::array arr3 {44.0, 117.0, 240.0}; + const double c_arr3[3] {44.0, 117.0, 240.0}; + EXPECT_NEAR(MathUtils::vec_mag_xy(arr3), 125.0, tolerance); + EXPECT_NEAR(MathUtils::vec_mag_xy(c_arr3), 125.0, tolerance); + EXPECT_NEAR(MathUtils::vec_mag_xz(arr3), 244.0, tolerance); + EXPECT_NEAR(MathUtils::vec_mag_xz(c_arr3), 244.0, tolerance); + EXPECT_NEAR(MathUtils::vec_mag_yz(arr3), 267.0, tolerance); + EXPECT_NEAR(MathUtils::vec_mag_yz(c_arr3), 267.0, tolerance); +} + +// Test unit vector functions. +TEST(MathUtils, UnitVector) { + using testing::DoubleNear; + using testing::Eq; + using testing::Pointwise; + + // Unit vector calculation. Magnitude is 11. + const std::array vec4 {-4.0, 4.0, 5.0, -8.0}; + const std::array c_vec4 {-4.0, 4.0, 5.0, -8.0}; + const std::array expected { + -4.0 / 11.0, // -0.3636363636363636 + 4.0 / 11.0, // 0.3636363636363636 + 5.0 / 11.0, // 0.4545454545454545 + -8.0 / 11.0 // -0.7272727272727273 + }; + EXPECT_THAT(MathUtils::unit_vector(vec4), Pointwise(DoubleNear(tolerance), expected)); + EXPECT_THAT(MathUtils::unit_vector(c_vec4), Pointwise(DoubleNear(tolerance), expected)); + + // Unit vector of a vector with magnitude zero. + const std::array vec10 {}; + const double c_vec10[10] {}; + const std::array zero {}; + EXPECT_THAT(MathUtils::unit_vector(vec10), Pointwise(Eq(), zero)); + EXPECT_THAT(MathUtils::unit_vector(c_vec10), Pointwise(Eq(), zero)); +} + +// Test zeroing vectors. +TEST(MathUtils, ZeroVector) { + using testing::Eq; + using testing::Pointwise; + + std::array arr {1.0, 2.0, 3.33333, -4.4}; + double c_arr[4] {1.0, 2.0, 3.33333, -4.4}; + const std::array zero {}; + + MathUtils::zero_vector(arr); + MathUtils::zero_vector(c_arr); + EXPECT_THAT(MathUtils::unit_vector(arr), Pointwise(Eq(), zero)); + EXPECT_THAT(MathUtils::unit_vector(c_arr), Pointwise(Eq(), zero)); +} + +// Test the element-wise square root functions. +TEST(MathUtils, VectorElementWiseSquareRoot) { + using testing::DoubleNear; + using testing::Pointwise; + + const std::array arr {4.0, 1.0, 16.0, 25.0}; + const double c_arr[4] {4.0, 1.0, 16.0, 25.0}; + const std::array expected {2.0, 1.0, 4.0, 5.0}; + EXPECT_THAT(MathUtils::vector_elementwise_sqrt(arr), Pointwise(DoubleNear(tolerance), expected)); + EXPECT_THAT(MathUtils::vector_elementwise_sqrt(c_arr), Pointwise(DoubleNear(tolerance), expected)); + + double output[4] {}; + MathUtils::vector_elementwise_sqrt(c_arr, output); + EXPECT_THAT(output, Pointwise(DoubleNear(tolerance), expected)); +} + +// Test the cross product functions. +TEST(MathUtils, VectorCrossProduct) { + using testing::DoubleNear; + using testing::Pointwise; + + const std::array arr1 {-3.0, 2.5, 1.0}; + const std::array arr2 {7.0, 0.0, -3.5}; + const double c_arr1[3] {arr1[0], arr1[1], arr1[2]}; + const double c_arr2[3] {arr2[0], arr2[1], arr2[2]}; + + const std::array arr1_cross_arr2 {-8.75, -3.5, -17.5}; + EXPECT_THAT(MathUtils::vector_cross_product(arr1, arr2), Pointwise(DoubleNear(tolerance), arr1_cross_arr2)); + EXPECT_THAT(MathUtils::vector_cross_product(c_arr1, arr2), Pointwise(DoubleNear(tolerance), arr1_cross_arr2)); + EXPECT_THAT(MathUtils::vector_cross_product(arr1, c_arr2), Pointwise(DoubleNear(tolerance), arr1_cross_arr2)); + EXPECT_THAT(MathUtils::vector_cross_product(c_arr1, c_arr2), Pointwise(DoubleNear(tolerance), arr1_cross_arr2)); +} + +// Test the scalar product functions. +TEST(MathUtils, VectorScalarProduct) { + +} + +// Test the functions which swap a vector/matrix between a 1D vector and 2D matrix. +TEST(MathUtils, ConvertBetweenOneDimensionalVectorAndMatrix) { + +} + +// Test the various matrix copy functions. +TEST(MathUtils, MatrixCopyOperations) { + +} + +// Test the various matrix algebra functions. +TEST(MathUtils, MatrixAlgebra) { + +} + +// Test the various matrix multiplication functions. +TEST(MathUtils, MatrixMultiplication) { + +} + +// Test the function which generates a correlation matrix from a square covariance matrix. +TEST(MathUtils, CorrelationMatrix) { + +} } diff --git a/models/utilities/math_utils/test/math_utils_numerical_methods_test.cc b/models/utilities/math_utils/test/math_utils_numerical_methods_test.cc index 2f84e4f5..902edc5a 100644 --- a/models/utilities/math_utils/test/math_utils_numerical_methods_test.cc +++ b/models/utilities/math_utils/test/math_utils_numerical_methods_test.cc @@ -89,7 +89,6 @@ TEST(MathUtils, IsWithinRelativeTolerance) { testing::StrictMock cml_message_mock; - // Nominal tests. EXPECT_TRUE(MathUtils::is_within_rel_tolerance(1.65, 1.5, 0.1)); EXPECT_FALSE(MathUtils::is_within_rel_tolerance(1.65, 1.5, 0.09999)); From 7f11a5dc2ff0b89a73e252a26ebb1d4f45c5f757 Mon Sep 17 00:00:00 2001 From: Nino Tarantino Date: Thu, 1 Oct 2026 10:53:58 -0500 Subject: [PATCH 11/13] Fill out linear algebra tests --- .../test/math_utils_linear_algebra_test.cc | 443 +++++++++++++++++- 1 file changed, 437 insertions(+), 6 deletions(-) diff --git a/models/utilities/math_utils/test/math_utils_linear_algebra_test.cc b/models/utilities/math_utils/test/math_utils_linear_algebra_test.cc index f55e23e2..a40258aa 100644 --- a/models/utilities/math_utils/test/math_utils_linear_algebra_test.cc +++ b/models/utilities/math_utils/test/math_utils_linear_algebra_test.cc @@ -10,6 +10,16 @@ namespace { // Floating point comparison tolerance. constexpr double tolerance = 1e-12; +// Function to compare the values between two matrices. +template +void test_matrices_equal(const double (&lhs)[rows][cols], const double (&rhs)[rows][cols]) { + using testing::DoubleNear; + using testing::Pointwise; + for (unsigned int row = 0; row < rows; ++row) { + EXPECT_THAT(lhs[row], Pointwise(DoubleNear(tolerance), rhs[row])); + } +} + // Test addition and subtraction of C-style vectors. TEST(MathUtils, VectorAlgebra) { using testing::DoubleNear; @@ -121,32 +131,453 @@ TEST(MathUtils, VectorCrossProduct) { // Test the scalar product functions. TEST(MathUtils, VectorScalarProduct) { - + const std::array lhs {5.0, -3.0, 2.0, 6.0}; + const std::array rhs {-3.0, 1.0, 4.0, -2.0}; + const double c_lhs[4] {lhs[0], lhs[1], lhs[2], lhs[3]}; + const double c_rhs[4] {rhs[0], rhs[1], rhs[2], rhs[3]}; + + ASSERT_NEAR(MathUtils::vector_scalar_product(lhs, rhs), -22.0, tolerance); + ASSERT_NEAR(MathUtils::vector_scalar_product(c_lhs, rhs), -22.0, tolerance); + ASSERT_NEAR(MathUtils::vector_scalar_product(lhs, c_rhs), -22.0, tolerance); + ASSERT_NEAR(MathUtils::vector_scalar_product(c_lhs, c_rhs), -22.0, tolerance); + + double output = 0.0; + MathUtils::vector_scalar_product(c_lhs, c_rhs, output); + ASSERT_NEAR(output, -22.0, tolerance); } // Test the functions which swap a vector/matrix between a 1D vector and 2D matrix. TEST(MathUtils, ConvertBetweenOneDimensionalVectorAndMatrix) { + using testing::DoubleNear; + using testing::Pointwise; + + // Set up the following matrix as a column-major and row-major vector: + const double expected[4][3] { + { 1.0, -2.0, 3.0}, + {-4.0, 5.5, 6.0}, + {-7.0, -8.0, 9.0}, + { 0.0, 10.0, -1.0} + }; + const double col_major_vec[12] {1.0, -4.0, -7.0, 0.0, -2.0, 5.5, -8.0, 10.0, 3.0, 6.0, 9.0, -1.0}; + const double row_major_vec[12] {1.0, -2.0, 3.0, -4.0, 5.5, 6.0, -7.0, -8.0, 9.0, 0.0, 10.0, -1.0}; + + double col_major_output[4][3] {}; + double row_major_output[4][3] {}; + MathUtils::col_maj_vec_to_matrix(col_major_vec, col_major_output); + MathUtils::row_maj_vec_to_matrix(row_major_vec, row_major_output); + test_matrices_equal(col_major_output, expected); + test_matrices_equal(row_major_output, expected); } // Test the various matrix copy functions. TEST(MathUtils, MatrixCopyOperations) { + using testing::_; + + testing::StrictMock cml_message_mock; + + const double input[3][4] { + {1.0, 2.0, 3.0, 4.0}, + {5.0, 6.0, 7.0, 8.0}, + {9.0, 10.0, 11.0, 12.0} + }; + // Matrix copy + { + double output[3][4] {}; + MathUtils::matrix_copy(input, output); + test_matrices_equal(input, output); + } + + // Copy submatrix out + { + // Valid case. + double output[2][2] {}; + const double expected1[2][2] { + {6.0, 7.0}, + {10.0, 11.0} + }; + MathUtils::matrix_copy_submatrix_out(input, output, 1, 1); + test_matrices_equal(output, expected1); + + // Invalid case. + const double expected2[2][2] { + {8.0, 0.0}, + {12.0, 0.0} + }; + EXPECT_CALL(cml_message_mock, publish(CMLMessage::Error, _, _, _)); + MathUtils::matrix_copy_submatrix_out(input, output, 1, 3); + test_matrices_equal(output, expected2); + } + + // Copy submatrix in + { + // Valid case. + double output1[4][5] { + {100.0, 200.0, 300.0, 400.0, 500.0}, + {600.0, 700.0, 800.0, 900.0, 1000.0}, + {1100.0, 1200.0, 1300.0, 1400.0, 1500.0}, + {1600.0, 1700.0, 1800.0, 1900.0, 2000.0} + }; + const double expected1[4][5] { + {100.0, 1.0, 2.0, 3.0, 4.0}, + {600.0, 5.0, 6.0, 7.0, 8.0}, + {1100.0, 9.0, 10.0, 11.0, 12.0}, + {1600.0, 1700.0, 1800.0, 1900.0, 2000.0} + }; + MathUtils::matrix_copy_submatrix_in(input, output1, 0, 1); + test_matrices_equal(output1, expected1); + + // Invalid case. + double output2[4][5] { + {100.0, 200.0, 300.0, 400.0, 500.0}, + {600.0, 700.0, 800.0, 900.0, 1000.0}, + {1100.0, 1200.0, 1300.0, 1400.0, 1500.0}, + {1600.0, 1700.0, 1800.0, 1900.0, 2000.0} + }; + const double expected2[4][5] { + {100.0, 200.0, 300.0, 400.0, 500.0}, + {600.0, 700.0, 800.0, 900.0, 1000.0}, + {1100.0, 1200.0, 1300.0, 1.0, 2.0}, + {1600.0, 1700.0, 1800.0, 5.0, 6.0} + }; + EXPECT_CALL(cml_message_mock, publish(CMLMessage::Error, _, _, _)); + MathUtils::matrix_copy_submatrix_in(input, output2, 2, 3); + test_matrices_equal(output2, expected2); + } } // Test the various matrix algebra functions. TEST(MathUtils, MatrixAlgebra) { - + // Zero matrix + { + double input[3][2] { + {1.0, 2.0}, + {3.0, 4.0}, + {5.0, 6.0} + }; + const double zeros[3][2] {}; + MathUtils::zero_matrix(input); + test_matrices_equal(input, zeros); + } + + // Increment + { + double input[3][2] { + {1.0, 2.0}, + {3.0, 4.0}, + {5.0, 6.0} + }; + const double increment[3][2] { + {-1.0, 5.0}, + {2.0, -3.0}, + {0.0, 10.0} + }; + const double expected[3][2] { + {0.0, 7.0}, + {5.0, 1.0}, + {5.0, 16.0} + }; + MathUtils::matrix_incr(increment, input); + test_matrices_equal(input, expected); + } + + // Decrement + { + double input[3][2] { + {1.0, 2.0}, + {3.0, 4.0}, + {5.0, 6.0} + }; + const double decrement[3][2] { + {-1.0, 5.0}, + {2.0, -3.0}, + {0.0, 10.0} + }; + const double expected[3][2] { + {2.0, -3.0}, + {1.0, 7.0}, + {5.0, -4.0} + }; + MathUtils::matrix_decr(decrement, input); + test_matrices_equal(input, expected); + } + + // Scale + { + double input[3][2] { + {1.0, 2.0}, + {-0.5, 0.0}, + {-10.0, -11.0} + }; + const double expected[3][2] { + {2.0, 4.0}, + {-1.0, 0.0}, + {-20.0, -22.0} + }; + MathUtils::matrix_scale(2.0, input); + test_matrices_equal(input, expected); + } } -// Test the various matrix multiplication functions. -TEST(MathUtils, MatrixMultiplication) { - +// Test the various matrix multiplication and miscellaneous functions. +TEST(MathUtils, MatrixOperations) { + // Transpose + { + const double input[2][3] { + {1.0, 2.0, 3.0}, + {4.0, 5.0, 6.0} + }; + double output[3][2] {}; + const double expected[3][2] { + {1.0, 4.0}, + {2.0, 5.0}, + {3.0, 6.0} + }; + MathUtils::matrix_trans(input, output); + test_matrices_equal(output, expected); + } + + // Multiplication: L * R + { + const double lhs[3][2] { + { 1.0, -2.0}, + {-3.0, 0.0}, + { 5.0, 3.0} + }; + const double rhs[2][3] { + {7.0, -3.0, 2.0}, + {-4.0, -10.0, 0.0} + }; + double output[3][3] {}; + const double expected[3][3] { + {15.0, 17.0, 2.0}, + {-21.0, 9.0, -6.0}, + {23.0, -45.0, 10.0} + }; + MathUtils::matrix_mult(lhs, rhs, output); + test_matrices_equal(output, expected); + } + + // Multiplication: L' * R + { + const double lhs[2][3] { + {1.0, -3.0, 5.0}, + {-2.0, 0.0, 3.0} + }; + const double rhs[2][3] { + {7.0, -3.0, 2.0}, + {-4.0, -10.0, 0.0} + }; + double output[3][3] {}; + const double expected[3][3] { + {15.0, 17.0, 2.0}, + {-21.0, 9.0, -6.0}, + {23.0, -45.0, 10.0} + }; + MathUtils::matrix_mult_left_trans(lhs, rhs, output); + test_matrices_equal(output, expected); + } + + // Multiplication: L * R' + { + const double lhs[3][2] { + { 1.0, -2.0}, + {-3.0, 0.0}, + { 5.0, 3.0} + }; + const double rhs[3][2] { + {7.0, -4.0}, + {-3.0, -10.0}, + {2.0, 0.0} + }; + double output[3][3] {}; + const double expected[3][3] { + {15.0, 17.0, 2.0}, + {-21.0, 9.0, -6.0}, + {23.0, -45.0, 10.0} + }; + MathUtils::matrix_mult_right_trans(lhs, rhs, output); + test_matrices_equal(output, expected); + } + + // Multiplication: L' * R' + { + const double lhs[3][2] { + { 1.0, -2.0}, + {-3.0, 0.0}, + { 5.0, 3.0} + }; + const double rhs[2][3] { + {7.0, -3.0, 2.0}, + {-4.0, -10.0, 0.0} + }; + double output[2][2] {}; + const double expected[2][2] { + {26.0, 26.0}, + {-8.0, 8.0} + }; + MathUtils::matrix_mult_trans_trans(lhs, rhs, output); + test_matrices_equal(output, expected); + } } // Test the function which generates a correlation matrix from a square covariance matrix. TEST(MathUtils, CorrelationMatrix) { - + using testing::_; + using testing::HasSubstr; + + testing::StrictMock cml_message_mock; + + // Ordinary covariance matrix. + { + const double covariance[3][3] { + {4.0, 100.0, 100.0}, + {2.0, 3.0, 100.0}, + {0.6, 0.9, 2.0} + }; + double output[3][3] {}; + const double expected[3][3] { + {1.0, 0.5773502691896258, 0.2121320343559642}, + {0.5773502691896258, 1.0, 0.3674234614174767}, + {0.2121320343559642, 0.3674234614174767, 1.0} + }; + + const bool success = MathUtils::extract_correlation_coefficients(covariance, output); + EXPECT_TRUE(success); + test_matrices_equal(output, expected); + } + + // Positive variances and negative covariances. + { + const double covariance[3][3] { + {4.0, 100.0, 100.0}, + {-1.2, 3.0, 100.0}, + {0.6, -0.9, 2.5} + }; + double output[3][3] {}; + const double expected[3][3] { + {1.0, -0.3464101615137755, 0.1897366596101027}, + {-0.3464101615137755, 1.0, -0.3286335345030997}, + {0.1897366596101027, -0.3286335345030997, 1.0} + }; + + const bool success = MathUtils::extract_correlation_coefficients(covariance, output); + EXPECT_TRUE(success); + test_matrices_equal(output, expected); + } + + // Negative variances. + { + const double covariance[3][3] { + {-1.0, 100.0, 100.0}, + {0.0, -2.0, 100.0}, + {0.0, 0.0, -3.0} + }; + double output[3][3] {}; + const double expected[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("a diagonal element is negative"))); + const bool success = MathUtils::extract_correlation_coefficients(covariance, output); + ASSERT_FALSE(success); + test_matrices_equal(output, expected); + } + + // Only positive variances. + { + const double covariance[3][3] { + {1.0, 100.0, 100.0}, + {0.0, 2.0, 100.0}, + {0.0, 0.0, 3.0} + }; + double output[3][3] {}; + const double expected[3][3] { + {1.0, 0.0, 0.0}, + {0.0, 1.0, 0.0}, + {0.0, 0.0, 1.0} + }; + + const bool success = MathUtils::extract_correlation_coefficients(covariance, output); + EXPECT_TRUE(success); + test_matrices_equal(output, expected); + } + + // Zero variance and non-zero covariance in row. + { + const double covariance[3][3] { + {1.0, 100.0, 100.0}, + {1.0, 0.0, 100.0}, + {0.0, 0.0, 1.0} + }; + double output[3][3] {}; + const double expected[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("a diagonal element is zero\nwith non-zero off-diagonals"))); + const bool success = MathUtils::extract_correlation_coefficients(covariance, output); + EXPECT_FALSE(success); + test_matrices_equal(output, expected); + } + + // Zero variance and non-zero covariance in column. + { + const double covariance[3][3] { + {1.0, 100.0, 100.0}, + {0.0, 0.0, 100.0}, + {0.0, 1.0, 1.0} + }; + double output[3][3] {}; + const double expected[3][3] {}; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Error, _, _, HasSubstr("a diagonal element is zero\nwith non-zero off-diagonals"))); + const bool success = MathUtils::extract_correlation_coefficients(covariance, output); + EXPECT_FALSE(success); + test_matrices_equal(output, expected); + } + + // Zero variance and zero respective covariance. + { + const double covariance[3][3] { + {0.0, 100.0, 100.0}, + {0.0, 1.0, 100.0}, + {0.0, 1.0, 1.0} + }; + double output[3][3] {}; + const double expected[3][3] { + {1.0, 0.0, 0.0}, + {0.0, 1.0, 1.0}, + {0.0, 1.0, 1.0} + }; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Inform, _, _, HasSubstr("diagonal element that is zero\nwith zero off-diagonals"))); + const bool success = MathUtils::extract_correlation_coefficients(covariance, output); + EXPECT_TRUE(success); + test_matrices_equal(output, expected); + } + + // All zero variance and covariances. + { + const double covariance[3][3] { + {0.0, 100.0, 100.0}, + {0.0, 0.0, 100.0}, + {0.0, 0.0, 0.0} + }; + double output[3][3] {}; + const double expected[3][3] { + {1.0, 0.0, 0.0}, + {0.0, 1.0, 0.0}, + {0.0, 0.0, 1.0} + }; + + EXPECT_CALL(cml_message_mock, + publish(CMLMessage::Inform, _, _, HasSubstr("diagonal element that is zero\nwith zero off-diagonals"))).Times(3); + const bool success = MathUtils::extract_correlation_coefficients(covariance, output); + EXPECT_TRUE(success); + test_matrices_equal(output, expected); + } } } From a226e1ef9a14894bd361084bbc0392d09499ba5a Mon Sep 17 00:00:00 2001 From: Nino Tarantino Date: Thu, 1 Oct 2026 11:25:39 -0500 Subject: [PATCH 12/13] Add matrix inversion using Cholesky decomposition tests --- .../math_utils_cholesky_decomposition_test.cc | 54 +++++++++++++++++++ 1 file changed, 54 insertions(+) diff --git a/models/utilities/math_utils/test/math_utils_cholesky_decomposition_test.cc b/models/utilities/math_utils/test/math_utils_cholesky_decomposition_test.cc index 7497c90d..91d39209 100644 --- a/models/utilities/math_utils/test/math_utils_cholesky_decomposition_test.cc +++ b/models/utilities/math_utils/test/math_utils_cholesky_decomposition_test.cc @@ -216,6 +216,60 @@ TEST(MathUtils, CholeskyDecompositionErrorConditions) { // Test inverting a matrix using Cholesky decomposition TEST(MathUtils, MatrixInversionUsingCholeskyDecomp) { + using testing::_; + using testing::HasSubstr; + + testing::StrictMock cml_message_mock; + + // Valid case. + { + const double input[3][3] { + {1.0, 1.0, 1.0}, + {1.0, 2.0, 2.0}, + {1.0, 2.0, 3.0} + }; + const double expected[3][3] { + {2.0, -1.0, 0.0}, + {-1.0, 2.0, -1.0}, + {0.0, -1.0, 1.0} + }; + double output[3][3] {}; + + const bool success = MathUtils::matrix_inv_using_cholesky(input, output); + EXPECT_TRUE(success); + test_matrices_equal(output, expected); + } + + // Cholesky decomposition failed. + { + const double input[2][2] { + {0.0, 1.0}, + {1.0, 0.0} + }; + // Set some default data in the output location to test that a failed decomposition doesn't + // override it. + double output[2][2] { + {1.0, 2.0}, + {3.0, 4.0} + }; + const double original_output[2][2] { + {output[0][0], output[0][1]}, + {output[1][0], output[1][1]}, + }; + + // Fail on error. Original output location remains unchanged. + EXPECT_CALL(cml_message_mock, publish(CMLMessage::Error, _, _, HasSubstr("Decomposition failed"))); + EXPECT_CALL(cml_message_mock, publish(CMLMessage::Fail, _, _, HasSubstr("Matrix Inverse failed"))); + EXPECT_FALSE(MathUtils::matrix_inv_using_cholesky(input, output)); + test_matrices_equal(output, original_output); + + // Warn on error. Output location is filled with zeros. + const double zeros[2][2] {}; + EXPECT_CALL(cml_message_mock, publish(CMLMessage::Error, _, _, HasSubstr("Decomposition failed"))); + EXPECT_CALL(cml_message_mock, publish(CMLMessage::Error, _, _, HasSubstr("Matrix Inverse failed"))); + EXPECT_FALSE(MathUtils::matrix_inv_using_cholesky(input, output, false)); + test_matrices_equal(output, zeros); + } } } From b2d44544b4461f9888ffc39880863fe8bd65862e Mon Sep 17 00:00:00 2001 From: Nino Tarantino Date: Thu, 1 Oct 2026 13:04:18 -0500 Subject: [PATCH 13/13] Fill out remaining missing math utils line coverage --- .../test/math_utils_linear_algebra_test.cc | 63 ++++++++++++++++++- .../math_utils/test/std_array_ops_test.cc | 22 +++++++ 2 files changed, 84 insertions(+), 1 deletion(-) diff --git a/models/utilities/math_utils/test/math_utils_linear_algebra_test.cc b/models/utilities/math_utils/test/math_utils_linear_algebra_test.cc index a40258aa..3dd36c13 100644 --- a/models/utilities/math_utils/test/math_utils_linear_algebra_test.cc +++ b/models/utilities/math_utils/test/math_utils_linear_algebra_test.cc @@ -63,7 +63,7 @@ TEST(MathUtils, UnitVector) { // Unit vector calculation. Magnitude is 11. const std::array vec4 {-4.0, 4.0, 5.0, -8.0}; - const std::array c_vec4 {-4.0, 4.0, 5.0, -8.0}; + const double c_vec4[4] {-4.0, 4.0, 5.0, -8.0}; const std::array expected { -4.0 / 11.0, // -0.3636363636363636 4.0 / 11.0, // 0.3636363636363636 @@ -420,6 +420,39 @@ TEST(MathUtils, MatrixOperations) { MathUtils::matrix_mult_trans_trans(lhs, rhs, output); test_matrices_equal(output, expected); } + + // Matrix transformations. + { + const double mat[3][3] { + {1.0, -3.0, 2.0}, + {0.0, 4.0, 5.0}, + {-1.0, -2.0, 6.0} + }; + const double trans[3][3] { + {0.0, 1.0, 0.0}, + {-1.0, 0.0, 0.0}, + {0.0, 0.0, 1.0} + }; + const double transformation_expected[3][3] { + {4.0, 0.0, 5.0}, + {3.0, 1.0, -2.0}, + {-2.0, 1.0, 6.0} + }; + const double inverse_transform_expected[3][3] { + {4.0, 0.0, -5.0}, + {3.0, 1.0, 2.0}, + {2.0, -1.0, 6.0} + }; + double output[3][3] {}; + + MathUtils::matrix_transformation(trans, mat, output); + test_matrices_equal(output, transformation_expected); + + MathUtils::matrix_inverse_transformation(trans, mat, output); + test_matrices_equal(output, inverse_transform_expected); + } + + // Matrix inverse transformation. } // Test the function which generates a correlation matrix from a square covariance matrix. @@ -578,6 +611,34 @@ TEST(MathUtils, CorrelationMatrix) { EXPECT_TRUE(success); test_matrices_equal(output, expected); } + + // Transform PV matrix. + { + const double transform[3][3] { + {0.0, 1.0, 0.0}, + {-1.0, 0.0, 0.0}, + {0.0, 0.0, 1.0} + }; + const double pv_matrix[6][6] { + {1.0, 2.0, 3.0, 4.0, 5.0, 6.0}, + {-2.0, 3.0, -4.0, 5.0, 6.0, 7.0}, + {0.5, 0.25, 0.1625, -1.0, -0.5, -0.6}, + {4.0, 2.0, 3.0, 1.0, 5.0, 9.0}, + {10.0, -11.0, 0.1, 0.4, 0.5, -0.6}, + {-5.0, -8.0, -20.0, 3.0, 5.0, 2.0} + }; + const double expected[6][6] { + { 3.0, 2.0, -4.0, 6.0, -5.0, 7.0}, + {-2.0, 1.0, -3.0, -5.0, 4.0, -6.0}, + { 0.25, -0.5, 0.1625, -0.5, 1.0, -0.6}, + {-11.0, -10.0, 0.1, 0.5, -0.4, -0.6}, + {-2.0, 4.0, -3.0, -5.0, 1.0, -9.0}, + {-8.0, 5.0, -20.0, 5.0, -3.0, 2.0} + }; + double output[6][6]; + MathUtils::transform_pv_matrix(transform, pv_matrix, output); + test_matrices_equal(output, expected); + } } } diff --git a/models/utilities/math_utils/test/std_array_ops_test.cc b/models/utilities/math_utils/test/std_array_ops_test.cc index e0e4ab22..d03aa897 100644 --- a/models/utilities/math_utils/test/std_array_ops_test.cc +++ b/models/utilities/math_utils/test/std_array_ops_test.cc @@ -1,3 +1,4 @@ +#include "../include/math_utils.hh" #include "../include/std_array_ops.hh" #include @@ -10,6 +11,27 @@ namespace { // Floating point comparison tolerance. constexpr double tolerance = 1e-12; +// Test the is_near_equal function for arrays. +TEST(StdArrayOps, Equality) { + // Double precision + { + const std::array lhs {1.0, -2.0, 3.0}; + const std::array rhs1 {1.0, -2.0, 3.0}; + const std::array rhs2 {1.0, 0.0, 0.0}; + EXPECT_TRUE(MathUtils::is_near_equal(lhs, rhs1)); + EXPECT_FALSE(MathUtils::is_near_equal(lhs, rhs2)); + } + + // Single precision + { + const std::array lhs {1.0, -2.0, 3.0}; + const std::array rhs1 {1.0, -2.0, 3.0}; + const std::array rhs2 {1.0, 0.0, 0.0}; + EXPECT_TRUE(MathUtils::is_near_equal(lhs, rhs1)); + EXPECT_FALSE(MathUtils::is_near_equal(lhs, rhs2)); + } +} + // Test the std::ostream operator<< overload. TEST(StdArrayOps, Ostream) { const std::array array {1.0, 2.0, 3.3, -4.1234};