diff --git a/Sofa/framework/Type/src/sofa/type/Mat.h b/Sofa/framework/Type/src/sofa/type/Mat.h index 8a82d419889..3fa20369ff6 100644 --- a/Sofa/framework/Type/src/sofa/type/Mat.h +++ b/Sofa/framework/Type/src/sofa/type/Mat.h @@ -21,14 +21,15 @@ ******************************************************************************/ #pragma once +#include #include -#include - #include -#include +#include -#include #include +#include +#include +#include namespace // anonymous { @@ -914,6 +915,63 @@ class MatNoInit : public Mat } }; +template +real determinant(const Mat& A) +{ + // Compute det(A) using Gaussian elimination with partial pivoting. + // Complexity: O(N^3), no heap allocations. + + Mat m = A; // local copy we can modify + real det = static_cast(1); + int sign = 1; + + for (sofa::Size k = 0; k < N; ++k) + { + // Find pivot row (max abs in column k, rows k..N-1) + sofa::Size pivotRow = k; + real pivotAbs = rabs(m(k, k)); + + for (sofa::Size i = k + 1; i < N; ++i) + { + const real vAbs = rabs(m(i, k)); + if (vAbs > pivotAbs) + { + pivotAbs = vAbs; + pivotRow = i; + } + } + + if (equalsZero(pivotAbs)) + { + return static_cast(0); + } + + if (pivotRow != k) + { + std::swap(m(pivotRow), m(k)); // swap rows (Vec/C-line swap) + sign = -sign; + } + + const real pivot = m(k, k); + + // Eliminate entries below pivot + for (sofa::Size i = k + 1; i < N; ++i) + { + const real factor = m(i, k) / pivot; + // Start at k+1 since m(i,k) becomes 0 + for (sofa::Size j = k + 1; j < N; ++j) + { + m(i, j) -= factor * m(k, j); + } + m(i, k) = static_cast(0); + } + + det *= pivot; + } + + return (sign > 0) ? det : -det; +} + /// Determinant of a 3x3 matrix. template constexpr real determinant(const Mat<3,3,real>& m) noexcept diff --git a/Sofa/framework/Type/test/MatTypes_test.cpp b/Sofa/framework/Type/test/MatTypes_test.cpp index a763777bb94..74e80626675 100644 --- a/Sofa/framework/Type/test/MatTypes_test.cpp +++ b/Sofa/framework/Type/test/MatTypes_test.cpp @@ -517,14 +517,36 @@ TEST(MatTypesTest, determinant3x3) EXPECT_DOUBLE_EQ(sofa::type::determinant(sofa::type::Mat<3,3,SReal>{{1, 1, 0}, {1, 0, 1}, {0, 1, 1}}), -2_sreal); } -//TEST(MatTypesTest, determinantRectangular) -//{ -// const Mat<2,3,double> m232{{1., 2., 3.}, {4., 5., 6.}}; -// EXPECT_DOUBLE_EQ(sofa::type::determinant(m232), -3.); -// -// const Mat<3,2,double> m322{{1., 2.}, {3., 4.}, {5., 6.}}; -// EXPECT_DOUBLE_EQ(sofa::type::determinant(m322), -12.); -//} +TEST(MatTypesTest, determinant4x4) +{ + EXPECT_DOUBLE_EQ(sofa::type::determinant(sofa::type::Mat<4,4,SReal>{{1,0,0,0}, {0,1,0,0}, {0,0,1,0}, {0,0,0,1}}), 1_sreal); + EXPECT_DOUBLE_EQ(sofa::type::determinant(sofa::type::Mat<4,4,SReal>{{2,0,0,0}, {0,3,0,0}, {0,0,4,0}, {0,0,0,5}}), 120_sreal); + EXPECT_DOUBLE_EQ(sofa::type::determinant(sofa::type::Mat<4,4,SReal>{{0,1,0,0}, {1,0,0,0}, {0,0,1,0}, {0,0,0,1}}), -1_sreal); + EXPECT_DOUBLE_EQ(sofa::type::determinant(sofa::type::Mat<4,4,SReal>{{1,0,0,0}, {0,2,0,0}, {0,0,3,0}, {0,0,0,4}}), 24_sreal); + EXPECT_DOUBLE_EQ(sofa::type::determinant(sofa::type::Mat<4,4,SReal>{{1,3,5,9}, {1,3,1,7}, {4,3,9,7}, {5,2,0,9}}), -376_sreal); +} + +TEST(MatTypesTest, determinant12x12) +{ + using Mat12 = sofa::type::Mat<12, 12, SReal>; + + // Identity: det = 1 + EXPECT_DOUBLE_EQ(sofa::type::determinant(Mat12::Identity()), 1_sreal); + + // Diagonal matrix: det = product of diagonal entries + Mat12 M; + M.clear(); + + SReal expected = 1_sreal; + for (sofa::Size i = 0; i < 12; ++i) + { + const SReal d = static_cast(i + 1); // 1..12 + M(i, i) = d; + expected *= d; + } + + EXPECT_DOUBLE_EQ(sofa::type::determinant(M), expected); +} TEST(MatTypesTest, fill) {