From 2a4b9120ee1c77354a1feb97db102822401dec3c Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Wed, 21 Jan 2026 10:20:39 +0100 Subject: [PATCH 1/4] [Type] Introduce function to compute the determinant of any square matrix --- Sofa/framework/Type/src/sofa/type/Mat.h | 21 +++++++++++++++++++++ Sofa/framework/Type/test/MatTypes_test.cpp | 9 +++++++++ 2 files changed, 30 insertions(+) diff --git a/Sofa/framework/Type/src/sofa/type/Mat.h b/Sofa/framework/Type/src/sofa/type/Mat.h index 651fff3f146..7ece94d3fee 100644 --- a/Sofa/framework/Type/src/sofa/type/Mat.h +++ b/Sofa/framework/Type/src/sofa/type/Mat.h @@ -914,6 +914,27 @@ class MatNoInit : public Mat } }; +template +real determinant(const Mat& mat) +{ + real det = 0; + for (size_t p = 0; p < N; ++p) + { + Mat submat; + for (size_t i = 1; i < N; ++i) + { + size_t colIndex = 0; + for (size_t j = 0; j < N; ++j) + { + if (j == p) continue; + submat(i - 1, colIndex++) = mat(i, j); + } + } + det += ((p % 2 == 0) ? 1 : -1) * mat(0, p) * determinant(submat); + } + return 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 312cf438592..e1515a4f7f5 100644 --- a/Sofa/framework/Type/test/MatTypes_test.cpp +++ b/Sofa/framework/Type/test/MatTypes_test.cpp @@ -443,3 +443,12 @@ TEST(MatTypesTest, determinant3x3) EXPECT_DOUBLE_EQ(sofa::type::determinant(sofa::type::Mat<3,3,SReal>{{0, 1, 0}, {0, 0, 1}, {1, 0, 0}}), 1_sreal); 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, 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); +} From f655f469fffd5abcf5c76c30388129557c73fd96 Mon Sep 17 00:00:00 2001 From: Paul Baksic <30337881+bakpaul@users.noreply.github.com> Date: Thu, 5 Feb 2026 14:04:44 +0100 Subject: [PATCH 2/4] Make the funciton scalable and usable with sparse matrix (#9) * Add a version without copy of the matrix content, also works with sparse matrix * Remove unwanted header --- Sofa/framework/Type/src/sofa/type/Mat.h | 42 ++++++++++++++----------- 1 file changed, 24 insertions(+), 18 deletions(-) diff --git a/Sofa/framework/Type/src/sofa/type/Mat.h b/Sofa/framework/Type/src/sofa/type/Mat.h index 7ece94d3fee..85b99725ead 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 { @@ -915,26 +916,31 @@ class MatNoInit : public Mat }; template -real determinant(const Mat& mat) +real recDeterminant(const Mat& mat,const Index& currRow, const std::vector& cols ) { - real det = 0; - for (size_t p = 0; p < N; ++p) + if (currRow == N-1) + return mat(currRow, cols[0]); + + real det = 0.0; + for (size_t c = 0; c < cols.size(); ++c) { - Mat submat; - for (size_t i = 1; i < N; ++i) - { - size_t colIndex = 0; - for (size_t j = 0; j < N; ++j) - { - if (j == p) continue; - submat(i - 1, colIndex++) = mat(i, j); - } - } - det += ((p % 2 == 0) ? 1 : -1) * mat(0, p) * determinant(submat); + std::vector newCols(cols.size() -1); + memcpy(&newCols[0], cols.data(), c * sizeof(Index)); + memcpy(&newCols[c], &cols[c+1], (cols.size() - 1 - c) * sizeof(Index)); + det += ((c % 2 == 0) ? 1 : -1) * mat(currRow, cols[c]) * recDeterminant(mat, currRow + 1, newCols ); } return det; } + +template +real determinant(const Mat& mat) +{ + std::vector initcols(N); + std::iota(initcols.begin(), initcols.end(), real(0)); + return recDeterminant(mat, 0, initcols); +} + /// Determinant of a 3x3 matrix. template constexpr real determinant(const Mat<3,3,real>& m) noexcept From 5e64c274fe04df7748e322bcf8c4419d96149175 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Thu, 5 Feb 2026 14:53:53 +0100 Subject: [PATCH 3/4] [Type] Add unit test for determinant of 12x12 matrices --- Sofa/framework/Type/test/MatTypes_test.cpp | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/Sofa/framework/Type/test/MatTypes_test.cpp b/Sofa/framework/Type/test/MatTypes_test.cpp index e1515a4f7f5..53ff157ad0a 100644 --- a/Sofa/framework/Type/test/MatTypes_test.cpp +++ b/Sofa/framework/Type/test/MatTypes_test.cpp @@ -452,3 +452,25 @@ TEST(MatTypesTest, determinant4x4) 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); +} From c60201d109a83f089555396f75e3121bb8f8e841 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Thu, 5 Feb 2026 15:04:15 +0100 Subject: [PATCH 4/4] [Type] Refactor determinant computation with Gaussian elimination --- Sofa/framework/Type/src/sofa/type/Mat.h | 67 ++++++++++++++++++------- 1 file changed, 49 insertions(+), 18 deletions(-) diff --git a/Sofa/framework/Type/src/sofa/type/Mat.h b/Sofa/framework/Type/src/sofa/type/Mat.h index 85b99725ead..868e84b5c6c 100644 --- a/Sofa/framework/Type/src/sofa/type/Mat.h +++ b/Sofa/framework/Type/src/sofa/type/Mat.h @@ -916,29 +916,60 @@ class MatNoInit : public Mat }; template -real recDeterminant(const Mat& mat,const Index& currRow, const std::vector& cols ) +real determinant(const Mat& A) { - if (currRow == N-1) - return mat(currRow, cols[0]); + // Compute det(A) using Gaussian elimination with partial pivoting. + // Complexity: O(N^3), no heap allocations. - real det = 0.0; - for (size_t c = 0; c < cols.size(); ++c) + Mat m = A; // local copy we can modify + real det = static_cast(1); + int sign = 1; + + for (sofa::Size k = 0; k < N; ++k) { - std::vector newCols(cols.size() -1); - memcpy(&newCols[0], cols.data(), c * sizeof(Index)); - memcpy(&newCols[c], &cols[c+1], (cols.size() - 1 - c) * sizeof(Index)); - det += ((c % 2 == 0) ? 1 : -1) * mat(currRow, cols[c]) * recDeterminant(mat, currRow + 1, newCols ); - } - return det; -} + // 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); + } -template -real determinant(const Mat& mat) -{ - std::vector initcols(N); - std::iota(initcols.begin(), initcols.end(), real(0)); - return recDeterminant(mat, 0, initcols); + 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.