diff --git a/doc/solvers/DurandKerner.md b/doc/solvers/DurandKerner.md index 89e19bf8..ce09abe7 100644 --- a/doc/solvers/DurandKerner.md +++ b/doc/solvers/DurandKerner.md @@ -74,7 +74,7 @@ After ~15–20 iterations the roots converge to $z \approx \{1, 2, 3\}$ (imagina - **Repeated roots.** Convergence degrades from quadratic to linear. Higher tolerance or more iterations may be needed. - **Near-degenerate denominators.** When two root estimates are very close ($|z_r - z_j| < 10^{-15}$), the denominator product approaches zero. The implementation excludes such terms to avoid division by near-zero. - **Leading coefficient must be non-zero.** The polynomial degree is determined by the first coefficient. -- **Complex arithmetic required.** This algorithm operates entirely in $\mathbb{C}$, so it is limited to floating-point types (`float`, `double`). Fixed-point types are not supported. +- **Complex arithmetic required.** This algorithm operates entirely in $\mathbb{C}$, using `math::Complex` (from `numerical/math/ComplexNumber.hpp`), so it is limited to floating-point types (`float`, `double`). Fixed-point types are not supported. - **No convergence guarantee for all polynomials.** Wilkinson's polynomial and other pathological cases may require higher precision or alternative methods. - **Root ordering.** Results are sorted by real part (ascending), which may not correspond to meaningful branch ordering in applications like [Root Locus](../control_analysis/RootLocus.md). diff --git a/numerical/control_analysis/RootLocus.hpp b/numerical/control_analysis/RootLocus.hpp index 4a852a4b..894f07b1 100644 --- a/numerical/control_analysis/RootLocus.hpp +++ b/numerical/control_analysis/RootLocus.hpp @@ -7,10 +7,10 @@ #include "infra/util/BoundedVector.hpp" #include "infra/util/ReallyAssert.hpp" #include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/ComplexNumber.hpp" #include "numerical/solvers/DurandKerner.hpp" #include #include -#include #include #include #include @@ -27,9 +27,9 @@ namespace control_analysis "MaxGainSteps must be greater than 1"); public: - using RootVector = typename infra::BoundedVector>::template WithMaxSize; + using RootVector = typename infra::BoundedVector>::template WithMaxSize; using GainVector = typename infra::BoundedVector::template WithMaxSize; - using LociBranch = typename infra::BoundedVector>::template WithMaxSize; + using LociBranch = typename infra::BoundedVector>::template WithMaxSize; struct Result { diff --git a/numerical/control_analysis/test/TestRootLocus.cpp b/numerical/control_analysis/test/TestRootLocus.cpp index 53ce15f2..b4cef769 100644 --- a/numerical/control_analysis/test/TestRootLocus.cpp +++ b/numerical/control_analysis/test/TestRootLocus.cpp @@ -2,7 +2,6 @@ #include "numerical/math/Tolerance.hpp" #include #include -#include #include #include @@ -23,8 +22,8 @@ TEST_F(TestRootLocus, open_loop_poles_first_order_plant) auto result = rootLocus.Calculate(num, den, 1.0f); ASSERT_EQ(result.openLoopPoles.size(), 1u); - EXPECT_NEAR(result.openLoopPoles[0].real(), -2.0f, math::Tolerance()); - EXPECT_NEAR(result.openLoopPoles[0].imag(), 0.0f, math::Tolerance()); + EXPECT_NEAR(result.openLoopPoles[0].Real(), -2.0f, math::Tolerance()); + EXPECT_NEAR(result.openLoopPoles[0].Imaginary(), 0.0f, math::Tolerance()); } TEST_F(TestRootLocus, open_loop_poles_second_order_underdamped) @@ -37,10 +36,10 @@ TEST_F(TestRootLocus, open_loop_poles_second_order_underdamped) auto result = rootLocus.Calculate(num, den, 1.0f); ASSERT_EQ(result.openLoopPoles.size(), 2u); - EXPECT_NEAR(result.openLoopPoles[0].real(), -zeta * wn, 1e-2f); - EXPECT_NEAR(result.openLoopPoles[1].real(), -zeta * wn, 1e-2f); + EXPECT_NEAR(result.openLoopPoles[0].Real(), -zeta * wn, 1e-2f); + EXPECT_NEAR(result.openLoopPoles[1].Real(), -zeta * wn, 1e-2f); float expectedImag = wn * std::sqrt(1.0f - zeta * zeta); - EXPECT_NEAR(std::abs(result.openLoopPoles[0].imag()), expectedImag, 1e-2f); + EXPECT_NEAR(std::abs(result.openLoopPoles[0].Imaginary()), expectedImag, 1e-2f); } TEST_F(TestRootLocus, open_loop_zeros_identified) @@ -51,8 +50,8 @@ TEST_F(TestRootLocus, open_loop_zeros_identified) auto result = rootLocus.Calculate(num, den, 1.0f); ASSERT_EQ(result.openLoopZeros.size(), 1u); - EXPECT_NEAR(result.openLoopZeros[0].real(), -3.0f, math::Tolerance()); - EXPECT_NEAR(result.openLoopZeros[0].imag(), 0.0f, math::Tolerance()); + EXPECT_NEAR(result.openLoopZeros[0].Real(), -3.0f, math::Tolerance()); + EXPECT_NEAR(result.openLoopZeros[0].Imaginary(), 0.0f, math::Tolerance()); } TEST_F(TestRootLocus, gain_sweep_step_count_and_active_branches) @@ -88,10 +87,10 @@ TEST_F(TestRootLocus, closed_loop_poles_match_analytic_at_k1_doc_example) ASSERT_EQ(result.closedLoopPoles.size(), 2u); float r0 = (-3.0f - std::sqrt(5.0f)) / 2.0f; float r1 = (-3.0f + std::sqrt(5.0f)) / 2.0f; - EXPECT_NEAR(result.closedLoopPoles[0].real(), r0, 1e-2f); - EXPECT_NEAR(result.closedLoopPoles[0].imag(), 0.0f, 1e-2f); - EXPECT_NEAR(result.closedLoopPoles[1].real(), r1, 1e-2f); - EXPECT_NEAR(result.closedLoopPoles[1].imag(), 0.0f, 1e-2f); + EXPECT_NEAR(result.closedLoopPoles[0].Real(), r0, 1e-2f); + EXPECT_NEAR(result.closedLoopPoles[0].Imaginary(), 0.0f, 1e-2f); + EXPECT_NEAR(result.closedLoopPoles[1].Real(), r1, 1e-2f); + EXPECT_NEAR(result.closedLoopPoles[1].Imaginary(), 0.0f, 1e-2f); } TEST_F(TestRootLocus, current_gain_stored_in_result) @@ -114,11 +113,11 @@ TEST_F(TestRootLocus, branches_start_near_open_loop_poles_at_low_gain) ASSERT_GE(result.loci[0].size(), 1u); ASSERT_GE(result.loci[1].size(), 1u); - float p0 = result.openLoopPoles[0].real(); - float p1 = result.openLoopPoles[1].real(); + float p0 = result.openLoopPoles[0].Real(); + float p1 = result.openLoopPoles[1].Real(); - float loci0Start = result.loci[0].front().real(); - float loci1Start = result.loci[1].front().real(); + float loci0Start = result.loci[0].front().Real(); + float loci1Start = result.loci[1].front().Real(); bool branch0NearP0 = std::abs(loci0Start - p0) < 0.5f; bool branch0NearP1 = std::abs(loci0Start - p1) < 0.5f; @@ -140,11 +139,11 @@ TEST_F(TestRootLocus, asymptote_centroid_three_poles_one_zero) float sumPoles = 0.0f; for (const auto& p : result.openLoopPoles) - sumPoles += p.real(); + sumPoles += p.Real(); float sumZeros = 0.0f; for (const auto& z : result.openLoopZeros) - sumZeros += z.real(); + sumZeros += z.Real(); float centroid = (sumPoles - sumZeros) / static_cast(result.openLoopPoles.size() - result.openLoopZeros.size()); @@ -178,7 +177,7 @@ TEST_F(TestRootLocus, loci_move_left_with_increasing_gain_first_order) auto result = rootLocus.Calculate(num, den, 1.0f, 0.1f, 100.0f); ASSERT_GE(result.loci[0].size(), 2u); - EXPECT_LT(result.loci[0].back().real(), result.loci[0].front().real()); + EXPECT_LT(result.loci[0].back().Real(), result.loci[0].front().Real()); } TEST_F(TestRootLocus, second_order_poles_become_complex_at_high_gain) @@ -193,7 +192,7 @@ TEST_F(TestRootLocus, second_order_poles_become_complex_at_high_gain) bool foundComplex = false; for (const auto& root : result.loci[0]) { - if (std::abs(root.imag()) > 0.1f) + if (std::abs(root.Imaginary()) > 0.1f) { foundComplex = true; break; @@ -201,7 +200,7 @@ TEST_F(TestRootLocus, second_order_poles_become_complex_at_high_gain) } for (const auto& root : result.loci[1]) { - if (std::abs(root.imag()) > 0.1f) + if (std::abs(root.Imaginary()) > 0.1f) { foundComplex = true; break; @@ -222,12 +221,12 @@ TEST_F(TestRootLocus, conjugate_symmetry_when_complex_pair_present) bool verified = false; for (std::size_t i = 0; i < result.loci[0].size(); ++i) { - float im0 = result.loci[0][i].imag(); - float im1 = result.loci[1][i].imag(); + float im0 = result.loci[0][i].Imaginary(); + float im1 = result.loci[1][i].Imaginary(); if (std::abs(im0) > 0.1f && std::abs(im1) > 0.1f) { - float re0 = result.loci[0][i].real(); - float re1 = result.loci[1][i].real(); + float re0 = result.loci[0][i].Real(); + float re1 = result.loci[1][i].Real(); EXPECT_NEAR(re0, re1, 1e-2f); EXPECT_NEAR(im0, -im1, 1e-2f); verified = true; @@ -244,7 +243,7 @@ TEST_F(TestRootLocus, closed_loop_poles_in_lhp_for_stable_gain) auto result = rootLocus.Calculate(num, den, 1.0f); for (const auto& p : result.closedLoopPoles) - EXPECT_LT(p.real(), 0.0f); + EXPECT_LT(p.Real(), 0.0f); } TEST_F(TestRootLocus, all_loci_points_are_finite) @@ -258,8 +257,8 @@ TEST_F(TestRootLocus, all_loci_points_are_finite) { for (const auto& pt : result.loci[b]) { - EXPECT_TRUE(std::isfinite(pt.real())); - EXPECT_TRUE(std::isfinite(pt.imag())); + EXPECT_TRUE(std::isfinite(pt.Real())); + EXPECT_TRUE(std::isfinite(pt.Imaginary())); } } } @@ -278,8 +277,8 @@ TEST_F(TestRootLocus, determinism_same_input_same_output) ASSERT_EQ(result1.loci[b].size(), result2.loci[b].size()); for (std::size_t i = 0; i < result1.loci[b].size(); ++i) { - EXPECT_FLOAT_EQ(result1.loci[b][i].real(), result2.loci[b][i].real()); - EXPECT_FLOAT_EQ(result1.loci[b][i].imag(), result2.loci[b][i].imag()); + EXPECT_FLOAT_EQ(result1.loci[b][i].Real(), result2.loci[b][i].Real()); + EXPECT_FLOAT_EQ(result1.loci[b][i].Imaginary(), result2.loci[b][i].Imaginary()); } } } @@ -315,6 +314,6 @@ TEST_F(TestRootLocus, closed_loop_pole_first_order_analytic) auto result = rootLocus.Calculate(num, den, 2.0f); ASSERT_EQ(result.closedLoopPoles.size(), 1u); - EXPECT_NEAR(result.closedLoopPoles[0].real(), -3.0f, math::Tolerance()); - EXPECT_NEAR(result.closedLoopPoles[0].imag(), 0.0f, math::Tolerance()); + EXPECT_NEAR(result.closedLoopPoles[0].Real(), -3.0f, math::Tolerance()); + EXPECT_NEAR(result.closedLoopPoles[0].Imaginary(), 0.0f, math::Tolerance()); } diff --git a/numerical/filters/active/test/CMakeLists.txt b/numerical/filters/active/test/CMakeLists.txt index 6c1cc174..8b59cfee 100644 --- a/numerical/filters/active/test/CMakeLists.txt +++ b/numerical/filters/active/test/CMakeLists.txt @@ -5,6 +5,7 @@ emil_add_test(numerical.filters.active_test) target_link_libraries(numerical.filters.active_test PUBLIC gmock_main numerical.filters.active + numerical.math_test_helper ) target_sources(numerical.filters.active_test PRIVATE diff --git a/numerical/filters/active/test/TestExtendedKalmanFilter.cpp b/numerical/filters/active/test/TestExtendedKalmanFilter.cpp index 72b937d6..06aafb55 100644 --- a/numerical/filters/active/test/TestExtendedKalmanFilter.cpp +++ b/numerical/filters/active/test/TestExtendedKalmanFilter.cpp @@ -1,22 +1,13 @@ #include "numerical/filters/active/ExtendedKalmanFilter.hpp" #include "numerical/math/Tolerance.hpp" +#include "numerical/math/test_doubles/MatrixTestSupport.hpp" #include namespace { - template - bool AreVectorsNear(const math::Vector& a, - const math::Vector& b, - float epsilon) - { - for (std::size_t i = 0; i < Size; ++i) - if (std::abs(math::ToFloat(a.at(i, 0)) - math::ToFloat(b.at(i, 0))) >= epsilon) - return false; - - return true; - } + using math::test::AreVectorsNear; - // Linear state transition: x_new = F * x (constant velocity model) + using StateVec2 = math::Vector; using StateVec2 = math::Vector; using MeasVec1 = math::Vector; using StateMat2 = math::SquareMatrix; diff --git a/numerical/filters/active/test/TestKalmanFilter.cpp b/numerical/filters/active/test/TestKalmanFilter.cpp index 30df6e72..ed02a3b7 100644 --- a/numerical/filters/active/test/TestKalmanFilter.cpp +++ b/numerical/filters/active/test/TestKalmanFilter.cpp @@ -1,34 +1,13 @@ #include "numerical/filters/active/KalmanFilter.hpp" #include "numerical/math/LinearTimeInvariant.hpp" #include "numerical/math/Tolerance.hpp" +#include "numerical/math/test_doubles/MatrixTestSupport.hpp" #include namespace { - template - bool AreVectorsNear(const math::Vector& a, - const math::Vector& b, - float epsilon) - { - for (std::size_t i = 0; i < Size; ++i) - if (std::abs(math::ToFloat(a.at(i, 0)) - math::ToFloat(b.at(i, 0))) >= epsilon) - return false; - - return true; - } - - template - bool AreMatricesNear(const math::Matrix& a, - const math::Matrix& b, - float epsilon) - { - for (std::size_t i = 0; i < Rows; ++i) - for (std::size_t j = 0; j < Cols; ++j) - if (std::abs(math::ToFloat(a.at(i, j)) - math::ToFloat(b.at(i, j))) >= epsilon) - return false; - - return true; - } + using math::test::AreMatricesNear; + using math::test::AreVectorsNear; template class KalmanFilterTest diff --git a/numerical/math/CMakeLists.txt b/numerical/math/CMakeLists.txt index d282c459..f89db26a 100644 --- a/numerical/math/CMakeLists.txt +++ b/numerical/math/CMakeLists.txt @@ -42,6 +42,8 @@ numerical_add_coverage_sources(numerical.math MatrixExponential.cpp QNumber.cpp Quaternion.cpp + RecursiveBuffer.cpp + Toeplitz.cpp ) add_subdirectory(test) diff --git a/numerical/math/ComplexNumber.hpp b/numerical/math/ComplexNumber.hpp index b0d5fb3a..dce807da 100644 --- a/numerical/math/ComplexNumber.hpp +++ b/numerical/math/ComplexNumber.hpp @@ -1,5 +1,7 @@ #pragma once #include "numerical/math/QNumber.hpp" +#include +#include namespace math { @@ -120,6 +122,13 @@ namespace math return Complex(-real, -imag); } + template + std::enable_if_t, QNumberType> + Abs(const Complex& c) + { + return std::hypot(c.Real(), c.Imaginary()); + } + #ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD extern template class Complex; extern template class Complex; diff --git a/numerical/math/LinearTimeInvariant.hpp b/numerical/math/LinearTimeInvariant.hpp index 84ec75e9..7cff2eb7 100644 --- a/numerical/math/LinearTimeInvariant.hpp +++ b/numerical/math/LinearTimeInvariant.hpp @@ -6,17 +6,9 @@ #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Matrix.hpp" -#include "numerical/math/QNumber.hpp" namespace math { - // Discrete-time linear time-invariant state-space model: - // x_{k+1} = A x_k + B u_k - // y_k = C x_k + D u_k - // - // Template args: - // OutputSize defaults to StateSize. All matrices are zero-initialised by default; - // use the WithFullStateOutput factory to get C = I. template #include @@ -17,7 +18,7 @@ namespace math { template inline constexpr bool is_supported_type_v = - std::disjunction_v, is_qnumber>; + std::is_floating_point_v || is_qnumber_v; template inline constexpr bool is_valid_dimensions_v = (Rows > 0 && Cols > 0); @@ -143,8 +144,6 @@ namespace math return Matrix(init); } - // Implementation // - template constexpr Matrix::Matrix() noexcept : data{} @@ -287,9 +286,9 @@ namespace math for (size_type i = 0; i < Rows; ++i) { if constexpr (std::is_floating_point_v) - result.at(i, i) = T(1); + result.at(i, i) = T{ 1 }; else - result.at(i, i) = T(0.9999f); + result.at(i, i) = T{ 0.9999f }; } return result; @@ -309,11 +308,9 @@ namespace math return sum; } - // Implementation of new block methods // - template template - constexpr void + OPTIMIZE_FOR_SPEED constexpr void Matrix::SetBlock(const Matrix& src, size_type rowOffset, size_type colOffset) { @@ -326,7 +323,7 @@ namespace math template template - [[nodiscard]] constexpr Matrix + [[nodiscard]] OPTIMIZE_FOR_SPEED constexpr Matrix Matrix::GetBlock(size_type rowOffset, size_type colOffset) const { static_assert(BlockRows <= Rows && BlockCols <= Cols, @@ -339,7 +336,7 @@ namespace math } template - [[nodiscard]] constexpr Matrix + [[nodiscard]] OPTIMIZE_FOR_SPEED constexpr Matrix Matrix::GetColumn(size_type col) const { Vector result; @@ -348,6 +345,17 @@ namespace math return result; } + template + OPTIMIZE_FOR_SPEED constexpr void SwapRows(Matrix& matrix, size_t row1, size_t row2) + { + for (size_t j = 0; j < Cols; ++j) + { + T tmp = matrix.at(row1, j); + matrix.at(row1, j) = matrix.at(row2, j); + matrix.at(row2, j) = tmp; + } + } + #ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD extern template class Matrix; extern template class Matrix; diff --git a/numerical/math/RecursiveBuffer.cpp b/numerical/math/RecursiveBuffer.cpp new file mode 100644 index 00000000..4aa46670 --- /dev/null +++ b/numerical/math/RecursiveBuffer.cpp @@ -0,0 +1,9 @@ +#include "numerical/math/QNumber.hpp" +#include "numerical/math/RecursiveBuffer.hpp" + +namespace math +{ + template class RecursiveBuffer; + template class RecursiveBuffer; + template class RecursiveBuffer; +} diff --git a/numerical/math/RecursiveBuffer.hpp b/numerical/math/RecursiveBuffer.hpp index 9a612831..179ada36 100644 --- a/numerical/math/RecursiveBuffer.hpp +++ b/numerical/math/RecursiveBuffer.hpp @@ -94,4 +94,10 @@ namespace math { return buffer[n.offset]; } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class RecursiveBuffer; + extern template class RecursiveBuffer; + extern template class RecursiveBuffer; +#endif } diff --git a/numerical/math/Toeplitz.cpp b/numerical/math/Toeplitz.cpp new file mode 100644 index 00000000..1c2a8304 --- /dev/null +++ b/numerical/math/Toeplitz.cpp @@ -0,0 +1,9 @@ +#include "numerical/math/QNumber.hpp" +#include "numerical/math/Toeplitz.hpp" + +namespace math +{ + template class ToeplitzMatrix; + template class ToeplitzMatrix; + template class ToeplitzMatrix; +} diff --git a/numerical/math/Toeplitz.hpp b/numerical/math/Toeplitz.hpp index e29ccecc..7d4db9ed 100644 --- a/numerical/math/Toeplitz.hpp +++ b/numerical/math/Toeplitz.hpp @@ -162,4 +162,10 @@ namespace math { return ToeplitzMatrix(autocorrelation); } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class ToeplitzMatrix; + extern template class ToeplitzMatrix; + extern template class ToeplitzMatrix; +#endif } diff --git a/numerical/math/TriangularSolve.hpp b/numerical/math/TriangularSolve.hpp index a456c179..ec82c2e8 100644 --- a/numerical/math/TriangularSolve.hpp +++ b/numerical/math/TriangularSolve.hpp @@ -13,6 +13,25 @@ namespace math { + template + [[nodiscard]] OPTIMIZE_FOR_SPEED std::size_t FindPartialPivotRow(const Matrix& matrix, std::size_t col) + { + std::size_t pivotRow = col; + float maxVal = std::abs(ToFloat(matrix.at(col, col))); + + for (std::size_t row = col + 1; row < N; ++row) + { + float absVal = std::abs(ToFloat(matrix.at(row, col))); + if (absVal > maxVal) + { + maxVal = absVal; + pivotRow = row; + } + } + + return pivotRow; + } + template [[nodiscard]] OPTIMIZE_FOR_SPEED Vector SolveUnitLowerTriangular(const Matrix& l, const Vector& c) { diff --git a/numerical/math/test/CMakeLists.txt b/numerical/math/test/CMakeLists.txt index 6a1e74dd..804e38c5 100644 --- a/numerical/math/test/CMakeLists.txt +++ b/numerical/math/test/CMakeLists.txt @@ -5,6 +5,7 @@ emil_add_test(numerical.math_test) target_link_libraries(numerical.math_test PUBLIC gmock_main numerical.math + numerical.math_test_helper ) target_sources(numerical.math_test PRIVATE diff --git a/numerical/math/test/TestComplexNumber.cpp b/numerical/math/test/TestComplexNumber.cpp index 938230ff..2d50728a 100644 --- a/numerical/math/test/TestComplexNumber.cpp +++ b/numerical/math/test/TestComplexNumber.cpp @@ -1,151 +1,290 @@ #include "numerical/math/ComplexNumber.hpp" +#include "numerical/math/Tolerance.hpp" +#include #include namespace { - template - class ComplexTest + class TestComplexNumber : public ::testing::Test { protected: - using ComplexType = math::Complex; - static constexpr float kEpsilon = 1e-4f; - - static ComplexType MakeComplex(float real, float imag) - { - return ComplexType(QNumberType(real), QNumberType(imag)); - } - - static float ToFloat(const float& value) - { - return value; - } - - static float ToFloat(const math::Q15& value) - { - return value.ToFloat(); - } - - static float ToFloat(const math::Q31& value) - { - return value.ToFloat(); - } + using ComplexType = math::Complex; }; +} + +TEST_F(TestComplexNumber, DefaultConstructorIsZero) +{ + ComplexType num; + + EXPECT_NEAR(num.Real(), 0.0f, math::Tolerance()); + EXPECT_NEAR(num.Imaginary(), 0.0f, math::Tolerance()); +} + +TEST_F(TestComplexNumber, ComponentConstructorStoresValues) +{ + ComplexType num(0.5f, 0.3f); + + EXPECT_NEAR(num.Real(), 0.5f, math::Tolerance()); + EXPECT_NEAR(num.Imaginary(), 0.3f, math::Tolerance()); +} + +TEST_F(TestComplexNumber, Addition) +{ + ComplexType a(0.3f, 0.4f); + ComplexType b(0.1f, 0.2f); + + ComplexType result = a + b; + + EXPECT_NEAR(result.Real(), 0.4f, math::Tolerance()); + EXPECT_NEAR(result.Imaginary(), 0.6f, math::Tolerance()); +} + +TEST_F(TestComplexNumber, Subtraction) +{ + ComplexType a(0.3f, 0.4f); + ComplexType b(0.1f, 0.2f); + + ComplexType result = a - b; + + EXPECT_NEAR(result.Real(), 0.2f, math::Tolerance()); + EXPECT_NEAR(result.Imaginary(), 0.2f, math::Tolerance()); +} + +TEST_F(TestComplexNumber, Multiplication) +{ + ComplexType a(0.3f, 0.4f); + ComplexType b(0.1f, 0.2f); - using TestedTypes = ::testing::Types; - TYPED_TEST_SUITE(ComplexTest, TestedTypes); + ComplexType result = a * b; + + EXPECT_NEAR(result.Real(), -0.05f, math::Tolerance()); + EXPECT_NEAR(result.Imaginary(), 0.10f, math::Tolerance()); } -TYPED_TEST(ComplexTest, DefaultConstructor) +TEST_F(TestComplexNumber, Division) { - typename TestFixture::ComplexType num; - EXPECT_FLOAT_EQ(this->ToFloat(num.Real()), 0.0f); - EXPECT_FLOAT_EQ(this->ToFloat(num.Imaginary()), 0.0f); + ComplexType a(0.3f, 0.4f); + ComplexType b(0.1f, 0.2f); + + ComplexType result = a / b; + + EXPECT_NEAR(result.Real(), 2.2f, math::Tolerance()); + EXPECT_NEAR(result.Imaginary(), -0.4f, math::Tolerance()); } -TYPED_TEST(ComplexTest, ComponentConstructor) +TEST_F(TestComplexNumber, DivisionByPureRealDivisor) { - TypeParam real(0.5f); - TypeParam imag(0.3f); - typename TestFixture::ComplexType num(real, imag); + ComplexType a(0.3f, 0.4f); + ComplexType b(2.0f, 0.0f); - EXPECT_NEAR(this->ToFloat(num.Real()), 0.5f, TestFixture::kEpsilon); - EXPECT_NEAR(this->ToFloat(num.Imaginary()), 0.3f, TestFixture::kEpsilon); + ComplexType result = a / b; + + EXPECT_NEAR(result.Real(), 0.15f, math::Tolerance()); + EXPECT_NEAR(result.Imaginary(), 0.2f, math::Tolerance()); } -TYPED_TEST(ComplexTest, FloatConstructor) +TEST_F(TestComplexNumber, DivisionByPureImaginaryDivisor) { - auto num = TestFixture::MakeComplex(0.5f, 0.3f); + ComplexType a(0.3f, 0.4f); + ComplexType b(0.0f, 1.0f); + + ComplexType result = a / b; - EXPECT_NEAR(this->ToFloat(num.Real()), 0.5f, TestFixture::kEpsilon); - EXPECT_NEAR(this->ToFloat(num.Imaginary()), 0.3f, TestFixture::kEpsilon); + EXPECT_NEAR(result.Real(), 0.4f, math::Tolerance()); + EXPECT_NEAR(result.Imaginary(), -0.3f, math::Tolerance()); } -TYPED_TEST(ComplexTest, Addition) +TEST_F(TestComplexNumber, MultiplyThenDivideRoundTripRecoversOperand) { - auto a = TestFixture::MakeComplex(0.3f, 0.4f); - auto b = TestFixture::MakeComplex(0.1f, 0.2f); - typename TestFixture::ComplexType result = a + b; + ComplexType a(0.3f, 0.4f); + ComplexType b(0.1f, 0.2f); + + ComplexType result = (a * b) / b; - EXPECT_NEAR(this->ToFloat(result.Real()), 0.4f, TestFixture::kEpsilon); - EXPECT_NEAR(this->ToFloat(result.Imaginary()), 0.6f, TestFixture::kEpsilon); + EXPECT_NEAR(result.Real(), 0.3f, math::Tolerance()); + EXPECT_NEAR(result.Imaginary(), 0.4f, math::Tolerance()); } -TYPED_TEST(ComplexTest, Subtraction) +TEST_F(TestComplexNumber, AdditiveIdentity) { - auto a = TestFixture::MakeComplex(0.3f, 0.4f); - auto b = TestFixture::MakeComplex(0.1f, 0.2f); - typename TestFixture::ComplexType result = a - b; + ComplexType a(0.3f, -0.4f); + ComplexType zero; - EXPECT_NEAR(this->ToFloat(result.Real()), 0.2f, TestFixture::kEpsilon); - EXPECT_NEAR(this->ToFloat(result.Imaginary()), 0.2f, TestFixture::kEpsilon); + ComplexType result = a + zero; + + EXPECT_NEAR(result.Real(), 0.3f, math::Tolerance()); + EXPECT_NEAR(result.Imaginary(), -0.4f, math::Tolerance()); } -TYPED_TEST(ComplexTest, Multiplication) +TEST_F(TestComplexNumber, MultiplicativeIdentity) { - auto a = TestFixture::MakeComplex(0.3f, 0.4f); - auto b = TestFixture::MakeComplex(0.1f, 0.2f); - typename TestFixture::ComplexType result = a * b; + ComplexType a(0.3f, -0.4f); + ComplexType one(1.0f, 0.0f); + + ComplexType result = a * one; - // (0.3 + 0.4i)(0.1 + 0.2i) = (0.3*0.1 - 0.4*0.2) + (0.3*0.2 + 0.4*0.1)i - EXPECT_NEAR(this->ToFloat(result.Real()), -0.05f, TestFixture::kEpsilon); - EXPECT_NEAR(this->ToFloat(result.Imaginary()), 0.10f, TestFixture::kEpsilon); + EXPECT_NEAR(result.Real(), 0.3f, math::Tolerance()); + EXPECT_NEAR(result.Imaginary(), -0.4f, math::Tolerance()); } -TYPED_TEST(ComplexTest, CompoundAddition) +TEST_F(TestComplexNumber, ImaginaryUnitSquaredIsNegativeOne) { - auto a = TestFixture::MakeComplex(0.3f, 0.4f); - auto b = TestFixture::MakeComplex(0.1f, 0.2f); + ComplexType i(0.0f, 1.0f); + + ComplexType result = i * i; + + EXPECT_NEAR(result.Real(), -1.0f, math::Tolerance()); + EXPECT_NEAR(result.Imaginary(), 0.0f, math::Tolerance()); +} + +TEST_F(TestComplexNumber, AdditiveInverseYieldsZero) +{ + ComplexType a(0.7f, -0.5f); + + ComplexType result = a + (-a); + + EXPECT_NEAR(result.Real(), 0.0f, math::Tolerance()); + EXPECT_NEAR(result.Imaginary(), 0.0f, math::Tolerance()); +} + +TEST_F(TestComplexNumber, MultiplicationIsCommutative) +{ + ComplexType a(0.3f, 0.4f); + ComplexType b(0.5f, -0.2f); + + ComplexType ab = a * b; + ComplexType ba = b * a; + + EXPECT_NEAR(ab.Real(), ba.Real(), math::Tolerance()); + EXPECT_NEAR(ab.Imaginary(), ba.Imaginary(), math::Tolerance()); +} + +TEST_F(TestComplexNumber, MultiplicationDistributesOverAddition) +{ + ComplexType a(0.3f, 0.4f); + ComplexType b(0.1f, 0.2f); + ComplexType c(0.5f, -0.1f); + + ComplexType lhs = a * (b + c); + ComplexType rhs = a * b + a * c; + + EXPECT_NEAR(lhs.Real(), rhs.Real(), math::Tolerance()); + EXPECT_NEAR(lhs.Imaginary(), rhs.Imaginary(), math::Tolerance()); +} + +TEST_F(TestComplexNumber, AbsMatchesHypot) +{ + ComplexType a(3.0f, 4.0f); + + float result = math::Abs(a); + + EXPECT_NEAR(result, 5.0f, math::Tolerance()); +} + +TEST_F(TestComplexNumber, AbsOfPureReal) +{ + ComplexType a(-2.5f, 0.0f); + + float result = math::Abs(a); + + EXPECT_NEAR(result, 2.5f, math::Tolerance()); +} + +TEST_F(TestComplexNumber, AbsOfZeroIsZero) +{ + ComplexType zero; + + float result = math::Abs(zero); + + EXPECT_NEAR(result, 0.0f, math::Tolerance()); +} + +TEST_F(TestComplexNumber, ModulusSquaredEqualsSumOfSquares) +{ + ComplexType a(3.0f, 4.0f); + + float modulus = math::Abs(a); + float expected = std::sqrt(a.Real() * a.Real() + a.Imaginary() * a.Imaginary()); + + EXPECT_NEAR(modulus, expected, math::Tolerance()); +} + +TEST_F(TestComplexNumber, CompoundAddition) +{ + ComplexType a(0.3f, 0.4f); + ComplexType b(0.1f, 0.2f); + a += b; - EXPECT_NEAR(this->ToFloat(a.Real()), 0.4f, TestFixture::kEpsilon); - EXPECT_NEAR(this->ToFloat(a.Imaginary()), 0.6f, TestFixture::kEpsilon); + EXPECT_NEAR(a.Real(), 0.4f, math::Tolerance()); + EXPECT_NEAR(a.Imaginary(), 0.6f, math::Tolerance()); } -TYPED_TEST(ComplexTest, CompoundSubtraction) +TEST_F(TestComplexNumber, CompoundSubtraction) { - auto a = TestFixture::MakeComplex(0.3f, 0.4f); - auto b = TestFixture::MakeComplex(0.1f, 0.2f); + ComplexType a(0.3f, 0.4f); + ComplexType b(0.1f, 0.2f); + a -= b; - EXPECT_NEAR(this->ToFloat(a.Real()), 0.2f, TestFixture::kEpsilon); - EXPECT_NEAR(this->ToFloat(a.Imaginary()), 0.2f, TestFixture::kEpsilon); + EXPECT_NEAR(a.Real(), 0.2f, math::Tolerance()); + EXPECT_NEAR(a.Imaginary(), 0.2f, math::Tolerance()); } -TYPED_TEST(ComplexTest, CompoundMultiplication) +TEST_F(TestComplexNumber, CompoundMultiplication) { - auto a = TestFixture::MakeComplex(0.3f, 0.4f); - auto b = TestFixture::MakeComplex(0.1f, 0.2f); + ComplexType a(0.3f, 0.4f); + ComplexType b(0.1f, 0.2f); + a *= b; - EXPECT_NEAR(this->ToFloat(a.Real()), -0.05f, TestFixture::kEpsilon); - EXPECT_NEAR(this->ToFloat(a.Imaginary()), 0.10f, TestFixture::kEpsilon); + EXPECT_NEAR(a.Real(), -0.05f, math::Tolerance()); + EXPECT_NEAR(a.Imaginary(), 0.10f, math::Tolerance()); +} + +TEST_F(TestComplexNumber, UnaryPlus) +{ + ComplexType a(0.3f, 0.4f); + + ComplexType result = +a; + + EXPECT_NEAR(result.Real(), 0.3f, math::Tolerance()); + EXPECT_NEAR(result.Imaginary(), 0.4f, math::Tolerance()); } -TYPED_TEST(ComplexTest, UnaryPlus) +TEST_F(TestComplexNumber, UnaryNegation) { - auto a = TestFixture::MakeComplex(0.3f, 0.4f); - typename TestFixture::ComplexType result = +a; + ComplexType a(0.3f, 0.4f); + + ComplexType result = -a; - EXPECT_NEAR(this->ToFloat(result.Real()), 0.3f, TestFixture::kEpsilon); - EXPECT_NEAR(this->ToFloat(result.Imaginary()), 0.4f, TestFixture::kEpsilon); + EXPECT_NEAR(result.Real(), -0.3f, math::Tolerance()); + EXPECT_NEAR(result.Imaginary(), -0.4f, math::Tolerance()); } -TYPED_TEST(ComplexTest, UnaryNegation) +TEST_F(TestComplexNumber, DoubleNegationIsIdentity) { - auto a = TestFixture::MakeComplex(0.3f, 0.4f); - typename TestFixture::ComplexType result = -a; + ComplexType a(0.3f, -0.7f); - EXPECT_NEAR(this->ToFloat(result.Real()), -0.3f, TestFixture::kEpsilon); - EXPECT_NEAR(this->ToFloat(result.Imaginary()), -0.4f, TestFixture::kEpsilon); + ComplexType result = -(-a); + + EXPECT_NEAR(result.Real(), 0.3f, math::Tolerance()); + EXPECT_NEAR(result.Imaginary(), -0.7f, math::Tolerance()); } -TYPED_TEST(ComplexTest, EqualityComparison) +TEST_F(TestComplexNumber, EqualityHoldsForIdenticalComponents) { - auto a = TestFixture::MakeComplex(0.3f, 0.4f); - auto b = TestFixture::MakeComplex(0.3f, 0.4f); - auto c = TestFixture::MakeComplex(0.3f, 0.5f); + ComplexType a(0.3f, 0.4f); + ComplexType b(0.3f, 0.4f); EXPECT_TRUE(a == b); +} + +TEST_F(TestComplexNumber, EqualityFailsForDifferentImaginaryComponent) +{ + ComplexType a(0.3f, 0.4f); + ComplexType c(0.3f, 0.5f); + EXPECT_FALSE(a == c); } diff --git a/numerical/math/test/TestCordic.cpp b/numerical/math/test/TestCordic.cpp index b97636c5..2ff0aabb 100644 --- a/numerical/math/test/TestCordic.cpp +++ b/numerical/math/test/TestCordic.cpp @@ -25,11 +25,12 @@ TEST_F(TestCordic, SinCosZero) TEST_F(TestCordic, SinCosQuarterPi) { auto result = cordic.SineCosine(std::numbers::pi_v / 4.0f); - EXPECT_NEAR(result.sin, 0.7071f, math::Tolerance()); - EXPECT_NEAR(result.cos, 0.7071f, math::Tolerance()); + const float ref{ std::sqrt(2.0f) / 2.0f }; + EXPECT_NEAR(result.sin, ref, math::Tolerance()); + EXPECT_NEAR(result.cos, ref, math::Tolerance()); } -TEST_F(TestCordic, SinCosMatchesStdOverSweep) +TEST_F(TestCordic, SinCosMatchesStdInConvergenceDomain) { const float tolerance{ 1.0f / static_cast(1 << 15) }; std::array angles{ -1.5f, -1.0f, -0.5f, -0.25f, 0.0f, 0.25f, 0.5f, 1.0f, 1.5f }; @@ -43,7 +44,7 @@ TEST_F(TestCordic, SinCosMatchesStdOverSweep) TEST_F(TestCordic, PythagoreanIdentity) { - std::array angles{ -1.5f, -1.0f, -0.5f, 0.0f, 0.5f, 1.0f, 1.5f }; + std::array angles{ -2.5f, -2.0f, -1.5f, -1.0f, 0.0f, 1.0f, 1.5f, 2.0f, 2.5f }; for (float angle : angles) { auto result = cordic.SineCosine(angle); @@ -52,6 +53,24 @@ TEST_F(TestCordic, PythagoreanIdentity) } } +TEST_F(TestCordic, SinCosAboveHalfPi) +{ + const float angle{ 2.0f }; + const float tol{ 1.0f / static_cast(1 << 15) }; + auto result = cordic.SineCosine(angle); + EXPECT_NEAR(result.sin, std::sin(angle), tol); + EXPECT_NEAR(result.cos, std::cos(angle), tol); +} + +TEST_F(TestCordic, SinCosBelowNegHalfPi) +{ + const float angle{ -2.0f }; + const float tol{ 1.0f / static_cast(1 << 15) }; + auto result = cordic.SineCosine(angle); + EXPECT_NEAR(result.sin, std::sin(angle), tol); + EXPECT_NEAR(result.cos, std::cos(angle), tol); +} + TEST_F(TestCordic, Atan2AllQuadrants) { const float tol{ math::Tolerance() }; @@ -67,9 +86,21 @@ TEST_F(TestCordic, Atan2Axes) const float tol{ math::Tolerance() }; EXPECT_NEAR(cordic.Arctangent2(0.0f, 1.0f), 0.0f, tol); EXPECT_NEAR(cordic.Arctangent2(1.0f, 0.0f), pi / 2.0f, tol); + EXPECT_NEAR(cordic.Arctangent2(-1.0f, 0.0f), -pi / 2.0f, tol); EXPECT_NEAR(cordic.Arctangent2(0.0f, -1.0f), pi, tol); } +TEST_F(TestCordic, Atan2RoundTripWithSineCosine) +{ + std::array angles{ -1.4f, -1.0f, -0.5f, 0.0f, 0.5f, 1.0f, 1.4f }; + for (float angle : angles) + { + auto sc = cordic.SineCosine(angle); + float recovered{ cordic.Arctangent2(sc.sin, sc.cos) }; + EXPECT_NEAR(recovered, angle, math::Tolerance()); + } +} + TEST_F(TestCordic, MagnitudeMatchesHypot) { const float tol{ math::Tolerance() }; @@ -78,7 +109,20 @@ TEST_F(TestCordic, MagnitudeMatchesHypot) EXPECT_NEAR(cordic.Magnitude(0.0f, 0.5f), 0.5f, tol); } -TEST_F(TestCordic, RotateVector) +TEST_F(TestCordic, MagnitudeNegativeComponents) +{ + const float tol{ math::Tolerance() }; + EXPECT_NEAR(cordic.Magnitude(-0.6f, 0.8f), std::hypot(0.6f, 0.8f), tol); + EXPECT_NEAR(cordic.Magnitude(0.6f, -0.8f), std::hypot(0.6f, 0.8f), tol); + EXPECT_NEAR(cordic.Magnitude(-0.6f, -0.8f), std::hypot(0.6f, 0.8f), tol); +} + +TEST_F(TestCordic, MagnitudeZeroInput) +{ + EXPECT_NEAR(cordic.Magnitude(0.0f, 0.0f), 0.0f, math::Tolerance()); +} + +TEST_F(TestCordic, RotateVectorByHalfPi) { const float pi{ std::numbers::pi_v }; std::array v{ 1.0f, 0.0f }; @@ -87,22 +131,35 @@ TEST_F(TestCordic, RotateVector) EXPECT_NEAR(result[1], 1.0f, math::Tolerance()); } -TEST_F(TestCordic, SinCosAboveHalfPi) +TEST_F(TestCordic, RotateIdentity) { - const float angle{ 2.0f }; - const float tol{ 1.0f / static_cast(1 << 15) }; - auto result = cordic.SineCosine(angle); - EXPECT_NEAR(result.sin, std::sin(angle), tol); - EXPECT_NEAR(result.cos, std::cos(angle), tol); + std::array v{ 0.6f, 0.8f }; + auto result = cordic.Rotate(v, 0.0f); + EXPECT_NEAR(result[0], 0.6f, math::Tolerance()); + EXPECT_NEAR(result[1], 0.8f, math::Tolerance()); } -TEST_F(TestCordic, SinCosBelowNegHalfPi) +TEST_F(TestCordic, RotateByPi) { - const float angle{ -2.0f }; + const float pi{ std::numbers::pi_v }; + std::array v{ 1.0f, 0.0f }; + auto result = cordic.Rotate(v, pi); const float tol{ 1.0f / static_cast(1 << 15) }; - auto result = cordic.SineCosine(angle); - EXPECT_NEAR(result.sin, std::sin(angle), tol); - EXPECT_NEAR(result.cos, std::cos(angle), tol); + EXPECT_NEAR(result[0], -1.0f, tol); + EXPECT_NEAR(result[1], 0.0f, tol); +} + +TEST_F(TestCordic, RotatePreservesNorm) +{ + std::array angles{ -2.0f, -1.0f, -0.5f, 0.0f, 0.5f, 1.0f, 2.0f }; + std::array v{ 3.0f, 4.0f }; + const float normV{ std::hypot(v[0], v[1]) }; + for (float angle : angles) + { + auto rotated = cordic.Rotate(v, angle); + float normRotated{ std::hypot(rotated[0], rotated[1]) }; + EXPECT_NEAR(normRotated, normV, math::Tolerance()); + } } TEST_F(TestCordic, AccuracyScalesWithIterations) diff --git a/numerical/math/test/TestGivensRotation.cpp b/numerical/math/test/TestGivensRotation.cpp index b5623c31..9f0e7bdf 100644 --- a/numerical/math/test/TestGivensRotation.cpp +++ b/numerical/math/test/TestGivensRotation.cpp @@ -1,7 +1,9 @@ #include "numerical/math/GivensRotation.hpp" #include "numerical/math/Tolerance.hpp" +#include #include #include +#include namespace { @@ -22,6 +24,13 @@ TEST_F(GivensRotationTest, ZerosSecondComponent) EXPECT_NEAR(y, 0.0f, math::Tolerance()); } +TEST_F(GivensRotationTest, ComputeGivensUnitarity) +{ + auto g = math::ComputeGivens(5.0f, 12.0f); + + EXPECT_NEAR(g.c * g.c + g.s * g.s, 1.0f, math::Tolerance()); +} + TEST_F(GivensRotationTest, PreservesNorm) { auto g = math::ComputeGivens(1.0f, 2.0f); @@ -42,16 +51,68 @@ TEST_F(GivensRotationTest, DegenerateInputIsIdentity) EXPECT_NEAR(g.s, 0.0f, math::Tolerance()); } -TEST_F(GivensRotationTest, JacobiRotationAnnihilatesSymmetricOffDiagonal) +TEST_F(GivensRotationTest, SubthresholdInputIsIdentity) +{ + auto g = math::ComputeGivens(1e-31f, 1e-31f); + + EXPECT_NEAR(g.c, 1.0f, math::Tolerance()); + EXPECT_NEAR(g.s, 0.0f, math::Tolerance()); +} + +TEST_F(GivensRotationTest, ApplyGivensOrthogonalBasisVectors) +{ + auto g = math::ComputeGivens(3.0f, 4.0f); + + float x0 = 1.0f, y0 = 0.0f; + float x1 = 0.0f, y1 = 1.0f; + math::ApplyGivens(g, x0, y0); + math::ApplyGivens(g, x1, y1); + + float dot = x0 * x1 + y0 * y1; + EXPECT_NEAR(dot, 0.0f, math::Tolerance()); + EXPECT_NEAR(x0 * x0 + y0 * y0, 1.0f, math::Tolerance()); + EXPECT_NEAR(x1 * x1 + y1 * y1, 1.0f, math::Tolerance()); +} + +TEST_F(GivensRotationTest, JacobiRotationPositiveTheta) +{ + float app = 1.0f, aqq = 4.0f, apq = 2.0f; + auto g = math::ComputeJacobiRotation(app, aqq, apq); + + float ref_c = 1.0f / std::sqrt(1.25f); + float ref_s = 0.5f / std::sqrt(1.25f); + + EXPECT_NEAR(g.c, ref_c, math::Tolerance()); + EXPECT_NEAR(g.s, ref_s, math::Tolerance()); +} + +TEST_F(GivensRotationTest, JacobiRotationNegativeTheta) { - float app = 4.0f; - float aqq = 1.0f; - float apq = 2.0f; + float app = 4.0f, aqq = 1.0f, apq = 2.0f; + auto g = math::ComputeJacobiRotation(app, aqq, apq); + + float ref_c = 1.0f / std::sqrt(1.25f); + float ref_s = -0.5f / std::sqrt(1.25f); + + EXPECT_NEAR(g.c, ref_c, math::Tolerance()); + EXPECT_NEAR(g.s, ref_s, math::Tolerance()); +} +TEST_F(GivensRotationTest, JacobiRotationAnnihilatesSymmetricOffDiagonal) +{ + float app = 4.0f, aqq = 1.0f, apq = 2.0f; auto g = math::ComputeJacobiRotation(app, aqq, apq); + float offDiagonal = (g.c * g.c - g.s * g.s) * apq + g.c * g.s * (app - aqq); EXPECT_NEAR(offDiagonal, 0.0f, math::Tolerance()); +} + +TEST_F(GivensRotationTest, JacobiRotationUnitarity) +{ + float app = 3.0f, aqq = 7.0f, apq = 2.0f; + auto g = math::ComputeJacobiRotation(app, aqq, apq); + EXPECT_NEAR(g.c * g.c + g.s * g.s, 1.0f, math::Tolerance()); } @@ -62,3 +123,31 @@ TEST_F(GivensRotationTest, JacobiRotationZeroOffDiagonalIsIdentity) EXPECT_NEAR(g.c, 1.0f, math::Tolerance()); EXPECT_NEAR(g.s, 0.0f, math::Tolerance()); } + +TEST_F(GivensRotationTest, NormPreservationOverSeededSweep) +{ + std::mt19937 prng{ 42u }; + std::uniform_real_distribution dist{ -10.0f, 10.0f }; + + std::array as{}; + std::array bs{}; + for (auto& v : as) + v = dist(prng); + for (auto& v : bs) + v = dist(prng); + + for (std::size_t i = 0; i < as.size(); ++i) + { + float norm = std::sqrt(as[i] * as[i] + bs[i] * bs[i]); + if (norm < 1e-30f) + continue; + + auto g = math::ComputeGivens(as[i], bs[i]); + float x = as[i]; + float y = bs[i]; + math::ApplyGivens(g, x, y); + + EXPECT_NEAR(std::sqrt(x * x + y * y), norm, math::Tolerance()); + EXPECT_NEAR(y, 0.0f, math::Tolerance()); + } +} diff --git a/numerical/math/test/TestHouseholderTransform.cpp b/numerical/math/test/TestHouseholderTransform.cpp index 1fade4f1..125a659f 100644 --- a/numerical/math/test/TestHouseholderTransform.cpp +++ b/numerical/math/test/TestHouseholderTransform.cpp @@ -22,6 +22,7 @@ namespace }; } + TEST_F(HouseholderTransformTest, ZerosEntriesBelowPivot) { math::Vector x{ { 4.0f }, { 3.0f }, { 0.0f }, { 0.0f } }; @@ -62,3 +63,108 @@ TEST_F(HouseholderTransformTest, ZeroSubvectorYieldsZeroBeta) EXPECT_NEAR(beta, 0.0f, math::Tolerance()); } + +TEST_F(HouseholderTransformTest, ApplyReflectorLeftZerosSubcolumn) +{ + math::Matrix a{ 12.0f, -51.0f, 4.0f, + 6.0f, 167.0f, -68.0f, + -4.0f, 24.0f, -41.0f }; + + math::Vector col0{ { 12.0f }, { 6.0f }, { -4.0f } }; + math::Vector v; + float beta{}; + math::HouseholderVector(col0, 0, v, beta); + + math::ApplyReflectorLeft(a, v, beta, 0, 0); + + EXPECT_NEAR(a.at(0, 0), 14.0f, math::Tolerance()); + EXPECT_NEAR(a.at(1, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(a.at(2, 0), 0.0f, math::Tolerance()); +} + +TEST_F(HouseholderTransformTest, ApplyReflectorLeftRespectsColStart) +{ + math::Matrix a{ 12.0f, -51.0f, 4.0f, + 6.0f, 167.0f, -68.0f, + -4.0f, 24.0f, -41.0f }; + + math::Vector col0{ { 12.0f }, { 6.0f }, { -4.0f } }; + math::Vector v; + float beta{}; + math::HouseholderVector(col0, 0, v, beta); + + math::ApplyReflectorLeft(a, v, beta, 0, 1); + + EXPECT_NEAR(a.at(0, 0), 12.0f, math::Tolerance()); + EXPECT_NEAR(a.at(1, 0), 6.0f, math::Tolerance()); + EXPECT_NEAR(a.at(2, 0), -4.0f, math::Tolerance()); +} + +TEST_F(HouseholderTransformTest, ApplyReflectorLeftIsInvolution) +{ + math::Matrix a{ 12.0f, -51.0f, 4.0f, + 6.0f, 167.0f, -68.0f, + -4.0f, 24.0f, -41.0f }; + + math::Vector col0{ { 12.0f }, { 6.0f }, { -4.0f } }; + math::Vector v; + float beta{}; + math::HouseholderVector(col0, 0, v, beta); + + math::ApplyReflectorLeft(a, v, beta, 0, 0); + math::ApplyReflectorLeft(a, v, beta, 0, 0); + + EXPECT_NEAR(a.at(0, 0), 12.0f, math::Tolerance()); + EXPECT_NEAR(a.at(1, 0), 6.0f, math::Tolerance()); + EXPECT_NEAR(a.at(2, 0), -4.0f, math::Tolerance()); +} + +TEST_F(HouseholderTransformTest, ApplyReflectorRightZerosSubrow) +{ + math::Matrix b{ 3.0f, 0.0f, 8.0f, + 5.0f, 12.0f, 16.0f }; + + math::Vector row0{ { 0.0f }, { 0.0f }, { 8.0f } }; + math::Vector v; + float beta{}; + math::HouseholderVector(row0, 1, v, beta); + + math::ApplyReflectorRight(b, v, beta, 0, 1); + + EXPECT_NEAR(b.at(0, 0), 3.0f, math::Tolerance()); + EXPECT_NEAR(b.at(0, 1), 8.0f, math::Tolerance()); + EXPECT_NEAR(b.at(0, 2), 0.0f, math::Tolerance()); +} + +TEST_F(HouseholderTransformTest, ApplyReflectorRightTransformsAllRows) +{ + math::Matrix b{ 3.0f, 0.0f, 8.0f, + 5.0f, 12.0f, 16.0f }; + + math::Vector row0{ { 0.0f }, { 0.0f }, { 8.0f } }; + math::Vector v; + float beta{}; + math::HouseholderVector(row0, 1, v, beta); + + math::ApplyReflectorRight(b, v, beta, 0, 1); + + EXPECT_NEAR(b.at(1, 1), 16.0f, math::Tolerance()); + EXPECT_NEAR(b.at(1, 2), 12.0f, math::Tolerance()); +} + +TEST_F(HouseholderTransformTest, ApplyReflectorRightIsInvolution) +{ + math::Matrix b{ 3.0f, 0.0f, 8.0f, + 5.0f, 12.0f, 16.0f }; + + math::Vector row0{ { 0.0f }, { 0.0f }, { 8.0f } }; + math::Vector v; + float beta{}; + math::HouseholderVector(row0, 1, v, beta); + + math::ApplyReflectorRight(b, v, beta, 0, 1); + math::ApplyReflectorRight(b, v, beta, 0, 1); + + EXPECT_NEAR(b.at(0, 1), 0.0f, math::Tolerance()); + EXPECT_NEAR(b.at(0, 2), 8.0f, math::Tolerance()); +} diff --git a/numerical/math/test/TestLinearTimeInvariant.cpp b/numerical/math/test/TestLinearTimeInvariant.cpp index 3dacd0fd..60ff1536 100644 --- a/numerical/math/test/TestLinearTimeInvariant.cpp +++ b/numerical/math/test/TestLinearTimeInvariant.cpp @@ -1,198 +1,146 @@ #include "numerical/math/LinearTimeInvariant.hpp" +#include "numerical/math/Tolerance.hpp" #include namespace { - template - class TestLinearTimeInvariant : public ::testing::Test + class LinearTimeInvariantTest : public ::testing::Test { - protected: - static T Val(float f) - { - f = std::max(std::min(f, 0.9999f), -0.9999f); - if constexpr (std::is_same_v) - return f; - else - return T(f); - } - - static float ToF(T v) - { - if constexpr (std::is_same_v) - return v; - else - return v.ToFloat(); - } }; - - using TestTypes = ::testing::Types; - TYPED_TEST_SUITE(TestLinearTimeInvariant, TestTypes); } -TYPED_TEST(TestLinearTimeInvariant, default_construction_zeroes_A_B_C_D) +TEST_F(LinearTimeInvariantTest, DefaultConstructionZerosAllMatrices) { - math::LinearTimeInvariant lti; - - for (std::size_t r = 0; r < 2; ++r) - for (std::size_t c = 0; c < 2; ++c) - EXPECT_NEAR(this->ToF(lti.A.at(r, c)), 0.0f, 1e-5f); - - for (std::size_t r = 0; r < 2; ++r) - EXPECT_NEAR(this->ToF(lti.B.at(r, 0)), 0.0f, 1e-5f); + math::LinearTimeInvariant lti; + + EXPECT_NEAR(lti.A.at(0, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(lti.A.at(0, 1), 0.0f, math::Tolerance()); + EXPECT_NEAR(lti.A.at(1, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(lti.A.at(1, 1), 0.0f, math::Tolerance()); + EXPECT_NEAR(lti.B.at(0, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(lti.B.at(1, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(lti.C.at(0, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(lti.C.at(0, 1), 0.0f, math::Tolerance()); + EXPECT_NEAR(lti.D.at(0, 0), 0.0f, math::Tolerance()); } -TYPED_TEST(TestLinearTimeInvariant, step_returns_A_x_plus_B_u) +TEST_F(LinearTimeInvariantTest, StepReturnsAxPlusBu) { - math::LinearTimeInvariant lti; - lti.A = math::SquareMatrix{ - { this->Val(0.9999f), this->Val(0.5f) }, - { this->Val(0.0f), this->Val(0.9f) } + math::LinearTimeInvariant lti; + lti.A = math::SquareMatrix{ + { 0.9f, 0.5f }, + { 0.0f, 0.9f } }; - lti.B = math::Matrix{ - { this->Val(0.0f) }, - { this->Val(0.1f) } + lti.B = math::Matrix{ + { 0.0f }, + { 0.1f } }; - math::Vector x{ { this->Val(0.5f) }, { this->Val(0.25f) } }; - math::Vector u{ { this->Val(0.1f) } }; + math::Vector x{ { 0.5f }, { 0.25f } }; + math::Vector u{ { 0.1f } }; auto xNext = lti.Step(x, u); - // x_next[0] = ~1.0*0.5 + 0.5*0.25 + 0.0*0.1 ~= 0.625 - // x_next[1] = 0.0*0.5 + 0.9*0.25 + 0.1*0.1 = 0.235 - EXPECT_NEAR(this->ToF(xNext.at(0, 0)), 0.625f, 1e-2f); - EXPECT_NEAR(this->ToF(xNext.at(1, 0)), 0.235f, 1e-2f); + EXPECT_NEAR(xNext.at(0, 0), 0.9f * 0.5f + 0.5f * 0.25f, math::Tolerance()); + EXPECT_NEAR(xNext.at(1, 0), 0.9f * 0.25f + 0.1f * 0.1f, math::Tolerance()); } -TYPED_TEST(TestLinearTimeInvariant, output_returns_C_x_plus_D_u) +TEST_F(LinearTimeInvariantTest, OutputReturnsCxPlusDu) { - math::LinearTimeInvariant lti; - lti.C = math::Matrix{ - { TypeParam(0.9f), TypeParam(0.0f) } - }; - lti.D = math::Matrix{ { TypeParam(0.0f) } }; + math::LinearTimeInvariant lti; + lti.C = math::Matrix{ { 0.9f, 0.0f } }; + lti.D = math::Matrix{ { 0.0f } }; - math::Vector x{ { TypeParam(0.5f) }, { TypeParam(0.25f) } }; - math::Vector u{ { TypeParam(0.1f) } }; + math::Vector x{ { 0.5f }, { 0.25f } }; + math::Vector u{ { 0.1f } }; auto y = lti.Output(x, u); - // y = 0.9*0.5 + 0.0*0.25 = 0.45 - EXPECT_NEAR(this->ToF(y.at(0, 0)), 0.45f, 5e-3f); + EXPECT_NEAR(y.at(0, 0), 0.9f * 0.5f, math::Tolerance()); } -TYPED_TEST(TestLinearTimeInvariant, with_full_state_output_sets_C_to_identity_and_D_to_zero) +TEST_F(LinearTimeInvariantTest, WithFullStateOutputSetsCIdentityAndDZero) { - auto A = math::SquareMatrix{ - { TypeParam(0.9f), TypeParam(0.1f) }, - { TypeParam(0.0f), TypeParam(0.8f) } + auto A = math::SquareMatrix{ + { 0.9f, 0.1f }, + { 0.0f, 0.8f } }; - auto B = math::Matrix{ - { TypeParam(0.0f) }, - { TypeParam(0.1f) } + auto B = math::Matrix{ + { 0.0f }, + { 0.1f } }; - auto lti = math::LinearTimeInvariant::WithFullStateOutput(A, B); - - // C should be identity - EXPECT_NEAR(this->ToF(lti.C.at(0, 0)), 1.0f, 5e-3f); - EXPECT_NEAR(this->ToF(lti.C.at(1, 1)), 1.0f, 5e-3f); - EXPECT_NEAR(this->ToF(lti.C.at(0, 1)), 0.0f, 1e-5f); - EXPECT_NEAR(this->ToF(lti.C.at(1, 0)), 0.0f, 1e-5f); + auto lti = math::LinearTimeInvariant::WithFullStateOutput(A, B); - // D should be zero - EXPECT_NEAR(this->ToF(lti.D.at(0, 0)), 0.0f, 1e-5f); + EXPECT_NEAR(lti.C.at(0, 0), 1.0f, math::Tolerance()); + EXPECT_NEAR(lti.C.at(1, 1), 1.0f, math::Tolerance()); + EXPECT_NEAR(lti.C.at(0, 1), 0.0f, math::Tolerance()); + EXPECT_NEAR(lti.C.at(1, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(lti.D.at(0, 0), 0.0f, math::Tolerance()); } -TYPED_TEST(TestLinearTimeInvariant, autonomous_factory_zeroes_B) +TEST_F(LinearTimeInvariantTest, AutonomousFactoryZerosB) { - auto A = math::SquareMatrix{ - { TypeParam(0.9f), TypeParam(0.0f) }, - { TypeParam(0.0f), TypeParam(0.8f) } - }; - auto C = math::Matrix{ - { TypeParam(0.9f), TypeParam(0.0f) } + auto A = math::SquareMatrix{ + { 0.9f, 0.0f }, + { 0.0f, 0.8f } }; + auto C = math::Matrix{ { 0.9f, 0.0f } }; - auto lti = math::LinearTimeInvariant::Autonomous(A, C); + auto lti = math::LinearTimeInvariant::Autonomous(A, C); - for (std::size_t r = 0; r < 2; ++r) - EXPECT_NEAR(this->ToF(lti.B.at(r, 0)), 0.0f, 1e-5f); + EXPECT_NEAR(lti.B.at(0, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(lti.B.at(1, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(lti.D.at(0, 0), 0.0f, math::Tolerance()); } -TYPED_TEST(TestLinearTimeInvariant, step_called_twice_propagates_state_correctly) +TEST_F(LinearTimeInvariantTest, MultiStepPropagatesStateCorrectly) { - auto lti = math::LinearTimeInvariant::WithFullStateOutput( - math::SquareMatrix{ - { this->Val(0.9999f), this->Val(0.1f) }, - { this->Val(0.0f), this->Val(0.9f) } }, - math::Matrix{ - { this->Val(0.0f) }, - { this->Val(0.1f) } }); - - math::Vector x{ { TypeParam(0.0f) }, { TypeParam(0.0f) } }; - math::Vector u{ { TypeParam(0.5f) } }; + auto lti = math::LinearTimeInvariant::WithFullStateOutput( + math::SquareMatrix{ + { 1.0f, 0.1f }, + { 0.0f, 0.9f } + }, + math::Matrix{ + { 0.0f }, + { 0.1f } + }); + + math::Vector x{ { 0.0f }, { 0.0f } }; + math::Vector u{ { 0.5f } }; x = lti.Step(x, u); x = lti.Step(x, u); - // After step 1: x = [0, 0.05] - // After step 2: x = [0.005, 0.095] - EXPECT_NEAR(this->ToF(x.at(0, 0)), 0.005f, 5e-3f); - EXPECT_NEAR(this->ToF(x.at(1, 0)), 0.095f, 5e-3f); -} - -namespace -{ - class TestLinearTimeInvariantFloat : public ::testing::Test - {}; + EXPECT_NEAR(x.at(0, 0), 0.0f * 1.0f + 0.05f * 0.1f, math::Tolerance()); + EXPECT_NEAR(x.at(1, 0), 0.9f * 0.05f + 0.1f * 0.5f, math::Tolerance()); } -TEST_F(TestLinearTimeInvariantFloat, three_state_system_step_and_output) +TEST_F(LinearTimeInvariantTest, ThreeStateStepAndOutput) { - auto A = math::SquareMatrix{ - { 0.9f, 0.1f, 0.0f }, - { 0.0f, 0.8f, 0.1f }, - { 0.0f, 0.0f, 0.7f } - }; - auto B = math::Matrix{ { 0.0f }, { 0.0f }, { 0.1f } }; - - auto lti = math::LinearTimeInvariant::WithFullStateOutput(A, B); + auto lti = math::LinearTimeInvariant::WithFullStateOutput( + math::SquareMatrix{ + { 0.9f, 0.1f, 0.0f }, + { 0.0f, 0.8f, 0.1f }, + { 0.0f, 0.0f, 0.7f } + }, + math::Matrix{ { 0.0f }, { 0.0f }, { 0.1f } }); math::Vector x{ { 1.0f }, { 0.5f }, { 0.25f } }; math::Vector u{ { 2.0f } }; auto xNext = lti.Step(x, u); - EXPECT_NEAR(xNext.at(0, 0), 0.9f * 1.0f + 0.1f * 0.5f, 1e-5f); - EXPECT_NEAR(xNext.at(1, 0), 0.8f * 0.5f + 0.1f * 0.25f, 1e-5f); - EXPECT_NEAR(xNext.at(2, 0), 0.7f * 0.25f + 0.1f * 2.0f, 1e-5f); + EXPECT_NEAR(xNext.at(0, 0), 0.9f * 1.0f + 0.1f * 0.5f, math::Tolerance()); + EXPECT_NEAR(xNext.at(1, 0), 0.8f * 0.5f + 0.1f * 0.25f, math::Tolerance()); + EXPECT_NEAR(xNext.at(2, 0), 0.7f * 0.25f + 0.1f * 2.0f, math::Tolerance()); - // C = I, D = 0, so Output(x, u) == x auto y = lti.Output(x, u); - EXPECT_NEAR(y.at(0, 0), x.at(0, 0), 5e-3f); - EXPECT_NEAR(y.at(1, 0), x.at(1, 0), 5e-3f); - EXPECT_NEAR(y.at(2, 0), x.at(2, 0), 5e-3f); -} - -TEST_F(TestLinearTimeInvariantFloat, four_state_system_step) -{ - auto lti = math::LinearTimeInvariant::WithFullStateOutput( - math::SquareMatrix{ - { 0.9f, 0.1f, 0.0f, 0.0f }, - { 0.0f, 0.8f, 0.1f, 0.0f }, - { 0.0f, 0.0f, 0.7f, 0.1f }, - { 0.0f, 0.0f, 0.0f, 0.6f } }, - math::Matrix{ { 0.0f }, { 0.0f }, { 0.0f }, { 0.1f } }); - - math::Vector x{ { 1.0f }, { 0.0f }, { 0.0f }, { 0.0f } }; - math::Vector u{ { 1.0f } }; - - auto xNext = lti.Step(x, u); - EXPECT_NEAR(xNext.at(0, 0), 0.9f, 1e-5f); - EXPECT_NEAR(xNext.at(3, 0), 0.1f, 1e-5f); + EXPECT_NEAR(y.at(0, 0), x.at(0, 0), math::Tolerance()); + EXPECT_NEAR(y.at(1, 0), x.at(1, 0), math::Tolerance()); + EXPECT_NEAR(y.at(2, 0), x.at(2, 0), math::Tolerance()); } -TEST_F(TestLinearTimeInvariantFloat, four_state_single_output_autonomous) +TEST_F(LinearTimeInvariantTest, FourStateAutonomousStep) { auto A = math::SquareMatrix{ { 0.9f, 0.0f, 0.0f, 0.0f }, @@ -204,38 +152,32 @@ TEST_F(TestLinearTimeInvariantFloat, four_state_single_output_autonomous) auto lti = math::LinearTimeInvariant::Autonomous(A, C); - for (std::size_t r = 0; r < 4; ++r) - EXPECT_NEAR(lti.B.at(r, 0), 0.0f, 1e-5f); - math::Vector x{ { 1.0f }, { 0.5f }, { 0.25f }, { 0.1f } }; math::Vector u{}; + auto xNext = lti.Step(x, u); - EXPECT_NEAR(xNext.at(0, 0), 0.9f, 1e-5f); + EXPECT_NEAR(xNext.at(0, 0), 0.9f, math::Tolerance()); + EXPECT_NEAR(xNext.at(1, 0), 0.8f * 0.5f, math::Tolerance()); + EXPECT_NEAR(xNext.at(2, 0), 0.7f * 0.25f, math::Tolerance()); + EXPECT_NEAR(xNext.at(3, 0), 0.6f * 0.1f, math::Tolerance()); } -TEST_F(TestLinearTimeInvariantFloat, two_state_two_input_with_full_state_output) +TEST_F(LinearTimeInvariantTest, TwoStateMultiInputStep) { - auto A = math::SquareMatrix{ - { 0.9f, 0.0f }, - { 0.0f, 0.8f } - }; - auto B = math::Matrix{ - { 0.1f, 0.0f }, - { 0.0f, 0.1f } - }; - - auto lti = math::LinearTimeInvariant::WithFullStateOutput(A, B); - - // C must be identity - EXPECT_NEAR(lti.C.at(0, 0), 1.0f, 5e-3f); - EXPECT_NEAR(lti.C.at(1, 1), 1.0f, 5e-3f); - EXPECT_NEAR(lti.C.at(0, 1), 0.0f, 5e-3f); - EXPECT_NEAR(lti.C.at(1, 0), 0.0f, 5e-3f); + auto lti = math::LinearTimeInvariant::WithFullStateOutput( + math::SquareMatrix{ + { 0.9f, 0.0f }, + { 0.0f, 0.8f } + }, + math::Matrix{ + { 0.1f, 0.0f }, + { 0.0f, 0.1f } + }); math::Vector x{ { 1.0f }, { 0.5f } }; math::Vector u{ { 1.0f }, { 2.0f } }; auto xNext = lti.Step(x, u); - EXPECT_NEAR(xNext.at(0, 0), 0.9f + 0.1f, 1e-5f); - EXPECT_NEAR(xNext.at(1, 0), 0.4f + 0.2f, 1e-5f); + EXPECT_NEAR(xNext.at(0, 0), 0.9f * 1.0f + 0.1f * 1.0f, math::Tolerance()); + EXPECT_NEAR(xNext.at(1, 0), 0.8f * 0.5f + 0.1f * 2.0f, math::Tolerance()); } diff --git a/numerical/math/test/TestMatrix.cpp b/numerical/math/test/TestMatrix.cpp index 0fc7fe89..71959109 100644 --- a/numerical/math/test/TestMatrix.cpp +++ b/numerical/math/test/TestMatrix.cpp @@ -1,28 +1,13 @@ #include "numerical/math/Matrix.hpp" -#include -#include +#include "numerical/math/QNumber.hpp" +#include "numerical/math/Tolerance.hpp" +#include "numerical/math/test_doubles/MatrixTestSupport.hpp" #include -#include namespace { - template - bool AreMatricesNear(const math::Matrix& a, - const math::Matrix& b, - float epsilon = 1e-4f) - { - for (size_t i = 0; i < 2; ++i) - { - for (size_t j = 0; j < 2; ++j) - { - if constexpr (std::is_same_v) - return (std::abs(a.at(i, j) - b.at(i, j)) < epsilon); - else - return (std::abs(a.at(i, j).ToFloat() - b.at(i, j).ToFloat()) < epsilon); - } - } - return true; - } + using math::test::AreMatricesNear; + using math::test::AreVectorsNear; template class MatrixTest @@ -52,166 +37,186 @@ namespace using TestTypes = ::testing::Types; TYPED_TEST_SUITE(MatrixTest, TestTypes); + + class MatrixBlockTest : public ::testing::Test + { + protected: + math::Matrix dest{}; + math::Matrix src{ + { 0.1f, 0.2f }, + { 0.3f, 0.4f } + }; + }; } -TYPED_TEST(MatrixTest, DefaultConstructor) +TYPED_TEST(MatrixTest, DefaultConstructorZeroInitializes) { typename TestFixture::MatrixType m; + for (size_t i = 0; i < 2; ++i) - { for (size_t j = 0; j < 2; ++j) - { - if constexpr (std::is_same_v) - EXPECT_FLOAT_EQ(m.at(i, j), 0.0f); - else - EXPECT_FLOAT_EQ(m.at(i, j).ToFloat(), 0.0f); - } - } + EXPECT_NEAR(math::ToFloat(m.at(i, j)), 0.0f, math::Tolerance()); } -TYPED_TEST(MatrixTest, InitializerListConstructor) +TYPED_TEST(MatrixTest, InitializerListConstructorStoresValues) { auto m = this->MakeMatrix(0.1f, 0.2f, 0.3f, 0.4f); - float expected[2][2] = { { 0.1f, 0.2f }, { 0.3f, 0.4f } }; - for (size_t i = 0; i < 2; ++i) - { - for (size_t j = 0; j < 2; ++j) - { - if constexpr (std::is_same_v) - EXPECT_NEAR(m.at(i, j), expected[i][j], 1e-4f); - else - EXPECT_NEAR(m.at(i, j).ToFloat(), expected[i][j], 1e-4f); - } - } + EXPECT_NEAR(math::ToFloat(m.at(0, 0)), 0.1f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(m.at(0, 1)), 0.2f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(m.at(1, 0)), 0.3f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(m.at(1, 1)), 0.4f, math::Tolerance()); } TYPED_TEST(MatrixTest, Addition) { auto m1 = this->MakeMatrix(0.3f, 0.2f, 0.1f, 0.2f); auto m2 = this->MakeMatrix(0.1f, 0.2f, 0.3f, 0.1f); + auto result = m1 + m2; - auto expected = this->MakeMatrix(0.4f, 0.4f, 0.4f, 0.3f); - EXPECT_TRUE(AreMatricesNear(result, expected)); + EXPECT_TRUE(AreMatricesNear(result, this->MakeMatrix(0.4f, 0.4f, 0.4f, 0.3f))); } TYPED_TEST(MatrixTest, Subtraction) { auto m1 = this->MakeMatrix(0.5f, 0.4f, 0.3f, 0.2f); auto m2 = this->MakeMatrix(0.1f, 0.2f, 0.1f, 0.1f); + auto result = m1 - m2; - auto expected = this->MakeMatrix(0.4f, 0.2f, 0.2f, 0.1f); - EXPECT_TRUE(AreMatricesNear(result, expected)); + EXPECT_TRUE(AreMatricesNear(result, this->MakeMatrix(0.4f, 0.2f, 0.2f, 0.1f))); } TYPED_TEST(MatrixTest, Multiplication) { auto m1 = this->MakeMatrix(0.5f, 0.3f, 0.2f, 0.4f); auto m2 = this->MakeMatrix(0.2f, 0.3f, 0.4f, 0.2f); + auto result = m1 * m2; - auto expected = this->MakeMatrix(0.22f, 0.21f, 0.20f, 0.14f); - EXPECT_TRUE(AreMatricesNear(result, expected)); + EXPECT_TRUE(AreMatricesNear(result, this->MakeMatrix(0.22f, 0.21f, 0.20f, 0.14f))); } TYPED_TEST(MatrixTest, ScalarMultiplication) { auto m = this->MakeMatrix(0.5f, 0.4f, 0.3f, 0.2f); auto scalar = this->MakeValue(0.5f); + auto result = m * scalar; - auto expected = this->MakeMatrix(0.25f, 0.2f, 0.15f, 0.1f); - EXPECT_TRUE(AreMatricesNear(result, expected)); + EXPECT_TRUE(AreMatricesNear(result, this->MakeMatrix(0.25f, 0.2f, 0.15f, 0.1f))); } TYPED_TEST(MatrixTest, Transpose) { auto m = this->MakeMatrix(0.1f, 0.2f, 0.3f, 0.4f); + auto result = m.Transpose(); - auto expected = this->MakeMatrix(0.1f, 0.3f, 0.2f, 0.4f); - EXPECT_TRUE(AreMatricesNear(result, expected)); + EXPECT_TRUE(AreMatricesNear(result, this->MakeMatrix(0.1f, 0.3f, 0.2f, 0.4f))); } -TYPED_TEST(MatrixTest, Identity) +TYPED_TEST(MatrixTest, TransposeOfTransposeIsIdentity) +{ + auto m = this->MakeMatrix(0.1f, 0.2f, 0.3f, 0.4f); + + auto result = m.Transpose().Transpose(); + + EXPECT_TRUE(AreMatricesNear(result, m)); +} + +TYPED_TEST(MatrixTest, IdentityDiagonalIsOneOffDiagonalIsZero) { auto identity = TestFixture::MatrixType::Identity(); - float expectedDiag = std::is_same_v ? 1.0f : 0.9999f; + constexpr float expectedDiag = std::is_floating_point_v ? 1.0f : 0.9999f; for (size_t i = 0; i < 2; ++i) - { for (size_t j = 0; j < 2; ++j) { float expected = (i == j) ? expectedDiag : 0.0f; - if constexpr (std::is_same_v) - EXPECT_NEAR(identity.at(i, j), expected, 1e-4f); - else - EXPECT_NEAR(identity.at(i, j).ToFloat(), expected, 1e-4f); + EXPECT_NEAR(math::ToFloat(identity.at(i, j)), expected, math::Tolerance()); } - } } -TYPED_TEST(MatrixTest, RangeLimits) +TYPED_TEST(MatrixTest, AdditionAssignAccumulatesInPlace) +{ + auto m1 = this->MakeMatrix(0.2f, 0.1f, 0.3f, 0.1f); + auto m2 = this->MakeMatrix(0.1f, 0.2f, 0.1f, 0.2f); + + m1 += m2; + + EXPECT_TRUE(AreMatricesNear(m1, this->MakeMatrix(0.3f, 0.3f, 0.4f, 0.3f))); +} + +TYPED_TEST(MatrixTest, SubtractionAssignDecreasesInPlace) +{ + auto m1 = this->MakeMatrix(0.4f, 0.3f, 0.5f, 0.3f); + auto m2 = this->MakeMatrix(0.1f, 0.2f, 0.1f, 0.2f); + + m1 -= m2; + + EXPECT_TRUE(AreMatricesNear(m1, this->MakeMatrix(0.3f, 0.1f, 0.4f, 0.1f))); +} + +TYPED_TEST(MatrixTest, ScalarMultiplyAssignScalesInPlace) +{ + auto m = this->MakeMatrix(0.4f, 0.2f, 0.6f, 0.2f); + auto scalar = this->MakeValue(0.5f); + + m *= scalar; + + EXPECT_TRUE(AreMatricesNear(m, this->MakeMatrix(0.2f, 0.1f, 0.3f, 0.1f))); +} + +TYPED_TEST(MatrixTest, TraceEqualsSumOfDiagonal) +{ + auto m = this->MakeMatrix(0.3f, 0.1f, 0.2f, 0.5f); + + auto trace = m.Trace(); + + EXPECT_NEAR(math::ToFloat(trace), 0.8f, math::Tolerance()); +} + +TYPED_TEST(MatrixTest, MaxRangeValuesStoredCorrectly) { auto max_matrix = this->MakeMatrix(0.9999f, 0.9999f, 0.9999f, 0.9999f); + for (size_t i = 0; i < 2; ++i) - { for (size_t j = 0; j < 2; ++j) - { - if constexpr (std::is_same_v) - EXPECT_NEAR(max_matrix.at(i, j), 0.9999f, 1e-4f); - else - EXPECT_NEAR(max_matrix.at(i, j).ToFloat(), 0.9999f, 1e-4f); - } - } + EXPECT_NEAR(math::ToFloat(max_matrix.at(i, j)), 0.9999f, math::Tolerance()); +} +TYPED_TEST(MatrixTest, MinRangeValuesStoredCorrectly) +{ auto min_matrix = this->MakeMatrix(-0.9999f, -0.9999f, -0.9999f, -0.9999f); + for (size_t i = 0; i < 2; ++i) - { for (size_t j = 0; j < 2; ++j) - { - if constexpr (std::is_same_v) - EXPECT_NEAR(min_matrix.at(i, j), -0.9999f, 1e-4f); - else - EXPECT_NEAR(min_matrix.at(i, j).ToFloat(), -0.9999f, 1e-4f); - } - } + EXPECT_NEAR(math::ToFloat(min_matrix.at(i, j)), -0.9999f, math::Tolerance()); } -TYPED_TEST(MatrixTest, MultiplicationRangeCheck) +TYPED_TEST(MatrixTest, AdditionIsCommutative) { - auto m1 = this->MakeMatrix(0.5f, 0.5f, 0.5f, 0.5f); - auto m2 = this->MakeMatrix(0.5f, 0.5f, 0.5f, 0.5f); - auto result = m1 * m2; + auto m1 = this->MakeMatrix(0.1f, 0.3f, 0.2f, 0.4f); + auto m2 = this->MakeMatrix(0.3f, 0.1f, 0.4f, 0.2f); - for (size_t i = 0; i < 2; ++i) - { - for (size_t j = 0; j < 2; ++j) - { - if constexpr (std::is_same_v) - EXPECT_LE(std::abs(result.at(i, j)), 0.9999f); - else - EXPECT_LE(std::abs(result.at(i, j).ToFloat()), 0.9999f); - } - } + EXPECT_TRUE(AreMatricesNear(m1 + m2, m2 + m1)); } -namespace +TYPED_TEST(MatrixTest, MultiplicationByZeroMatrixYieldsZero) { - class MatrixBlockTest : public ::testing::Test - { - protected: - math::Matrix dest{}; - math::Matrix src{ - { 0.1f, 0.2f }, - { 0.3f, 0.4f } - }; - }; + auto m = this->MakeMatrix(0.5f, 0.3f, 0.2f, 0.4f); + typename TestFixture::MatrixType zero{}; + + auto result = m * zero; + + for (size_t i = 0; i < 2; ++i) + for (size_t j = 0; j < 2; ++j) + EXPECT_NEAR(math::ToFloat(result.at(i, j)), 0.0f, math::Tolerance()); } -TEST_F(MatrixBlockTest, SetBlock_writes_correct_elements) +TEST_F(MatrixBlockTest, SetBlockWritesCorrectElements) { dest.SetBlock(src, 1, 1); @@ -223,7 +228,7 @@ TEST_F(MatrixBlockTest, SetBlock_writes_correct_elements) EXPECT_FLOAT_EQ(dest.at(3, 3), 0.0f); } -TEST_F(MatrixBlockTest, GetBlock_reads_correct_elements) +TEST_F(MatrixBlockTest, GetBlockReadsCorrectElements) { math::Matrix m{ { 0.0f, 0.0f, 0.0f, 0.0f }, @@ -240,7 +245,7 @@ TEST_F(MatrixBlockTest, GetBlock_reads_correct_elements) EXPECT_FLOAT_EQ(block.at(1, 1), 0.4f); } -TEST_F(MatrixBlockTest, GetColumn_reads_correct_column) +TEST_F(MatrixBlockTest, GetColumnReadsCorrectColumn) { math::Matrix m{ { 0.1f, 0.2f, 0.3f }, @@ -255,7 +260,7 @@ TEST_F(MatrixBlockTest, GetColumn_reads_correct_column) EXPECT_FLOAT_EQ(col.at(2, 0), 0.8f); } -TEST_F(MatrixBlockTest, SetBlock_then_GetBlock_roundtrip) +TEST_F(MatrixBlockTest, SetBlockThenGetBlockRoundtrip) { dest.SetBlock(src, 2, 2); auto result = dest.GetBlock<2, 2>(2, 2); @@ -265,3 +270,28 @@ TEST_F(MatrixBlockTest, SetBlock_then_GetBlock_roundtrip) EXPECT_FLOAT_EQ(result.at(1, 0), src.at(1, 0)); EXPECT_FLOAT_EQ(result.at(1, 1), src.at(1, 1)); } + +TEST_F(MatrixBlockTest, SetBlockDoesNotModifyUnaffectedElements) +{ + dest.SetBlock(src, 0, 0); + + EXPECT_FLOAT_EQ(dest.at(2, 2), 0.0f); + EXPECT_FLOAT_EQ(dest.at(3, 3), 0.0f); + EXPECT_FLOAT_EQ(dest.at(0, 2), 0.0f); + EXPECT_FLOAT_EQ(dest.at(2, 0), 0.0f); +} + +TEST_F(MatrixBlockTest, GetColumnFirstColumnMatchesFirstColumn) +{ + math::Matrix m{ + { 0.1f, 0.4f, 0.7f }, + { 0.2f, 0.5f, 0.8f }, + { 0.3f, 0.6f, 0.9f } + }; + + auto col = m.GetColumn(0); + + EXPECT_FLOAT_EQ(col.at(0, 0), 0.1f); + EXPECT_FLOAT_EQ(col.at(1, 0), 0.2f); + EXPECT_FLOAT_EQ(col.at(2, 0), 0.3f); +} diff --git a/numerical/math/test/TestMatrixExponential.cpp b/numerical/math/test/TestMatrixExponential.cpp index 95fbc7e9..14bdbb59 100644 --- a/numerical/math/test/TestMatrixExponential.cpp +++ b/numerical/math/test/TestMatrixExponential.cpp @@ -8,6 +8,7 @@ namespace class TestMatrixExponential : public ::testing::Test { protected: + math::MatrixExponential expm1{}; math::MatrixExponential expm2{}; math::MatrixExponential expm3{}; }; @@ -38,7 +39,6 @@ TEST_F(TestMatrixExponential, DiagonalIsElementwiseExp) TEST_F(TestMatrixExponential, Scalar1x1MatchesExp) { - math::MatrixExponential expm1{}; math::SquareMatrix a{}; a.at(0, 0) = 2.5f; auto result = expm1.Compute(a); @@ -111,3 +111,79 @@ TEST_F(TestMatrixExponential, ExpZeroDtIsIdentity) EXPECT_NEAR(result.at(1, 0), 0.0f, math::Tolerance()); EXPECT_NEAR(result.at(1, 1), 1.0f, math::Tolerance()); } + +TEST_F(TestMatrixExponential, GroupLawExpAExpNegAIsIdentity) +{ + math::SquareMatrix a{ + { 1.0f, 2.0f }, + { 0.0f, -1.0f } + }; + math::SquareMatrix neg{ + { -1.0f, -2.0f }, + { 0.0f, 1.0f } + }; + auto expA = expm2.Compute(a); + auto expNegA = expm2.Compute(neg); + auto product = expA * expNegA; + EXPECT_NEAR(product.at(0, 0), 1.0f, math::Tolerance()); + EXPECT_NEAR(product.at(0, 1), 0.0f, math::Tolerance()); + EXPECT_NEAR(product.at(1, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(product.at(1, 1), 1.0f, math::Tolerance()); +} + +TEST_F(TestMatrixExponential, DeterminantIdentityJacobiFormula) +{ + math::SquareMatrix a{ + { 2.0f, 1.0f }, + { 0.0f, 3.0f } + }; + auto result = expm2.Compute(a); + const float det = result.at(0, 0) * result.at(1, 1) - result.at(0, 1) * result.at(1, 0); + const float trace = a.at(0, 0) + a.at(1, 1); + EXPECT_NEAR(det, std::exp(trace), math::Tolerance()); +} + +TEST_F(TestMatrixExponential, ThreeByThreeDiagonalIsElementwiseExp) +{ + math::SquareMatrix a{ + { 1.0f, 0.0f, 0.0f }, + { 0.0f, -1.0f, 0.0f }, + { 0.0f, 0.0f, 2.0f } + }; + auto result = expm3.Compute(a); + EXPECT_NEAR(result.at(0, 0), std::exp(1.0f), math::Tolerance()); + EXPECT_NEAR(result.at(1, 1), std::exp(-1.0f), math::Tolerance()); + EXPECT_NEAR(result.at(2, 2), std::exp(2.0f), math::Tolerance()); + EXPECT_NEAR(result.at(0, 1), 0.0f, math::Tolerance()); + EXPECT_NEAR(result.at(0, 2), 0.0f, math::Tolerance()); + EXPECT_NEAR(result.at(1, 2), 0.0f, math::Tolerance()); +} + +TEST_F(TestMatrixExponential, ComputeWithDtScalesDiagonal) +{ + math::SquareMatrix a{ + { 2.0f, 0.0f }, + { 0.0f, -3.0f } + }; + const float dt = 0.5f; + auto result = expm2.Compute(a, dt); + EXPECT_NEAR(result.at(0, 0), std::exp(2.0f * dt), math::Tolerance()); + EXPECT_NEAR(result.at(0, 1), 0.0f, math::Tolerance()); + EXPECT_NEAR(result.at(1, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(result.at(1, 1), std::exp(-3.0f * dt), math::Tolerance()); +} + +TEST_F(TestMatrixExponential, StiffNegativeEigenvalueNearZeroNoNaN) +{ + math::SquareMatrix a{ + { -50.0f, 0.0f }, + { 0.0f, -50.0f } + }; + auto result = expm2.Compute(a); + EXPECT_FALSE(std::isnan(result.at(0, 0))); + EXPECT_FALSE(std::isnan(result.at(1, 1))); + EXPECT_FALSE(std::isinf(result.at(0, 0))); + EXPECT_FALSE(std::isinf(result.at(1, 1))); + EXPECT_NEAR(result.at(0, 1), 0.0f, math::Tolerance()); + EXPECT_NEAR(result.at(1, 0), 0.0f, math::Tolerance()); +} diff --git a/numerical/math/test/TestMatrixNorms.cpp b/numerical/math/test/TestMatrixNorms.cpp index 27b428ff..cb79eddc 100644 --- a/numerical/math/test/TestMatrixNorms.cpp +++ b/numerical/math/test/TestMatrixNorms.cpp @@ -79,3 +79,70 @@ TEST_F(MatrixNormsTest, NormalizeZeroVectorReturnsNullopt) auto result = math::Normalize(zero); EXPECT_FALSE(result.has_value()); } + +TEST_F(MatrixNormsTest, FrobeniusNormNonSquareMatrix) +{ + math::Matrix m{ + { 1.0f, 2.0f, 3.0f }, + { 4.0f, 5.0f, 6.0f } + }; + float result = math::FrobeniusNorm(m); + EXPECT_NEAR(result, 9.53939f, math::Tolerance()); +} + +TEST_F(MatrixNormsTest, FrobeniusNormZeroMatrix) +{ + math::Matrix zero{ + { 0.0f, 0.0f }, + { 0.0f, 0.0f } + }; + EXPECT_NEAR(math::FrobeniusNorm(zero), 0.0f, math::Tolerance()); +} + +TEST_F(MatrixNormsTest, OffDiagonalFrobeniusNormDiagonalMatrix) +{ + math::Matrix diag{ + { 2.0f, 0.0f, 0.0f }, + { 0.0f, 3.0f, 0.0f }, + { 0.0f, 0.0f, 5.0f } + }; + EXPECT_NEAR(math::OffDiagonalFrobeniusNorm(diag), 0.0f, math::Tolerance()); +} + +TEST_F(MatrixNormsTest, OneNormNonSquareMatrix) +{ + math::Matrix m{ + { 1.0f, 2.0f, 3.0f }, + { 4.0f, 5.0f, 6.0f } + }; + EXPECT_NEAR(math::OneNorm(m), 9.0f, math::Tolerance()); +} + +TEST_F(MatrixNormsTest, InfinityNormNonSquareMatrix) +{ + math::Matrix m{ + { 1.0f, 2.0f, 3.0f }, + { 4.0f, 5.0f, 6.0f } + }; + EXPECT_NEAR(math::InfinityNorm(m), 15.0f, math::Tolerance()); +} + +TEST_F(MatrixNormsTest, VectorNormZeroVector) +{ + math::Vector zero{ { 0.0f }, { 0.0f } }; + EXPECT_NEAR(math::VectorNorm(zero), 0.0f, math::Tolerance()); +} + +TEST_F(MatrixNormsTest, NormalizeResultHasUnitNorm) +{ + auto result = math::Normalize(v); + ASSERT_TRUE(result.has_value()); + float norm = math::VectorNorm(*result); + EXPECT_NEAR(norm, 1.0f, math::Tolerance()); +} + +TEST_F(MatrixNormsTest, DotProductCommutativity) +{ + math::Vector w{ { 1.0f }, { 2.0f } }; + EXPECT_NEAR(math::DotProduct(v, w), math::DotProduct(w, v), math::Tolerance()); +} diff --git a/numerical/math/test/TestMatrixOperations.cpp b/numerical/math/test/TestMatrixOperations.cpp index 482c7761..f4c42ea3 100644 --- a/numerical/math/test/TestMatrixOperations.cpp +++ b/numerical/math/test/TestMatrixOperations.cpp @@ -69,3 +69,95 @@ TEST_F(MatrixOperationsTest, CongruenceTransformPreservesSymmetry) EXPECT_NEAR(result.at(0, 1), result.at(1, 0), math::Tolerance()); } + +TEST_F(MatrixOperationsTest, SymmetrizeZeroMatrixYieldsZero) +{ + math::SquareMatrix zero{ + { 0.0f, 0.0f }, + { 0.0f, 0.0f } + }; + + auto result = math::Symmetrize(zero); + + for (std::size_t i = 0; i < 2; ++i) + for (std::size_t j = 0; j < 2; ++j) + EXPECT_NEAR(result.at(i, j), 0.0f, math::Tolerance()); +} + +TEST_F(MatrixOperationsTest, SymmetrizeOutputSatisfiesSymmetryInvariant) +{ + auto result = math::Symmetrize(asymmetric); + + for (std::size_t i = 0; i < 2; ++i) + for (std::size_t j = 0; j < 2; ++j) + EXPECT_NEAR(result.at(i, j), result.at(j, i), math::Tolerance()); +} + +TEST_F(MatrixOperationsTest, SymmetrizeAlreadySymmetricMatrixIsUnchanged) +{ + math::SquareMatrix sym{ + { 3.0f, -1.0f }, + { -1.0f, 5.0f } + }; + + auto result = math::Symmetrize(sym); + + for (std::size_t i = 0; i < 2; ++i) + for (std::size_t j = 0; j < 2; ++j) + EXPECT_NEAR(result.at(i, j), sym.at(i, j), math::Tolerance()); +} + +TEST_F(MatrixOperationsTest, CongruenceTransformWithIdentityEqualsAAt) +{ + math::Matrix a{ + { 1.0f, 2.0f }, + { 3.0f, 4.0f } + }; + auto identity = math::SquareMatrix::Identity(); + + auto result = math::CongruenceTransform(a, identity); + + EXPECT_NEAR(result.at(0, 0), 5.0f, math::Tolerance()); + EXPECT_NEAR(result.at(0, 1), 11.0f, math::Tolerance()); + EXPECT_NEAR(result.at(1, 0), 11.0f, math::Tolerance()); + EXPECT_NEAR(result.at(1, 1), 25.0f, math::Tolerance()); +} + +TEST_F(MatrixOperationsTest, CongruenceTransformScalesLinearlyWithScalarMatrix) +{ + math::Matrix a{ + { 1.0f, 2.0f }, + { 3.0f, 4.0f } + }; + auto identity = math::SquareMatrix::Identity(); + const float scalar = 3.0f; + math::SquareMatrix scaledIdentity{ + { scalar, 0.0f }, + { 0.0f, scalar } + }; + + auto base = math::CongruenceTransform(a, identity); + auto scaled = math::CongruenceTransform(a, scaledIdentity); + + for (std::size_t i = 0; i < 2; ++i) + for (std::size_t j = 0; j < 2; ++j) + EXPECT_NEAR(scaled.at(i, j), scalar * base.at(i, j), math::Tolerance()); +} + +TEST_F(MatrixOperationsTest, CongruenceTransformZeroMYieldsZeroResult) +{ + math::Matrix a{ + { 1.0f, 2.0f }, + { 3.0f, 4.0f } + }; + math::SquareMatrix zeroM{ + { 0.0f, 0.0f }, + { 0.0f, 0.0f } + }; + + auto result = math::CongruenceTransform(a, zeroM); + + for (std::size_t i = 0; i < 2; ++i) + for (std::size_t j = 0; j < 2; ++j) + EXPECT_NEAR(result.at(i, j), 0.0f, math::Tolerance()); +} diff --git a/numerical/math/test/TestQNumber.cpp b/numerical/math/test/TestQNumber.cpp index f25ae082..a900efd7 100644 --- a/numerical/math/test/TestQNumber.cpp +++ b/numerical/math/test/TestQNumber.cpp @@ -1,4 +1,9 @@ +// Copyright 2024 Numerical Toolbox Contributors +// SPDX-License-Identifier: MIT + #include "numerical/math/QNumber.hpp" +#include "numerical/math/Tolerance.hpp" +#include #include namespace @@ -6,181 +11,309 @@ namespace template class QNumberTest : public ::testing::Test - { - protected: - using IntType = typename std::decay().RawValue())>::type; - }; + {}; using QNumberTypes = ::testing::Types; TYPED_TEST_SUITE(QNumberTest, QNumberTypes); } -TYPED_TEST(QNumberTest, DefaultConstructor) +TYPED_TEST(QNumberTest, DefaultConstructorIsZero) { TypeParam num; + EXPECT_EQ(num.RawValue(), 0); EXPECT_FLOAT_EQ(num.ToFloat(), 0.0f); } -TYPED_TEST(QNumberTest, FloatConstructor) +TYPED_TEST(QNumberTest, FloatConstructorPositiveHalf) +{ + TypeParam num(0.5f); + + EXPECT_NEAR(num.ToFloat(), 0.5f, math::Tolerance()); +} + +TYPED_TEST(QNumberTest, FloatConstructorNegativeHalf) +{ + TypeParam num(-0.5f); + + EXPECT_NEAR(num.ToFloat(), -0.5f, math::Tolerance()); +} + +TYPED_TEST(QNumberTest, FloatConstructorQuarter) +{ + TypeParam num(0.25f); + + EXPECT_NEAR(num.ToFloat(), 0.25f, math::Tolerance()); +} + +TYPED_TEST(QNumberTest, FloatConstructorNegativeOne) { - struct TestCase - { - float input; - float expected; - } testCases[] = { - { -1.0f, -1.0f }, - { 0.5f, 0.5f }, - { -0.5f, -0.5f }, - { 0.25f, 0.25f } - }; + TypeParam num(-1.0f); - for (const auto& testCase : testCases) - { - TypeParam num(testCase.input); - EXPECT_NEAR(num.ToFloat(), testCase.expected, 1e-4f) - << "Failed for input: " << testCase.input; - } + EXPECT_NEAR(num.ToFloat(), -1.0f, math::Tolerance()); } -TYPED_TEST(QNumberTest, RawValueConstructor) +TYPED_TEST(QNumberTest, RawValueConstructorPreservesRawValue) { using IntType = typename std::decay().RawValue())>::type; - IntType rawValue = 1024; - TypeParam num(static_cast(rawValue)); + IntType rawValue{ 1024 }; + TypeParam num(rawValue); + EXPECT_EQ(num.RawValue(), rawValue); } +TYPED_TEST(QNumberTest, QuantizationErrorBoundedByHalfUlp) +{ + using IntType = typename std::decay().RawValue())>::type; + constexpr int fractionalBits = (sizeof(IntType) == 4) ? 31 : 15; + const float halfUlp = 1.0f / static_cast(1LL << fractionalBits); + TypeParam num(0.3f); + + float quantizationError = std::abs(num.ToFloat() - 0.3f); + + EXPECT_LE(quantizationError, halfUlp); +} + TYPED_TEST(QNumberTest, Addition) { TypeParam a(0.15f); TypeParam b(0.25f); + TypeParam result = a + b; - EXPECT_NEAR(result.ToFloat(), 0.40f, 1e-4f); + + EXPECT_NEAR(result.ToFloat(), 0.40f, math::Tolerance()); } TYPED_TEST(QNumberTest, Subtraction) { TypeParam a(0.50f); TypeParam b(0.20f); + TypeParam result = a - b; - EXPECT_NEAR(result.ToFloat(), 0.30f, 1e-4f); + + EXPECT_NEAR(result.ToFloat(), 0.30f, math::Tolerance()); } TYPED_TEST(QNumberTest, Multiplication) { TypeParam a(0.20f); TypeParam b(0.30f); + TypeParam result = a * b; - EXPECT_NEAR(result.ToFloat(), 0.06f, 1e-4f); + + EXPECT_NEAR(result.ToFloat(), 0.06f, math::Tolerance()); } TYPED_TEST(QNumberTest, Division) { TypeParam a(0.20f); TypeParam b(0.40f); + TypeParam result = a / b; - EXPECT_NEAR(result.ToFloat(), 0.50f, 1e-4f); + + EXPECT_NEAR(result.ToFloat(), 0.50f, math::Tolerance()); } TYPED_TEST(QNumberTest, CompoundAddition) { TypeParam a(0.15f); TypeParam b(0.25f); + a += b; - EXPECT_NEAR(a.ToFloat(), 0.40f, 1e-4f); + + EXPECT_NEAR(a.ToFloat(), 0.40f, math::Tolerance()); } TYPED_TEST(QNumberTest, CompoundSubtraction) { TypeParam a(0.50f); TypeParam b(0.20f); + a -= b; - EXPECT_NEAR(a.ToFloat(), 0.30f, 1e-4f); + + EXPECT_NEAR(a.ToFloat(), 0.30f, math::Tolerance()); } TYPED_TEST(QNumberTest, CompoundMultiplication) { TypeParam a(0.20f); TypeParam b(0.30f); + a *= b; - EXPECT_NEAR(a.ToFloat(), 0.06f, 1e-4f); + + EXPECT_NEAR(a.ToFloat(), 0.06f, math::Tolerance()); } TYPED_TEST(QNumberTest, CompoundDivision) { TypeParam a(0.20f); TypeParam b(0.40f); + a /= b; - EXPECT_NEAR(a.ToFloat(), 0.50f, 1e-4f); + + EXPECT_NEAR(a.ToFloat(), 0.50f, math::Tolerance()); } TYPED_TEST(QNumberTest, UnaryPlus) { TypeParam a(0.15f); + TypeParam result = +a; - EXPECT_NEAR(result.ToFloat(), 0.15f, 1e-4f); + + EXPECT_NEAR(result.ToFloat(), 0.15f, math::Tolerance()); } TYPED_TEST(QNumberTest, UnaryNegation) { TypeParam a(0.15f); + TypeParam result = -a; - EXPECT_NEAR(result.ToFloat(), -0.15f, 1e-4f); + + EXPECT_NEAR(result.ToFloat(), -0.15f, math::Tolerance()); +} + +TYPED_TEST(QNumberTest, AdditiveIdentity) +{ + TypeParam a(0.25f); + TypeParam zero; + + TypeParam result = a + zero; + + EXPECT_NEAR(result.ToFloat(), 0.25f, math::Tolerance()); +} + +TYPED_TEST(QNumberTest, AdditiveInverse) +{ + TypeParam a(0.25f); + TypeParam negA = -a; + + TypeParam result = a + negA; + + EXPECT_NEAR(result.ToFloat(), 0.0f, math::Tolerance()); +} + +TYPED_TEST(QNumberTest, MultiplicationByZeroIsZero) +{ + TypeParam a(0.25f); + TypeParam zero; + + TypeParam result = a * zero; + + EXPECT_NEAR(result.ToFloat(), 0.0f, math::Tolerance()); +} + +TYPED_TEST(QNumberTest, ZeroDividedByNonzeroIsZero) +{ + TypeParam zero; + TypeParam b(0.25f); + + TypeParam result = zero / b; + + EXPECT_NEAR(result.ToFloat(), 0.0f, math::Tolerance()); } -TYPED_TEST(QNumberTest, EqualityComparison) +TYPED_TEST(QNumberTest, EqualityComparisonEqual) { TypeParam a(0.15f); TypeParam b(0.15f); - TypeParam c(0.20f); EXPECT_TRUE(a == b); +} + +TYPED_TEST(QNumberTest, EqualityComparisonNotEqual) +{ + TypeParam a(0.15f); + TypeParam c(0.20f); + EXPECT_FALSE(a == c); } -TYPED_TEST(QNumberTest, LessThanComparison) +TYPED_TEST(QNumberTest, LessThanComparisonTrue) { TypeParam a(0.15f); TypeParam b(0.20f); EXPECT_TRUE(a < b); - EXPECT_FALSE(b < a); } -TYPED_TEST(QNumberTest, GreaterThanComparison) +TYPED_TEST(QNumberTest, LessThanComparisonFalse) +{ + TypeParam a(0.20f); + TypeParam b(0.15f); + + EXPECT_FALSE(a < b); +} + +TYPED_TEST(QNumberTest, GreaterThanComparisonTrue) { TypeParam a(0.20f); TypeParam b(0.15f); EXPECT_TRUE(a > b); - EXPECT_FALSE(b > a); } -TYPED_TEST(QNumberTest, FromDuration) +TYPED_TEST(QNumberTest, GreaterThanComparisonFalse) +{ + TypeParam a(0.15f); + TypeParam b(0.20f); + + EXPECT_FALSE(a > b); +} + +TYPED_TEST(QNumberTest, LessThanOrEqualComparisonLess) +{ + TypeParam a(0.15f); + TypeParam b(0.20f); + + EXPECT_TRUE(a <= b); +} + +TYPED_TEST(QNumberTest, LessThanOrEqualComparisonEqual) +{ + TypeParam a(0.15f); + TypeParam b(0.15f); + + EXPECT_TRUE(a <= b); +} + +TYPED_TEST(QNumberTest, GreaterThanOrEqualComparisonGreater) +{ + TypeParam a(0.20f); + TypeParam b(0.15f); + + EXPECT_TRUE(a >= b); +} + +TYPED_TEST(QNumberTest, GreaterThanOrEqualComparisonEqual) +{ + TypeParam a(0.20f); + TypeParam b(0.20f); + + EXPECT_TRUE(a >= b); +} + +TYPED_TEST(QNumberTest, FromDurationStoresRawCount) { auto duration = std::chrono::microseconds(1000); + TypeParam num = TypeParam::FromDuration(duration); + EXPECT_EQ(num.RawValue(), 1000); } -TYPED_TEST(QNumberTest, FixedPointInteraction) +TYPED_TEST(QNumberTest, RawValueRoundtripPreservesValue) { TypeParam a(0.15f); - auto rawVal = a.RawValue(); TypeParam b(rawVal); EXPECT_EQ(a, b); } -TYPED_TEST(QNumberTest, DivideByZeroHandling) +TYPED_TEST(QNumberTest, DivideByZeroDies) { TypeParam a(0.10f); TypeParam zero(0.0f); - EXPECT_DEATH({ - TypeParam result = a / zero; - }, - ""); + EXPECT_DEATH({ TypeParam result = a / zero; }, ""); // NOLINT } diff --git a/numerical/math/test/TestQuaternion.cpp b/numerical/math/test/TestQuaternion.cpp index b37a2524..b96871df 100644 --- a/numerical/math/test/TestQuaternion.cpp +++ b/numerical/math/test/TestQuaternion.cpp @@ -274,3 +274,120 @@ TEST_F(TestQuaternion, ToEulerGimbalLockNegativePitch) auto euler = q.ToEulerZYX(); EXPECT_NEAR(euler.at(1, 0), -std::numbers::pi_v / 2.0f, math::Tolerance()); } + +TEST_F(TestQuaternion, NormPreservedUnderProduct) +{ + auto q1 = math::Quaternion::FromAxisAngle( + math::Vector3{ { 1.0f }, { 0.0f }, { 0.0f } }, 0.6f); + auto q2 = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 1.0f }, { 0.0f } }, 1.1f); + auto product = q1 * q2; + EXPECT_NEAR(product.Norm(), q1.Norm() * q2.Norm(), math::Tolerance()); +} + +TEST_F(TestQuaternion, FromAxisAngleZeroAxisNoNaN) +{ + math::Vector3 zeroAxis{ { 0.0f }, { 0.0f }, { 0.0f } }; + auto q = math::Quaternion::FromAxisAngle(zeroAxis, 1.0f); + EXPECT_FALSE(std::isnan(q.w)); + EXPECT_FALSE(std::isnan(q.x)); + EXPECT_FALSE(std::isnan(q.y)); + EXPECT_FALSE(std::isnan(q.z)); +} + +TEST_F(TestQuaternion, InverseOfZeroQuaternionIsZero) +{ + math::Quaternion zero{ 0.0f, 0.0f, 0.0f, 0.0f }; + auto inv = zero.Inverse(); + EXPECT_NEAR(inv.w, 0.0f, math::Tolerance()); + EXPECT_NEAR(inv.x, 0.0f, math::Tolerance()); + EXPECT_NEAR(inv.y, 0.0f, math::Tolerance()); + EXPECT_NEAR(inv.z, 0.0f, math::Tolerance()); +} + +TEST_F(TestQuaternion, NormalizeZeroQuaternionNoNaN) +{ + math::Quaternion zero{ 0.0f, 0.0f, 0.0f, 0.0f }; + zero.Normalize(); + EXPECT_FALSE(std::isnan(zero.w)); + EXPECT_FALSE(std::isnan(zero.x)); + EXPECT_FALSE(std::isnan(zero.y)); + EXPECT_FALSE(std::isnan(zero.z)); +} + +TEST_F(TestQuaternion, RotatePreservesVectorNorm) +{ + auto q = math::Quaternion::FromAxisAngle( + math::Vector3{ { 1.0f }, { 1.0f }, { 1.0f } }, 1.3f); + q.Normalize(); + math::Vector3 v{ { 3.0f }, { -2.0f }, { 1.0f } }; + auto rv = q.Rotate(v); + float vNorm = std::sqrt(v.at(0, 0) * v.at(0, 0) + v.at(1, 0) * v.at(1, 0) + v.at(2, 0) * v.at(2, 0)); + float rvNorm = std::sqrt(rv.at(0, 0) * rv.at(0, 0) + rv.at(1, 0) * rv.at(1, 0) + rv.at(2, 0) * rv.at(2, 0)); + EXPECT_NEAR(rvNorm, vNorm, math::Tolerance()); +} + +TEST_F(TestQuaternion, ToRotationMatrixIdentityGivesIdentityMatrix) +{ + auto r = identity.ToRotationMatrix(); + EXPECT_NEAR(r.at(0, 0), 1.0f, math::Tolerance()); + EXPECT_NEAR(r.at(1, 1), 1.0f, math::Tolerance()); + EXPECT_NEAR(r.at(2, 2), 1.0f, math::Tolerance()); + EXPECT_NEAR(r.at(0, 1), 0.0f, math::Tolerance()); + EXPECT_NEAR(r.at(0, 2), 0.0f, math::Tolerance()); + EXPECT_NEAR(r.at(1, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(r.at(1, 2), 0.0f, math::Tolerance()); + EXPECT_NEAR(r.at(2, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(r.at(2, 1), 0.0f, math::Tolerance()); +} + +TEST_F(TestQuaternion, RotationCompositionMatchesSequential) +{ + auto q1 = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 0.0f }, { 1.0f } }, + std::numbers::pi_v / 4.0f); + auto q2 = math::Quaternion::FromAxisAngle( + math::Vector3{ { 1.0f }, { 0.0f }, { 0.0f } }, + std::numbers::pi_v / 6.0f); + math::Vector3 v{ { 1.0f }, { 0.0f }, { 0.0f } }; + auto composed = (q1 * q2).Rotate(v); + auto sequential = q1.Rotate(q2.Rotate(v)); + EXPECT_NEAR(composed.at(0, 0), sequential.at(0, 0), math::Tolerance()); + EXPECT_NEAR(composed.at(1, 0), sequential.at(1, 0), math::Tolerance()); + EXPECT_NEAR(composed.at(2, 0), sequential.at(2, 0), math::Tolerance()); +} + +TEST_F(TestQuaternion, FromEulerZYXPureYawMatchesClosedForm) +{ + float yaw{ std::numbers::pi_v / 3.0f }; + auto q = math::Quaternion::FromEulerZYX(0.0f, 0.0f, yaw); + float halfYaw{ yaw * 0.5f }; + EXPECT_NEAR(q.w, std::cos(halfYaw), math::Tolerance()); + EXPECT_NEAR(q.x, 0.0f, math::Tolerance()); + EXPECT_NEAR(q.y, 0.0f, math::Tolerance()); + EXPECT_NEAR(q.z, std::sin(halfYaw), math::Tolerance()); +} + +TEST_F(TestQuaternion, SlerpOutputNormIsUnity) +{ + auto a = math::Quaternion::FromAxisAngle( + math::Vector3{ { 1.0f }, { 0.0f }, { 0.0f } }, 0.4f); + auto b = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 1.0f }, { 0.0f } }, 1.2f); + std::array ts{ 0.0f, 0.25f, 0.5f, 0.75f, 1.0f }; + for (float t : ts) + { + auto s = math::Quaternion::Slerp(a, b, t); + EXPECT_NEAR(s.Norm(), 1.0f, math::Tolerance()); + } +} + +TEST_F(TestQuaternion, ConjugateNegatesVectorPartOnly) +{ + math::Quaternion q{ 0.5f, 0.3f, -0.7f, 0.1f }; + auto c = q.Conjugate(); + EXPECT_NEAR(c.w, 0.5f, math::Tolerance()); + EXPECT_NEAR(c.x, -0.3f, math::Tolerance()); + EXPECT_NEAR(c.y, 0.7f, math::Tolerance()); + EXPECT_NEAR(c.z, -0.1f, math::Tolerance()); +} diff --git a/numerical/math/test/TestRecursiveBuffer.cpp b/numerical/math/test/TestRecursiveBuffer.cpp index 4a6ba135..8e9a3d26 100644 --- a/numerical/math/test/TestRecursiveBuffer.cpp +++ b/numerical/math/test/TestRecursiveBuffer.cpp @@ -1,5 +1,6 @@ #include "numerical/math/QNumber.hpp" #include "numerical/math/RecursiveBuffer.hpp" +#include "numerical/math/Tolerance.hpp" #include #include @@ -9,37 +10,51 @@ namespace class RecursiveBufferTest : public ::testing::Test { - public: + protected: static constexpr std::size_t BufferSize = 4; math::RecursiveBuffer buffer; - const float tolerance = 0.001f; + + void SetUp() override + { + buffer.Reset(); + } }; using BufferTypes = ::testing::Types; TYPED_TEST_SUITE(RecursiveBufferTest, BufferTypes); } -TYPED_TEST(RecursiveBufferTest, DefaultConstructor) +TYPED_TEST(RecursiveBufferTest, DefaultInitializationAllSlotsZero) { math::Index n; + for (std::size_t i = 0; i < this->BufferSize; ++i) - EXPECT_NEAR(math::ToFloat(this->buffer[n - i]), 0.0f, this->tolerance) - << "Buffer not zero-initialized at index " << i; + EXPECT_NEAR(math::ToFloat(this->buffer[n - i]), 0.0f, math::Tolerance()); } -TYPED_TEST(RecursiveBufferTest, SingleUpdate) +TYPED_TEST(RecursiveBufferTest, SizeReturnsTemplateLength) +{ + EXPECT_EQ(this->buffer.Size(), this->BufferSize); +} + +TYPED_TEST(RecursiveBufferTest, SingleUpdatePlacesValueAtCurrentIndex) { math::Index n; - TypeParam value(0.5f); - this->buffer.Update(value); + this->buffer.Update(TypeParam(0.5f)); + + EXPECT_NEAR(math::ToFloat(this->buffer[+n]), 0.5f, math::Tolerance()); +} + +TYPED_TEST(RecursiveBufferTest, SingleUpdateLeavesOlderSlotsZero) +{ + math::Index n; + this->buffer.Update(TypeParam(0.5f)); - EXPECT_NEAR(math::ToFloat(this->buffer[n - 0]), 0.5f, this->tolerance); for (std::size_t i = 1; i < this->BufferSize; ++i) - EXPECT_NEAR(math::ToFloat(this->buffer[n - i]), 0.0f, this->tolerance) - << "Non-zero value found at index " << i; + EXPECT_NEAR(math::ToFloat(this->buffer[n - i]), 0.0f, math::Tolerance()); } -TYPED_TEST(RecursiveBufferTest, MultipleUpdates) +TYPED_TEST(RecursiveBufferTest, MultipleUpdatesShiftValuesCorrectly) { math::Index n; std::array values = { 0.1f, 0.2f, 0.3f, 0.4f }; @@ -47,25 +62,103 @@ TYPED_TEST(RecursiveBufferTest, MultipleUpdates) for (float val : values) this->buffer.Update(TypeParam(val)); - for (size_t i = 0; i < values.size(); ++i) + for (std::size_t i = 0; i < values.size(); ++i) EXPECT_NEAR( math::ToFloat(this->buffer[n - i]), - values[values.size() - 1 - i], this->tolerance) - << "Incorrect value at index " << i; + values[values.size() - 1 - i], + math::Tolerance()); } -TYPED_TEST(RecursiveBufferTest, ShiftingBehavior) +TYPED_TEST(RecursiveBufferTest, OverflowDropsOldestValue) { math::Index n; for (std::size_t i = 1; i <= this->BufferSize; ++i) this->buffer.Update(TypeParam(static_cast(i) * 0.1f)); - TypeParam newValue(0.5f); - this->buffer.Update(newValue); + this->buffer.Update(TypeParam(0.5f)); + + EXPECT_NEAR(math::ToFloat(this->buffer[n - 0]), 0.5f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(this->buffer[n - 1]), 0.4f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(this->buffer[n - 2]), 0.3f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(this->buffer[n - 3]), 0.2f, math::Tolerance()); +} + +TYPED_TEST(RecursiveBufferTest, ResetAfterUpdateClearsAllSlots) +{ + math::Index n; + this->buffer.Update(TypeParam(0.5f)); + this->buffer.Update(TypeParam(0.25f)); + this->buffer.Reset(); + + for (std::size_t i = 0; i < this->BufferSize; ++i) + EXPECT_NEAR(math::ToFloat(this->buffer[n - i]), 0.0f, math::Tolerance()); +} + +TYPED_TEST(RecursiveBufferTest, ResetRestoredStateMatchesFreshInstance) +{ + math::Index n; + math::RecursiveBuffer fresh; + + this->buffer.Update(TypeParam(0.75f)); + this->buffer.Reset(); + + for (std::size_t i = 0; i < this->BufferSize; ++i) + EXPECT_NEAR( + math::ToFloat(this->buffer[n - i]), + math::ToFloat(fresh[n - i]), + math::Tolerance()); +} + +TYPED_TEST(RecursiveBufferTest, AssignmentFromInitializerListSetsSlots) +{ + math::Index n; + this->buffer = { TypeParam(0.1f), TypeParam(0.2f), TypeParam(0.3f), TypeParam(0.4f) }; + + EXPECT_NEAR(math::ToFloat(this->buffer[n - 0]), 0.1f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(this->buffer[n - 1]), 0.2f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(this->buffer[n - 2]), 0.3f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(this->buffer[n - 3]), 0.4f, math::Tolerance()); +} + +TYPED_TEST(RecursiveBufferTest, PartialAssignmentZeroFillsRemainingSlots) +{ + math::Index n; + this->buffer = { TypeParam(0.5f), TypeParam(0.25f) }; + + EXPECT_NEAR(math::ToFloat(this->buffer[n - 0]), 0.5f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(this->buffer[n - 1]), 0.25f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(this->buffer[n - 2]), 0.0f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(this->buffer[n - 3]), 0.0f, math::Tolerance()); +} + +TYPED_TEST(RecursiveBufferTest, DeterministicOutputForIdenticalInputSequence) +{ + math::Index n; + math::RecursiveBuffer second; + + std::array seq = { 0.1f, 0.3f, 0.7f, 0.9f }; + + for (float v : seq) + { + this->buffer.Update(TypeParam(v)); + second.Update(TypeParam(v)); + } + + for (std::size_t i = 0; i < this->BufferSize; ++i) + EXPECT_FLOAT_EQ( + math::ToFloat(this->buffer[n - i]), + math::ToFloat(second[n - i])); +} + +TYPED_TEST(RecursiveBufferTest, TwoIndependentInstancesDoNotInterfere) +{ + math::Index n; + math::RecursiveBuffer other; + + this->buffer.Update(TypeParam(0.9f)); + other.Update(TypeParam(0.1f)); - EXPECT_NEAR(math::ToFloat(this->buffer[n - 0]), 0.5f, this->tolerance); - EXPECT_NEAR(math::ToFloat(this->buffer[n - 1]), 0.4f, this->tolerance); - EXPECT_NEAR(math::ToFloat(this->buffer[n - 2]), 0.3f, this->tolerance); - EXPECT_NEAR(math::ToFloat(this->buffer[n - 3]), 0.2f, this->tolerance); + EXPECT_NEAR(math::ToFloat(this->buffer[n - 0]), 0.9f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(other[n - 0]), 0.1f, math::Tolerance()); } diff --git a/numerical/math/test/TestStatistics.cpp b/numerical/math/test/TestStatistics.cpp index f8fbde29..a0d2e857 100644 --- a/numerical/math/test/TestStatistics.cpp +++ b/numerical/math/test/TestStatistics.cpp @@ -1,23 +1,17 @@ #include "numerical/math/QNumber.hpp" #include "numerical/math/Statistics.hpp" +#include "numerical/math/Tolerance.hpp" #include namespace { - template - bool AreValuesNear(T a, T b, float epsilon = 1e-4f) - { - printf("\noriginal: %f, result: %f\n", math::ToFloat(a), math::ToFloat(b)); - return (std::abs(math::ToFloat(a) - math::ToFloat(b)) < epsilon); - } - template class StatisticsTest : public ::testing::Test { protected: - using MatrixType = math::Matrix; using VectorType = math::Vector; + using MatrixType = math::Matrix; static T MakeValue(float f) { @@ -36,102 +30,136 @@ namespace { MakeValue(d) } }; } - - MatrixType MakeMatrix(float a11, float a12, float a21, float a22) - { - return MatrixType{ - { MakeValue(a11), MakeValue(a12) }, - { MakeValue(a21), MakeValue(a22) } - }; - } }; using TestTypes = ::testing::Types; TYPED_TEST_SUITE(StatisticsTest, TestTypes); + + class StatisticsFloatTest + : public ::testing::Test + {}; } -TYPED_TEST(StatisticsTest, Mean) +TYPED_TEST(StatisticsTest, MeanOfUniformlySpacedValues) { auto data = this->MakeVector(0.02f, 0.04f, 0.06f, 0.08f); + auto result = math::Mean(data); - EXPECT_TRUE(AreValuesNear(result, this->MakeValue(0.05f))); + + EXPECT_NEAR(math::ToFloat(result), 0.05f, math::Tolerance()); } -TYPED_TEST(StatisticsTest, Variance) +TYPED_TEST(StatisticsTest, PopulationVarianceOfUniformlySpacedValues) { auto data = this->MakeVector(0.02f, 0.04f, 0.06f, 0.08f); - auto resultPopulation = math::Variance(data, false); - EXPECT_TRUE(AreValuesNear(resultPopulation, this->MakeValue(0.0005f))); + auto result = math::Variance(data, false); - auto resultSample = math::Variance(data, true); - EXPECT_TRUE(AreValuesNear(resultSample, this->MakeValue(0.000667f))); + EXPECT_NEAR(math::ToFloat(result), 0.0005f, math::Tolerance()); } -TYPED_TEST(StatisticsTest, StandardDeviation) +TYPED_TEST(StatisticsTest, SampleVarianceOfUniformlySpacedValues) { auto data = this->MakeVector(0.02f, 0.04f, 0.06f, 0.08f); - auto resultPopulation = math::StandardDeviation(data, false); - EXPECT_TRUE(AreValuesNear(resultPopulation, this->MakeValue(0.02236f), 0.001f)); + auto result = math::Variance(data, true); - auto resultSample = math::StandardDeviation(data, true); - EXPECT_TRUE(AreValuesNear(resultSample, this->MakeValue(0.02582f), 0.001f)); + EXPECT_NEAR(math::ToFloat(result), 0.000667f, math::Tolerance()); } -TYPED_TEST(StatisticsTest, MeanSquaredError) +TYPED_TEST(StatisticsTest, PopulationStandardDeviationOfUniformlySpacedValues) +{ + auto data = this->MakeVector(0.02f, 0.04f, 0.06f, 0.08f); + + auto result = math::StandardDeviation(data, false); + + EXPECT_NEAR(math::ToFloat(result), 0.02236f, math::Tolerance()); +} + +TYPED_TEST(StatisticsTest, SampleStandardDeviationOfUniformlySpacedValues) +{ + auto data = this->MakeVector(0.02f, 0.04f, 0.06f, 0.08f); + + auto result = math::StandardDeviation(data, true); + + EXPECT_NEAR(math::ToFloat(result), 0.02582f, math::Tolerance()); +} + +TYPED_TEST(StatisticsTest, MeanSquaredErrorOfKnownPredictions) { auto actual = this->MakeVector(0.2f, 0.4f, 0.6f, 0.8f); auto predicted = this->MakeVector(0.3f, 0.3f, 0.7f, 0.7f); auto result = math::MeanSquaredError(actual, predicted); - EXPECT_TRUE(AreValuesNear(result, this->MakeValue(0.01f))); + + EXPECT_NEAR(math::ToFloat(result), 0.01f, math::Tolerance()); } -TYPED_TEST(StatisticsTest, RootMeanSquaredError) +TYPED_TEST(StatisticsTest, RootMeanSquaredErrorOfKnownPredictions) { auto actual = this->MakeVector(0.2f, 0.4f, 0.6f, 0.8f); auto predicted = this->MakeVector(0.3f, 0.3f, 0.7f, 0.7f); auto result = math::RootMeanSquaredError(actual, predicted); - EXPECT_TRUE(AreValuesNear(result, this->MakeValue(0.1f))); + + EXPECT_NEAR(math::ToFloat(result), 0.1f, math::Tolerance()); } -TYPED_TEST(StatisticsTest, MeanAbsoluteError) +TYPED_TEST(StatisticsTest, MeanAbsoluteErrorOfKnownPredictions) { auto actual = this->MakeVector(0.2f, 0.4f, 0.6f, 0.8f); auto predicted = this->MakeVector(0.3f, 0.3f, 0.7f, 0.7f); auto result = math::MeanAbsoluteError(actual, predicted); - EXPECT_TRUE(AreValuesNear(result, this->MakeValue(0.1f))); + + EXPECT_NEAR(math::ToFloat(result), 0.1f, math::Tolerance()); } -TYPED_TEST(StatisticsTest, RSquaredScore) +TYPED_TEST(StatisticsTest, RSquaredScoreOfNearPerfectPredictions) { auto actual = this->MakeVector(0.02f, 0.04f, 0.06f, 0.08f); auto predicted = this->MakeVector(0.03f, 0.03f, 0.07f, 0.07f); auto result = math::RSquaredScore(actual, predicted); - EXPECT_TRUE(AreValuesNear(result, this->MakeValue(0.8f), 0.001f)); + + EXPECT_NEAR(math::ToFloat(result), 0.8f, math::Tolerance()); +} + +TYPED_TEST(StatisticsTest, AutoCorrelationLagZeroIsUnity) +{ + auto data = this->MakeVector(0.02f, 0.04f, 0.06f, 0.08f); + + auto result = math::AutoCorrelation(data, 2); + + EXPECT_NEAR(math::ToFloat(result.at(0, 0)), 0.9999f, math::Tolerance()); } -TYPED_TEST(StatisticsTest, AutoCorrelation) +TYPED_TEST(StatisticsTest, AutoCorrelationLagOneValue) { auto data = this->MakeVector(0.02f, 0.04f, 0.06f, 0.08f); + auto result = math::AutoCorrelation(data, 2); - auto expected = this->MakeVector(0.9999f, 0.3333f, -0.6f, 0.0f); - for (size_t i = 0; i < 3; ++i) - EXPECT_TRUE(AreValuesNear(result.at(i, 0), expected.at(i, 0), 0.001f)); + EXPECT_NEAR(math::ToFloat(result.at(1, 0)), 0.3333f, math::Tolerance()); } -TEST(StatisticsTest, ZScore) +TYPED_TEST(StatisticsTest, AutoCorrelationLagTwoValue) { - auto data = math::Matrix(0.45f, 0.5f, 0.5f, 0.55f); + auto data = this->MakeVector(0.02f, 0.04f, 0.06f, 0.08f); + + auto result = math::AutoCorrelation(data, 2); + + EXPECT_NEAR(math::ToFloat(result.at(2, 0)), -0.6f, math::Tolerance()); +} + +TEST_F(StatisticsFloatTest, ZScoreNormalizesSymmetricData) +{ + auto data = math::Matrix{ 0.45f, 0.5f, 0.5f, 0.55f }; + auto result = math::ZScore(data); - auto expected = math::Matrix(-1.4142f, 0.0f, 0.0f, 1.4142f); - for (size_t i = 0; i < 2; ++i) - for (size_t j = 0; j < 2; ++j) - EXPECT_TRUE(AreValuesNear(result.at(i, j), expected.at(i, j))); + EXPECT_NEAR(result.at(0, 0), -1.4142f, math::Tolerance()); + EXPECT_NEAR(result.at(0, 1), 0.0f, math::Tolerance()); + EXPECT_NEAR(result.at(1, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(result.at(1, 1), 1.4142f, math::Tolerance()); } diff --git a/numerical/math/test/TestStepResponseMetrics.cpp b/numerical/math/test/TestStepResponseMetrics.cpp index 417a79b3..ff65b615 100644 --- a/numerical/math/test/TestStepResponseMetrics.cpp +++ b/numerical/math/test/TestStepResponseMetrics.cpp @@ -162,3 +162,69 @@ TEST_F(TestStepResponseMetrics, steady_state_error_known_offset) EXPECT_NEAR(result, 0.1f, 1e-4f); } + +TEST_F(TestStepResponseMetrics, rise_time_never_reaches_threshold_returns_max) +{ + Vec v; + for (std::size_t i = 0; i < N; ++i) + v.at(i, 0) = 0.05f; + + const float result{ math::RiseTime(v, 1.0f) }; + + EXPECT_NEAR(result, static_cast(N - 1), math::Tolerance()); +} + +TEST_F(TestStepResponseMetrics, rise_time_reaches_lo_but_not_hi_returns_max) +{ + Vec v; + for (std::size_t i = 0; i < N; ++i) + v.at(i, 0) = (i < 10) ? 0.0f : 0.5f; + + const float result{ math::RiseTime(v, 1.0f) }; + + EXPECT_NEAR(result, static_cast(N - 1), math::Tolerance()); +} + +TEST_F(TestStepResponseMetrics, settling_time_with_dt_scales_result) +{ + Vec v{ MakeRampPlateau(1.0f, 20) }; + const float dt{ 0.01f }; + + const float sampleResult{ math::SettlingTime(v, 1.0f, 0.02f) }; + const float timeResult{ math::SettlingTime(v, 1.0f, 0.02f, dt) }; + + EXPECT_NEAR(timeResult, sampleResult * dt, math::Tolerance()); +} + +TEST_F(TestStepResponseMetrics, settling_time_always_outside_band_returns_full_duration) +{ + Vec v; + for (std::size_t i = 0; i < N; ++i) + v.at(i, 0) = 0.0f; + + const float result{ math::SettlingTime(v, 1.0f, 0.02f) }; + + EXPECT_NEAR(result, static_cast(N), math::Tolerance()); +} + +TEST_F(TestStepResponseMetrics, peak_time_at_index_zero_for_monotone_decreasing) +{ + Vec v; + for (std::size_t i = 0; i < N; ++i) + v.at(i, 0) = static_cast(N - i); + + const float result{ math::PeakTime(v) }; + + EXPECT_NEAR(result, 0.0f, math::Tolerance()); +} + +TEST_F(TestStepResponseMetrics, steady_state_error_custom_tail_size) +{ + Vec v; + for (std::size_t i = 0; i < N; ++i) + v.at(i, 0) = (i < N / 2) ? 0.0f : 0.8f; + + const float result{ math::SteadyStateError(v, 1.0f) }; + + EXPECT_NEAR(result, 0.2f, 1e-4f); +} diff --git a/numerical/math/test/TestToeplitz.cpp b/numerical/math/test/TestToeplitz.cpp index 66722c3e..e05daf12 100644 --- a/numerical/math/test/TestToeplitz.cpp +++ b/numerical/math/test/TestToeplitz.cpp @@ -1,53 +1,47 @@ #include "numerical/math/QNumber.hpp" #include "numerical/math/Toeplitz.hpp" +#include "numerical/math/Tolerance.hpp" +#include "numerical/math/test_doubles/MatrixTestSupport.hpp" #include namespace { - template - bool AreMatricesNear(const math::Matrix& a, - const math::Matrix& b, - float epsilon = 1e-4f) - { - for (size_t i = 0; i < 2; ++i) - { - for (size_t j = 0; j < 2; ++j) - { - if (std::abs(math::ToFloat(a.at(i, j)) - math::ToFloat(b.at(i, j))) >= epsilon) - return false; - } - } - return true; - } + using math::test::AreMatricesNear; + using math::test::AreVectorsNear; template class ToeplitzMatrixTest : public ::testing::Test { protected: - static constexpr size_t N = 2; + static constexpr size_t N = 3; using ToeplitzType = math::ToeplitzMatrix; using VectorType = math::Vector; using MatrixType = math::Matrix; static T MakeValue(float f) { - return T(std::max(std::min(f, 0.1f), -0.1f)); + return T(std::max(std::min(f, 0.09f), -0.09f)); } - VectorType MakeVector(float a, float b) + VectorType MakeVector(float a, float b, float c) { return VectorType{ { MakeValue(a) }, - { MakeValue(b) } + { MakeValue(b) }, + { MakeValue(c) } }; } - MatrixType MakeMatrix(float a11, float a12, float a21, float a22) + MatrixType MakeMatrix( + float a00, float a01, float a02, + float a10, float a11, float a12, + float a20, float a21, float a22) { return MatrixType{ - { MakeValue(a11), MakeValue(a12) }, - { MakeValue(a21), MakeValue(a22) } + { MakeValue(a00), MakeValue(a01), MakeValue(a02) }, + { MakeValue(a10), MakeValue(a11), MakeValue(a12) }, + { MakeValue(a20), MakeValue(a21), MakeValue(a22) } }; } }; @@ -56,162 +50,269 @@ namespace TYPED_TEST_SUITE(ToeplitzMatrixTest, TestTypes); } -TYPED_TEST(ToeplitzMatrixTest, DefaultConstructor) +TYPED_TEST(ToeplitzMatrixTest, DefaultConstructorProducesAllZeroEntries) { typename TestFixture::ToeplitzType t; + auto full = t.ToFullMatrix(); for (size_t i = 0; i < TestFixture::N; ++i) for (size_t j = 0; j < TestFixture::N; ++j) - EXPECT_FLOAT_EQ(math::ToFloat(full.at(i, j)), 0.0f); + EXPECT_NEAR(math::ToFloat(full.at(i, j)), 0.0f, math::Tolerance()); } -TYPED_TEST(ToeplitzMatrixTest, SymmetricConstructor) +TYPED_TEST(ToeplitzMatrixTest, SymmetricConstructorProducesSymmetricToeplitzStructure) { - auto vec = this->MakeVector(0.02f, 0.01f); + auto vec = this->MakeVector(0.06f, 0.03f, 0.01f); + typename TestFixture::ToeplitzType t(vec); EXPECT_TRUE(t.IsSymmetric()); - - auto expected = this->MakeMatrix(0.02f, 0.01f, 0.01f, 0.02f); - EXPECT_TRUE(AreMatricesNear(t.ToFullMatrix(), expected)); + EXPECT_TRUE(AreMatricesNear(t.ToFullMatrix(), + this->MakeMatrix( + 0.06f, 0.03f, 0.01f, + 0.03f, 0.06f, 0.03f, + 0.01f, 0.03f, 0.06f))); } -TYPED_TEST(ToeplitzMatrixTest, GeneralConstructor) +TYPED_TEST(ToeplitzMatrixTest, GeneralConstructorProducesAsymmetricToeplitzStructure) { - auto row = this->MakeVector(0.02f, 0.01f); - auto col = this->MakeVector(0.02f, -0.01f); + auto row = this->MakeVector(0.06f, 0.03f, 0.01f); + auto col = this->MakeVector(0.06f, -0.03f, -0.01f); + typename TestFixture::ToeplitzType t(row, col); EXPECT_FALSE(t.IsSymmetric()); + EXPECT_TRUE(AreMatricesNear(t.ToFullMatrix(), + this->MakeMatrix( + 0.06f, 0.03f, 0.01f, + -0.03f, 0.06f, 0.03f, + -0.01f, -0.03f, 0.06f))); +} + +TYPED_TEST(ToeplitzMatrixTest, ElementAccessMatchesConstructedRowAndColumn) +{ + auto row = this->MakeVector(0.06f, 0.03f, 0.01f); + auto col = this->MakeVector(0.06f, -0.03f, -0.01f); + typename TestFixture::ToeplitzType t(row, col); + + EXPECT_NEAR(math::ToFloat(t.at(0, 0)), 0.06f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(t.at(0, 1)), 0.03f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(t.at(0, 2)), 0.01f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(t.at(1, 0)), -0.03f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(t.at(1, 1)), 0.06f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(t.at(1, 2)), 0.03f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(t.at(2, 0)), -0.01f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(t.at(2, 1)), -0.03f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(t.at(2, 2)), 0.06f, math::Tolerance()); +} + +TYPED_TEST(ToeplitzMatrixTest, ToFullMatrixConsistentWithAtAccessor) +{ + auto row = this->MakeVector(0.06f, 0.03f, 0.01f); + auto col = this->MakeVector(0.06f, -0.03f, -0.01f); + typename TestFixture::ToeplitzType t(row, col); + + auto full = t.ToFullMatrix(); - auto expected = this->MakeMatrix(0.02f, 0.01f, -0.01f, 0.02f); - EXPECT_TRUE(AreMatricesNear(t.ToFullMatrix(), expected)); + for (size_t i = 0; i < TestFixture::N; ++i) + for (size_t j = 0; j < TestFixture::N; ++j) + EXPECT_NEAR(math::ToFloat(full.at(i, j)), math::ToFloat(t.at(i, j)), math::Tolerance()); } -TYPED_TEST(ToeplitzMatrixTest, VectorMultiplication) +TYPED_TEST(ToeplitzMatrixTest, VectorMultiplicationMatchesFullMatrixMultiply) { - auto vec = this->MakeVector(0.02f, 0.01f); + auto vec = this->MakeVector(0.06f, 0.03f, 0.01f); typename TestFixture::ToeplitzType t(vec); + auto x = this->MakeVector(0.01f, 0.02f, 0.03f); - auto x = this->MakeVector(0.01f, 0.01f); auto result = t * x; - auto expected = this->MakeVector(0.0003f, 0.0003f); + auto full = t.ToFullMatrix(); + typename TestFixture::VectorType expected; + for (size_t i = 0; i < TestFixture::N; ++i) + { + float sum = 0.0f; + for (size_t j = 0; j < TestFixture::N; ++j) + sum += math::ToFloat(full.at(i, j)) * math::ToFloat(x.at(j, 0)); + expected.at(i, 0) = typename TestFixture::VectorType::value_type(sum); + } for (size_t i = 0; i < TestFixture::N; ++i) - EXPECT_NEAR(math::ToFloat(result.at(i, 0)), math::ToFloat(expected.at(i, 0)), 1e-4f); + EXPECT_NEAR(math::ToFloat(result.at(i, 0)), math::ToFloat(expected.at(i, 0)), math::Tolerance()); } -TYPED_TEST(ToeplitzMatrixTest, Addition) +TYPED_TEST(ToeplitzMatrixTest, ZeroVectorMultiplicationProducesZeroVector) { - auto vec1 = this->MakeVector(0.02f, 0.01f); - auto vec2 = this->MakeVector(0.01f, 0.005f); + auto vec = this->MakeVector(0.06f, 0.03f, 0.01f); + typename TestFixture::ToeplitzType t(vec); + typename TestFixture::VectorType zero; - typename TestFixture::ToeplitzType t1(vec1); - typename TestFixture::ToeplitzType t2(vec2); + auto result = t * zero; + + for (size_t i = 0; i < TestFixture::N; ++i) + EXPECT_NEAR(math::ToFloat(result.at(i, 0)), 0.0f, math::Tolerance()); +} + +TYPED_TEST(ToeplitzMatrixTest, AdditionProducesCorrectToeplitzSum) +{ + typename TestFixture::ToeplitzType t1(this->MakeVector(0.04f, 0.02f, 0.01f)); + typename TestFixture::ToeplitzType t2(this->MakeVector(0.02f, 0.01f, 0.005f)); auto result = t1 + t2; - auto expected = this->MakeMatrix(0.03f, 0.015f, 0.015f, 0.03f); - EXPECT_TRUE(AreMatricesNear(result.ToFullMatrix(), expected)); + EXPECT_TRUE(AreMatricesNear(result.ToFullMatrix(), + this->MakeMatrix( + 0.06f, 0.03f, 0.015f, + 0.03f, 0.06f, 0.03f, + 0.015f, 0.03f, 0.06f))); } -TYPED_TEST(ToeplitzMatrixTest, Subtraction) +TYPED_TEST(ToeplitzMatrixTest, AdditionIsCommutative) { - auto vec1 = this->MakeVector(0.02f, 0.01f); - auto vec2 = this->MakeVector(0.01f, 0.005f); - - typename TestFixture::ToeplitzType t1(vec1); - typename TestFixture::ToeplitzType t2(vec2); + typename TestFixture::ToeplitzType t1(this->MakeVector(0.04f, 0.02f, 0.01f)); + typename TestFixture::ToeplitzType t2(this->MakeVector(0.02f, 0.01f, 0.005f)); - auto result = t1 - t2; + auto ab = t1 + t2; + auto ba = t2 + t1; - auto expected = this->MakeMatrix(0.01f, 0.005f, 0.005f, 0.01f); - EXPECT_TRUE(AreMatricesNear(result.ToFullMatrix(), expected)); + EXPECT_TRUE(AreMatricesNear(ab.ToFullMatrix(), ba.ToFullMatrix())); } -TYPED_TEST(ToeplitzMatrixTest, ElementAccess) +TYPED_TEST(ToeplitzMatrixTest, SubtractionProducesCorrectToeplitzDifference) { - auto row = this->MakeVector(0.02f, 0.01f); - auto col = this->MakeVector(0.02f, -0.01f); - typename TestFixture::ToeplitzType t(row, col); + typename TestFixture::ToeplitzType t1(this->MakeVector(0.06f, 0.03f, 0.015f)); + typename TestFixture::ToeplitzType t2(this->MakeVector(0.02f, 0.01f, 0.005f)); - EXPECT_NEAR(math::ToFloat(t.at(0, 0)), 0.02f, 1e-4f); - EXPECT_NEAR(math::ToFloat(t.at(0, 1)), 0.01f, 1e-4f); - EXPECT_NEAR(math::ToFloat(t.at(1, 0)), -0.01f, 1e-4f); - EXPECT_NEAR(math::ToFloat(t.at(1, 1)), 0.02f, 1e-4f); + auto result = t1 - t2; + + EXPECT_TRUE(AreMatricesNear(result.ToFullMatrix(), + this->MakeMatrix( + 0.04f, 0.02f, 0.01f, + 0.02f, 0.04f, 0.02f, + 0.01f, 0.02f, 0.04f))); } -TYPED_TEST(ToeplitzMatrixTest, IsToeplitzMatrix_ValidCase) +TYPED_TEST(ToeplitzMatrixTest, IsToeplitzMatrixAcceptsValidPattern) { auto matrix = this->MakeMatrix( - 0.02f, 0.01f, // First row: [0.02, 0.01] - 0.03f, 0.02f // Second row: [0.03, 0.02] - ); + 0.06f, 0.03f, 0.01f, + 0.03f, 0.06f, 0.03f, + 0.01f, 0.03f, 0.06f); EXPECT_TRUE(TestFixture::ToeplitzType::IsToeplitzMatrix(matrix)); } -TYPED_TEST(ToeplitzMatrixTest, IsToeplitzMatrix_InvalidCase) +TYPED_TEST(ToeplitzMatrixTest, IsToeplitzMatrixRejectsNonToeplitzPattern) { auto matrix = this->MakeMatrix( - 0.02f, 0.01f, // First row: [0.02, 0.01] - 0.03f, 0.05f // Second row: [0.03, 0.05] - 0.05 breaks the pattern - ); + 0.06f, 0.03f, 0.01f, + 0.03f, 0.06f, 0.03f, + 0.01f, 0.03f, 0.09f); EXPECT_FALSE(TestFixture::ToeplitzType::IsToeplitzMatrix(matrix)); } -TYPED_TEST(ToeplitzMatrixTest, IsToeplitzMatrix_ZeroMatrix) +TYPED_TEST(ToeplitzMatrixTest, IsToeplitzMatrixAcceptsZeroMatrix) { auto matrix = this->MakeMatrix( - 0.0f, 0.0f, - 0.0f, 0.0f); + 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f); EXPECT_TRUE(TestFixture::ToeplitzType::IsToeplitzMatrix(matrix)); } -TYPED_TEST(ToeplitzMatrixTest, ExtractToeplitzVectors_GeneralCase) +TYPED_TEST(ToeplitzMatrixTest, ToFullMatrixPassesIsToeplitzCheck) +{ + auto row = this->MakeVector(0.06f, 0.03f, 0.01f); + auto col = this->MakeVector(0.06f, -0.03f, -0.01f); + typename TestFixture::ToeplitzType t(row, col); + + EXPECT_TRUE(TestFixture::ToeplitzType::IsToeplitzMatrix(t.ToFullMatrix())); +} + +TYPED_TEST(ToeplitzMatrixTest, ExtractToeplitzVectorsRecoverRowAndColumn) { auto matrix = this->MakeMatrix( - 0.02f, 0.01f, // First row: [0.02, 0.01] - 0.03f, 0.02f // Second row: [0.03, 0.02] - ); + 0.06f, 0.03f, 0.01f, + 0.03f, 0.06f, 0.03f, + 0.01f, 0.03f, 0.06f); auto [row, col] = TestFixture::ToeplitzType::ExtractToeplitzVectors(matrix); - EXPECT_NEAR(math::ToFloat(row.at(0, 0)), 0.02f, 1e-4f); - EXPECT_NEAR(math::ToFloat(row.at(1, 0)), 0.01f, 1e-4f); + EXPECT_NEAR(math::ToFloat(row.at(0, 0)), 0.06f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(row.at(1, 0)), 0.03f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(row.at(2, 0)), 0.01f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(col.at(0, 0)), 0.06f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(col.at(1, 0)), 0.03f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(col.at(2, 0)), 0.01f, math::Tolerance()); +} + +TYPED_TEST(ToeplitzMatrixTest, ExtractThenConstructRoundTripMatchesOriginalMatrix) +{ + auto matrix = this->MakeMatrix( + 0.06f, 0.03f, 0.01f, + -0.03f, 0.06f, 0.03f, + -0.01f, -0.03f, 0.06f); - EXPECT_NEAR(math::ToFloat(col.at(0, 0)), 0.02f, 1e-4f); - EXPECT_NEAR(math::ToFloat(col.at(1, 0)), 0.03f, 1e-4f); + auto [row, col] = TestFixture::ToeplitzType::ExtractToeplitzVectors(matrix); + typename TestFixture::ToeplitzType t(row, col); + + EXPECT_TRUE(AreMatricesNear(t.ToFullMatrix(), matrix)); } -TYPED_TEST(ToeplitzMatrixTest, ExtractToeplitzVectors_SymmetricCase) +TYPED_TEST(ToeplitzMatrixTest, ExtractToeplitzVectorsSymmetricMatrixYieldsEqualRowAndColumn) { auto matrix = this->MakeMatrix( - 0.02f, 0.01f, // First row: [0.02, 0.01] - 0.01f, 0.02f // Second row: [0.01, 0.02] - ); + 0.06f, 0.03f, 0.01f, + 0.03f, 0.06f, 0.03f, + 0.01f, 0.03f, 0.06f); auto [row, col] = TestFixture::ToeplitzType::ExtractToeplitzVectors(matrix); - EXPECT_NEAR(math::ToFloat(row.at(0, 0)), math::ToFloat(col.at(0, 0)), 1e-4f); - EXPECT_NEAR(math::ToFloat(row.at(1, 0)), math::ToFloat(col.at(1, 0)), 1e-4f); + for (size_t i = 0; i < TestFixture::N; ++i) + EXPECT_NEAR(math::ToFloat(row.at(i, 0)), math::ToFloat(col.at(i, 0)), math::Tolerance()); } -TYPED_TEST(ToeplitzMatrixTest, ExtractToeplitzVectors_ZeroMatrix) +TYPED_TEST(ToeplitzMatrixTest, ExtractToeplitzVectorsZeroMatrixYieldsZeroVectors) { auto matrix = this->MakeMatrix( - 0.0f, 0.0f, - 0.0f, 0.0f); + 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f); auto [row, col] = TestFixture::ToeplitzType::ExtractToeplitzVectors(matrix); for (size_t i = 0; i < TestFixture::N; ++i) { - EXPECT_NEAR(math::ToFloat(row.at(i, 0)), 0.0f, 1e-4f); - EXPECT_NEAR(math::ToFloat(col.at(i, 0)), 0.0f, 1e-4f); + EXPECT_NEAR(math::ToFloat(row.at(i, 0)), 0.0f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(col.at(i, 0)), 0.0f, math::Tolerance()); } } + +TYPED_TEST(ToeplitzMatrixTest, CreateToeplitzMatrixFactoryProducesSameResultAsConstructor) +{ + auto vec = this->MakeVector(0.06f, 0.03f, 0.01f); + + auto fromFactory = math::CreateToeplitzMatrix(vec); + typename TestFixture::ToeplitzType fromCtor(vec); + + EXPECT_TRUE(AreMatricesNear(fromFactory.ToFullMatrix(), fromCtor.ToFullMatrix())); +} + +TYPED_TEST(ToeplitzMatrixTest, TwoInstancesWithSameInputProduceIdenticalOutput) +{ + auto row = this->MakeVector(0.06f, 0.03f, 0.01f); + auto col = this->MakeVector(0.06f, -0.03f, -0.01f); + + typename TestFixture::ToeplitzType t1(row, col); + typename TestFixture::ToeplitzType t2(row, col); + + auto x = this->MakeVector(0.02f, 0.04f, 0.03f); + auto r1 = t1 * x; + auto r2 = t2 * x; + + for (size_t i = 0; i < TestFixture::N; ++i) + EXPECT_FLOAT_EQ(math::ToFloat(r1.at(i, 0)), math::ToFloat(r2.at(i, 0))); +} diff --git a/numerical/math/test_doubles/CMakeLists.txt b/numerical/math/test_doubles/CMakeLists.txt index bbcffa9e..6d00d87e 100644 --- a/numerical/math/test_doubles/CMakeLists.txt +++ b/numerical/math/test_doubles/CMakeLists.txt @@ -9,6 +9,7 @@ target_link_libraries(numerical.math_test_helper INTERFACE target_sources(numerical.math_test_helper PRIVATE AdvancedFunctionsStub.hpp HyperbolicFunctionsStub.hpp + MatrixTestSupport.hpp SingleInstructionMultipleDataStub.hpp TrigonometricFunctionsStub.hpp ) diff --git a/numerical/math/test_doubles/MatrixTestSupport.hpp b/numerical/math/test_doubles/MatrixTestSupport.hpp new file mode 100644 index 00000000..2d47d266 --- /dev/null +++ b/numerical/math/test_doubles/MatrixTestSupport.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include "numerical/math/Matrix.hpp" +#include "numerical/math/Tolerance.hpp" +#include + +namespace math::test +{ + template + bool AreMatricesNear(const math::Matrix& a, const math::Matrix& b, float eps = math::Tolerance()) + { + for (std::size_t i = 0; i < Rows; ++i) + for (std::size_t j = 0; j < Cols; ++j) + if (std::abs(math::ToFloat(a.at(i, j)) - math::ToFloat(b.at(i, j))) >= eps) + return false; + return true; + } + + template + bool AreVectorsNear(const math::Vector& a, const math::Vector& b, float eps = math::Tolerance()) + { + for (std::size_t i = 0; i < Size; ++i) + if (std::abs(math::ToFloat(a.at(i, 0)) - math::ToFloat(b.at(i, 0))) >= eps) + return false; + return true; + } +} diff --git a/numerical/robust_control/test/TestHInfinityStateFeedback.cpp b/numerical/robust_control/test/TestHInfinityStateFeedback.cpp index 71485bbf..09cfbc0e 100644 --- a/numerical/robust_control/test/TestHInfinityStateFeedback.cpp +++ b/numerical/robust_control/test/TestHInfinityStateFeedback.cpp @@ -73,7 +73,7 @@ TEST_F(TestHInfinityStateFeedback, closed_loop_is_schur_stable) auto roots = dk.Solve(std::span{ charPoly.data(), 3 }); for (const auto& root : roots) - EXPECT_LT(std::abs(root), 1.0f); + EXPECT_LT(math::Abs(root), 1.0f); } TEST_F(TestHInfinityStateFeedback, achieves_target_gamma) diff --git a/numerical/solvers/CMakeLists.txt b/numerical/solvers/CMakeLists.txt index 4ea9396e..90193092 100644 --- a/numerical/solvers/CMakeLists.txt +++ b/numerical/solvers/CMakeLists.txt @@ -34,6 +34,7 @@ numerical_add_coverage_sources(numerical.solver DurandKerner.cpp GaussianElimination.cpp JacobiEigenSolver.cpp + LevinsonDurbin.cpp LuDecomposition.cpp LyapunovSylvester.cpp QrDecomposition.cpp diff --git a/numerical/solvers/DurandKerner.hpp b/numerical/solvers/DurandKerner.hpp index 13692098..79ab6fcd 100644 --- a/numerical/solvers/DurandKerner.hpp +++ b/numerical/solvers/DurandKerner.hpp @@ -6,10 +6,10 @@ #include "infra/util/BoundedVector.hpp" #include "infra/util/ReallyAssert.hpp" +#include "numerical/math/ComplexNumber.hpp" #include "numerical/math/CompilerOptimizations.hpp" #include #include -#include #include #include #include @@ -24,16 +24,16 @@ namespace solvers "DurandKerner only supports floating-point types"); public: - using Roots = typename infra::BoundedVector>::template WithMaxSize; + using Roots = typename infra::BoundedVector>::template WithMaxSize; Roots Solve(std::span coefficients, std::size_t maxIterations = 200, T tolerance = T(1e-6)) const; private: - static std::complex EvaluatePolynomial( - std::span coefficients, std::complex x); + static math::Complex EvaluatePolynomial( + std::span coefficients, math::Complex x); - static std::complex ComputeDenominator( + static math::Complex ComputeDenominator( const Roots& roots, std::size_t r, std::size_t order); static bool Iterate(Roots& roots, std::span coefficients, @@ -43,26 +43,26 @@ namespace solvers //// Implementation //// template - std::complex DurandKerner::EvaluatePolynomial( - std::span coefficients, std::complex x) + math::Complex DurandKerner::EvaluatePolynomial( + std::span coefficients, math::Complex x) { - std::complex result(coefficients[0], T(0)); + math::Complex result(coefficients[0], T(0)); for (std::size_t c = 1; c < coefficients.size(); ++c) - result = result * x + std::complex(coefficients[c], T(0)); + result = result * x + math::Complex(coefficients[c], T(0)); return result; } template - std::complex DurandKerner::ComputeDenominator( + math::Complex DurandKerner::ComputeDenominator( const Roots& roots, std::size_t r, std::size_t order) { - std::complex product(T(1), T(0)); + math::Complex product(T(1), T(0)); for (std::size_t j = 0; j < order; ++j) { if (j == r) continue; auto diff = roots[r] - roots[j]; - if (std::abs(diff) > T(1e-15)) + if (math::Abs(diff) > T(1e-15)) product *= diff; } return product; @@ -81,7 +81,7 @@ namespace solvers auto correction = pVal / denominator; roots[r] -= correction; - if (std::abs(correction) > tolerance) + if (math::Abs(correction) > tolerance) converged = false; } @@ -130,11 +130,11 @@ namespace solvers } std::ranges::sort(roots, - [](const std::complex& a, const std::complex& b) + [](const math::Complex& a, const math::Complex& b) { - if (std::abs(a.real() - b.real()) > T(0.01)) - return a.real() < b.real(); - return a.imag() < b.imag(); + if (std::abs(a.Real() - b.Real()) > T(0.01)) + return a.Real() < b.Real(); + return a.Imaginary() < b.Imaginary(); }); return roots; diff --git a/numerical/solvers/GaussianElimination.hpp b/numerical/solvers/GaussianElimination.hpp index f5769549..5da54956 100644 --- a/numerical/solvers/GaussianElimination.hpp +++ b/numerical/solvers/GaussianElimination.hpp @@ -27,48 +27,12 @@ namespace solvers SolutionVector Solve(const InputMatrix& a, const InputVector& b) override; private: - std::size_t FindPivotRow(const InputMatrix& matrix, std::size_t col) const; - void SwapRows(InputMatrix& matrix, SolutionVector& vector, std::size_t row1, std::size_t row2) const; void EliminateBelow(InputMatrix& matrix, SolutionVector& vector, std::size_t col) const; }; template math::Matrix SolveSystem(const math::Matrix& a, const math::Matrix& b); - template - std::size_t GaussianElimination::FindPivotRow(const InputMatrix& matrix, std::size_t col) const - { - std::size_t pivotRow = col; - float maxVal = std::abs(math::ToFloat(matrix.at(col, col))); - - for (std::size_t row = col + 1; row < N; ++row) - { - float absVal = std::abs(math::ToFloat(matrix.at(row, col))); - if (absVal > maxVal) - { - maxVal = absVal; - pivotRow = row; - } - } - - return pivotRow; - } - - template - void GaussianElimination::SwapRows(InputMatrix& matrix, SolutionVector& vector, std::size_t row1, std::size_t row2) const - { - for (std::size_t j = 0; j < N; ++j) - { - T tmp = matrix.at(row1, j); - matrix.at(row1, j) = matrix.at(row2, j); - matrix.at(row2, j) = tmp; - } - - T tmp = vector.at(row1, 0); - vector.at(row1, 0) = vector.at(row2, 0); - vector.at(row2, 0) = tmp; - } - template void GaussianElimination::EliminateBelow(InputMatrix& matrix, SolutionVector& vector, std::size_t col) const { @@ -96,10 +60,13 @@ namespace solvers for (std::size_t col = 0; col < N; ++col) { - std::size_t pivotRow = FindPivotRow(augA, col); + std::size_t pivotRow = math::FindPartialPivotRow(augA, col); if (pivotRow != col) - SwapRows(augA, augB, col, pivotRow); + { + math::SwapRows(augA, col, pivotRow); + math::SwapRows(augB, col, pivotRow); + } EliminateBelow(augA, augB, col); } diff --git a/numerical/solvers/LevinsonDurbin.cpp b/numerical/solvers/LevinsonDurbin.cpp new file mode 100644 index 00000000..032b3682 --- /dev/null +++ b/numerical/solvers/LevinsonDurbin.cpp @@ -0,0 +1,7 @@ +#include "numerical/solvers/LevinsonDurbin.hpp" + +namespace solvers +{ + template class LevinsonDurbin; + template class LevinsonDurbin; +} diff --git a/numerical/solvers/LevinsonDurbin.hpp b/numerical/solvers/LevinsonDurbin.hpp index 286ada0a..f0516f57 100644 --- a/numerical/solvers/LevinsonDurbin.hpp +++ b/numerical/solvers/LevinsonDurbin.hpp @@ -83,4 +83,9 @@ namespace solvers { return LevinsonDurbin(); } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class LevinsonDurbin; + extern template class LevinsonDurbin; +#endif } diff --git a/numerical/solvers/SpectralRadius.hpp b/numerical/solvers/SpectralRadius.hpp index e8d8b1c3..6a151121 100644 --- a/numerical/solvers/SpectralRadius.hpp +++ b/numerical/solvers/SpectralRadius.hpp @@ -72,7 +72,7 @@ namespace solvers T rho{}; for (std::size_t i = 0; i < roots.size(); ++i) { - T magnitude = std::abs(roots[i]); + T magnitude = math::Abs(roots[i]); if (magnitude > rho) rho = magnitude; } diff --git a/numerical/solvers/test/TestDurandKerner.cpp b/numerical/solvers/test/TestDurandKerner.cpp index 29c361b1..d71bb39e 100644 --- a/numerical/solvers/test/TestDurandKerner.cpp +++ b/numerical/solvers/test/TestDurandKerner.cpp @@ -22,8 +22,8 @@ TYPED_TEST(TestDurandKerner, finds_roots_of_linear_polynomial) auto roots = this->solver.Solve(coefficients); ASSERT_EQ(roots.size(), 1u); - EXPECT_NEAR(roots[0].real(), -2.0, 1e-4); - EXPECT_NEAR(roots[0].imag(), 0.0, 1e-4); + EXPECT_NEAR(roots[0].Real(), -2.0, 1e-4); + EXPECT_NEAR(roots[0].Imaginary(), 0.0, 1e-4); } TYPED_TEST(TestDurandKerner, finds_real_roots_of_quadratic) @@ -33,10 +33,10 @@ TYPED_TEST(TestDurandKerner, finds_real_roots_of_quadratic) auto roots = this->solver.Solve(coefficients); ASSERT_EQ(roots.size(), 2u); - EXPECT_NEAR(roots[0].real(), 1.0, 1e-4); - EXPECT_NEAR(roots[0].imag(), 0.0, 1e-4); - EXPECT_NEAR(roots[1].real(), 2.0, 1e-4); - EXPECT_NEAR(roots[1].imag(), 0.0, 1e-4); + EXPECT_NEAR(roots[0].Real(), 1.0, 1e-4); + EXPECT_NEAR(roots[0].Imaginary(), 0.0, 1e-4); + EXPECT_NEAR(roots[1].Real(), 2.0, 1e-4); + EXPECT_NEAR(roots[1].Imaginary(), 0.0, 1e-4); } TYPED_TEST(TestDurandKerner, finds_complex_roots_of_quadratic) @@ -46,10 +46,10 @@ TYPED_TEST(TestDurandKerner, finds_complex_roots_of_quadratic) auto roots = this->solver.Solve(coefficients); ASSERT_EQ(roots.size(), 2u); - EXPECT_NEAR(roots[0].real(), 0.0, 1e-4); - EXPECT_NEAR(std::abs(roots[0].imag()), 1.0, 1e-4); - EXPECT_NEAR(roots[1].real(), 0.0, 1e-4); - EXPECT_NEAR(std::abs(roots[1].imag()), 1.0, 1e-4); + EXPECT_NEAR(roots[0].Real(), 0.0, 1e-4); + EXPECT_NEAR(std::abs(roots[0].Imaginary()), 1.0, 1e-4); + EXPECT_NEAR(roots[1].Real(), 0.0, 1e-4); + EXPECT_NEAR(std::abs(roots[1].Imaginary()), 1.0, 1e-4); } TYPED_TEST(TestDurandKerner, finds_roots_of_cubic) @@ -59,12 +59,12 @@ TYPED_TEST(TestDurandKerner, finds_roots_of_cubic) auto roots = this->solver.Solve(coefficients); ASSERT_EQ(roots.size(), 3u); - EXPECT_NEAR(roots[0].real(), 1.0, 1e-3); - EXPECT_NEAR(roots[0].imag(), 0.0, 1e-3); - EXPECT_NEAR(roots[1].real(), 2.0, 1e-3); - EXPECT_NEAR(roots[1].imag(), 0.0, 1e-3); - EXPECT_NEAR(roots[2].real(), 3.0, 1e-3); - EXPECT_NEAR(roots[2].imag(), 0.0, 1e-3); + EXPECT_NEAR(roots[0].Real(), 1.0, 1e-3); + EXPECT_NEAR(roots[0].Imaginary(), 0.0, 1e-3); + EXPECT_NEAR(roots[1].Real(), 2.0, 1e-3); + EXPECT_NEAR(roots[1].Imaginary(), 0.0, 1e-3); + EXPECT_NEAR(roots[2].Real(), 3.0, 1e-3); + EXPECT_NEAR(roots[2].Imaginary(), 0.0, 1e-3); } TYPED_TEST(TestDurandKerner, finds_roots_of_quartic) @@ -74,10 +74,10 @@ TYPED_TEST(TestDurandKerner, finds_roots_of_quartic) auto roots = this->solver.Solve(coefficients); ASSERT_EQ(roots.size(), 4u); - EXPECT_NEAR(roots[0].real(), 1.0, 1e-2); - EXPECT_NEAR(roots[1].real(), 2.0, 1e-2); - EXPECT_NEAR(roots[2].real(), 3.0, 1e-2); - EXPECT_NEAR(roots[3].real(), 4.0, 1e-2); + EXPECT_NEAR(roots[0].Real(), 1.0, 1e-2); + EXPECT_NEAR(roots[1].Real(), 2.0, 1e-2); + EXPECT_NEAR(roots[2].Real(), 3.0, 1e-2); + EXPECT_NEAR(roots[3].Real(), 4.0, 1e-2); } TYPED_TEST(TestDurandKerner, finds_repeated_roots) @@ -87,8 +87,8 @@ TYPED_TEST(TestDurandKerner, finds_repeated_roots) auto roots = this->solver.Solve(coefficients); ASSERT_EQ(roots.size(), 2u); - EXPECT_NEAR(roots[0].real(), 1.0, 1e-3); - EXPECT_NEAR(roots[1].real(), 1.0, 1e-3); + EXPECT_NEAR(roots[0].Real(), 1.0, 1e-3); + EXPECT_NEAR(roots[1].Real(), 1.0, 1e-3); } TYPED_TEST(TestDurandKerner, returns_empty_for_constant_polynomial) @@ -109,7 +109,7 @@ TYPED_TEST(TestDurandKerner, finds_roots_of_second_order_system_polynomial) auto roots = this->solver.Solve(coefficients); ASSERT_EQ(roots.size(), 2u); - EXPECT_NEAR(roots[0].real(), double(-zeta * wn), 1e-3); - EXPECT_NEAR(roots[1].real(), double(-zeta * wn), 1e-3); - EXPECT_NEAR(std::abs(roots[0].imag()), double(wn * std::sqrt(TypeParam(1.0) - zeta * zeta)), 1e-3); + EXPECT_NEAR(roots[0].Real(), double(-zeta * wn), 1e-3); + EXPECT_NEAR(roots[1].Real(), double(-zeta * wn), 1e-3); + EXPECT_NEAR(std::abs(roots[0].Imaginary()), double(wn * std::sqrt(TypeParam(1.0) - zeta * zeta)), 1e-3); } diff --git a/numerical/solvers/test/TestGaussianElimination.cpp b/numerical/solvers/test/TestGaussianElimination.cpp index 5d301dc2..c617c56b 100644 --- a/numerical/solvers/test/TestGaussianElimination.cpp +++ b/numerical/solvers/test/TestGaussianElimination.cpp @@ -14,9 +14,16 @@ namespace using TestTypes = ::testing::Types; TYPED_TEST_SUITE(TestGaussianElimination, TestTypes); + + class TestGaussianEliminationFloat + : public ::testing::Test + { + protected: + solvers::GaussianElimination solver; + }; } -TYPED_TEST(TestGaussianElimination, solve_returns_vector_of_correct_size) +TYPED_TEST(TestGaussianElimination, solve_identity_matrix_returns_rhs) { auto a = math::SquareMatrix::Identity(); math::Vector b{ { TypeParam(0.1f) }, { TypeParam(0.2f) }, { TypeParam(0.3f) } }; @@ -35,7 +42,6 @@ TYPED_TEST(TestGaussianElimination, solve_applies_pivot_when_diagonal_is_small) { TypeParam(0.5f), TypeParam(0.1f), TypeParam(0.0f) }, { TypeParam(0.0f), TypeParam(0.0f), TypeParam(0.5f) } }; - math::Vector b{ { TypeParam(0.25f) }, { TypeParam(0.3f) }, { TypeParam(0.1f) } }; auto result = this->solver.Solve(a, b); @@ -50,7 +56,6 @@ TYPED_TEST(TestGaussianElimination, solve_eliminates_below_diagonal) { TypeParam(0.25f), TypeParam(0.4f), TypeParam(0.0f) }, { TypeParam(0.0f), TypeParam(0.0f), TypeParam(0.5f) } }; - math::Vector b{ { TypeParam(0.07f) }, { TypeParam(0.115f) }, { TypeParam(0.15f) } }; auto result = this->solver.Solve(a, b); @@ -67,7 +72,6 @@ TYPED_TEST(TestGaussianElimination, solve_back_substitutes_upper_triangular) { TypeParam(0.0f), TypeParam(0.4f), TypeParam(0.1f) }, { TypeParam(0.0f), TypeParam(0.0f), TypeParam(0.3f) } }; - math::Vector b{ { TypeParam(0.13f) }, { TypeParam(0.11f) }, { TypeParam(0.09f) } }; auto result = this->solver.Solve(a, b); @@ -77,21 +81,12 @@ TYPED_TEST(TestGaussianElimination, solve_back_substitutes_upper_triangular) EXPECT_NEAR(math::ToFloat(result.at(0, 0)), 0.1f, 0.02f); } -namespace -{ - class TestGaussianEliminationFloat - : public ::testing::Test - { - }; -} - TEST_F(TestGaussianEliminationFloat, solve_system_delegates_per_column) { math::SquareMatrix a{ { 0.5f, 0.2f }, { 0.1f, 0.4f } }; - math::Matrix b{ { 0.29f, 0.5f }, { 0.22f, 0.1f } diff --git a/numerical/solvers/test/TestLevinsonDurbin.cpp b/numerical/solvers/test/TestLevinsonDurbin.cpp index 6dd50095..aa2fd0eb 100644 --- a/numerical/solvers/test/TestLevinsonDurbin.cpp +++ b/numerical/solvers/test/TestLevinsonDurbin.cpp @@ -1,78 +1,38 @@ +#include "numerical/math/Tolerance.hpp" #include "numerical/solvers/LevinsonDurbin.hpp" #include namespace { - template - bool AreVectorsNear(const math::Vector& a, - const math::Vector& b, - float epsilon = 1e-2f) - { - for (size_t i = 0; i < 2; ++i) - { - if (std::abs(math::ToFloat(a.at(i, 0)) - math::ToFloat(b.at(i, 0))) >= epsilon) - return false; - } - return true; - } - - template - class LevinsonDurbinTest + class TestLevinsonDurbin : public ::testing::Test { protected: - static constexpr size_t N = 2; - using SolverType = solvers::LevinsonDurbin; - using MatrixType = math::Matrix; - using VectorType = math::Vector; - using ToeplitzType = math::ToeplitzMatrix; - - static T MakeValue(float f) - { - return T(f); - } - - VectorType MakeVector(float a, float b) - { - return VectorType{ - { MakeValue(a) }, - { MakeValue(b) } - }; - } - - MatrixType MakeMatrix(float a11, float a12, float a21, float a22) - { - return MatrixType{ - { MakeValue(a11), MakeValue(a12) }, - { MakeValue(a21), MakeValue(a22) } - }; - } + static constexpr std::size_t N = 2; + solvers::LevinsonDurbin solver; }; - - using TestTypes = ::testing::Types; - TYPED_TEST_SUITE(LevinsonDurbinTest, TestTypes); } -TYPED_TEST(LevinsonDurbinTest, SolveSymmetricToeplitz) +TEST_F(TestLevinsonDurbin, solve_symmetric_toeplitz_returns_correct_coefficients) { - typename TestFixture::SolverType solver; - - auto r = this->MakeVector(0.02f, 0.01f); - auto toeplitz = math::ToeplitzMatrix(r); + math::Vector r{ { 0.02f }, { 0.01f } }; + auto toeplitz = math::ToeplitzMatrix(r); auto A = toeplitz.ToFullMatrix(); - auto b = this->MakeVector(0.01f, 0.005f); + math::Vector b{ { 0.01f }, { 0.005f } }; + auto x = solver.Solve(A, b); - auto expected = this->MakeVector(0.5f, 0.0f); - EXPECT_TRUE(AreVectorsNear(x, expected)); + EXPECT_NEAR(x.at(0, 0), 0.5f, math::Tolerance()); + EXPECT_NEAR(x.at(1, 0), 0.0f, math::Tolerance()); } -TYPED_TEST(LevinsonDurbinTest, NonToeplitzError) +TEST_F(TestLevinsonDurbin, solve_asserts_on_non_toeplitz_matrix) { - typename TestFixture::SolverType solver; - - auto A = this->MakeMatrix(0.01f, 0.02f, 0.03f, 0.04f); - auto b = this->MakeVector(0.01f, 0.005f); + math::Matrix A{ + { 0.01f, 0.02f }, + { 0.03f, 0.04f } + }; + math::Vector b{ { 0.01f }, { 0.005f } }; EXPECT_DEATH(solver.Solve(A, b), ""); } diff --git a/simulator/controllers/PidController/application/PidSimulator.cpp b/simulator/controllers/PidController/application/PidSimulator.cpp index 0a3c5e16..5c92531a 100644 --- a/simulator/controllers/PidController/application/PidSimulator.cpp +++ b/simulator/controllers/PidController/application/PidSimulator.cpp @@ -225,16 +225,23 @@ namespace simulator::controllers std::span(openDen), 1.0f); + auto toStd = [](const auto& source, std::vector>& destination) + { + destination.clear(); + for (const auto& value : source) + destination.emplace_back(value.Real(), value.Imaginary()); + }; + RootLocusResult result; result.currentGain = rlResult.currentGain; result.gains.assign(rlResult.gains.begin(), rlResult.gains.end()); - result.openLoopPoles.assign(rlResult.openLoopPoles.begin(), rlResult.openLoopPoles.end()); - result.openLoopZeros.assign(rlResult.openLoopZeros.begin(), rlResult.openLoopZeros.end()); - result.closedLoopPoles.assign(rlResult.closedLoopPoles.begin(), rlResult.closedLoopPoles.end()); + toStd(rlResult.openLoopPoles, result.openLoopPoles); + toStd(rlResult.openLoopZeros, result.openLoopZeros); + toStd(rlResult.closedLoopPoles, result.closedLoopPoles); result.loci.resize(rlResult.activeBranches); for (std::size_t branch = 0; branch < rlResult.activeBranches; ++branch) - result.loci[branch].assign(rlResult.loci[branch].begin(), rlResult.loci[branch].end()); + toStd(rlResult.loci[branch], result.loci[branch]); return result; }