[Type] Introduce function to compute the determinant of any square matrix

Sofa/framework/Type/src/sofa/type/Mat.h

@@ -21,14 +21,15 @@
 ******************************************************************************/
 #pragma once
 
+#include <sofa/type/Vec.h>
 #include <sofa/type/config.h>
-#include <sofa/type/fwd.h>
-
 #include <sofa/type/fixed_array.h>
-#include <sofa/type/Vec.h>
+#include <sofa/type/fwd.h>
 
-#include <iostream>
 #include <algorithm>
+#include <cstring>
+#include <iostream>
+#include <numeric>
 
 namespace // anonymous
 {
@@ -914,6 +915,63 @@ class MatNoInit : public Mat<L,C,real>
     }
 };
 
+template <sofa::Size N, typename real>
+real determinant(const Mat<N, N, real>& A)
+{
+    // Compute det(A) using Gaussian elimination with partial pivoting.
+    // Complexity: O(N^3), no heap allocations.
+
+    Mat<N, N, real> m = A; // local copy we can modify
+    real det = static_cast<real>(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<real>(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<real>(0);
+        }
+
+        det *= pivot;
+    }
+
+    return (sign > 0) ? det : -det;
+}
+
 /// Determinant of a 3x3 matrix.
 template<class real>
 constexpr real determinant(const Mat<3,3,real>& m) noexcept

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<SReal>(i + 1); // 1..12
+        M(i, i) = d;
+        expected *= d;
+    }
+
+    EXPECT_DOUBLE_EQ(sofa::type::determinant(M), expected);
+}
 
 TEST(MatTypesTest, fill)
 {