[Type] getsub for MatSym (#5800)

* [Type] getsub for MatSym

* add unit tests

Author: alxbilger

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

@@ -144,6 +144,32 @@ class MatSym : public VecNoInit<D * (D + 1) / 2, real>
         }
     }
 
+    template<Size D2>
+    constexpr void getsub(Size a, MatSym<D2, real>& m) const requires (D2 <= D)
+    {
+        assert(a + D2 <= D);
+        for (sofa::Size i = 0; i < D2; i++)
+        {
+            for (sofa::Size j = i; j < D2; j++)
+            {
+                m(i, j) = this->operator()(i + a, j + a);
+            }
+        }
+    }
+
+    template<Size L2, Size C2>
+    constexpr void getsub(Size L0, Size C0, Mat<L2, C2, real>& m) const requires (L2 <= D && C2 <= D)
+    {
+        assert(L0 + L2 <= D && C0 + C2 <= D);
+        for (Size i = 0; i < L2; i++)
+        {
+            for (Size j = 0; j < C2; j++)
+            {
+                m(i, j) = this->operator()(i + L0, j + C0);
+            }
+        }
+    }
+
     /// convert to Voigt notation (supported only for D == 2 and D == 3)
     template<sofa::Size TD = D, typename = std::enable_if_t<TD == 3 || TD == 2> >
     inline Vec<NumberStoredValues, real> getVoigt() const
@@ -584,14 +610,15 @@ bool invertMatrix(MatSym<2,real>& dest, const MatSym<2,real>& from)
 template<sofa::Size D,class real>
 std::ostream& operator<<(std::ostream& o, const MatSym<D,real>& m)
 {
-    o << '[' ;
+    o << '[';
     for(sofa::Size i=0; i<D; i++)
     {
         for(sofa::Size j=0; j<D; j++)
         {
-            o<<" "<<m(i,j);
+            o << " " << m(i, j);
         }
-        o<<" ,";
+        if (i != D-1)
+            o<<" ,";
     }
     o << ']';
     return o;

Sofa/framework/Type/test/MatSym_test.cpp

@@ -200,4 +200,46 @@ TYPED_TEST(MatSym3x3Test, elementAccessor)
     ASSERT_NO_THROW (this->elementAccessor());
 }
 
+
+
+TEST(MatSym_Test, getsub_3x3_2x2)
+{
+    // [ 1 2 4 ]
+    // [ 2 3 5 ]
+    // [ 4 5 6 ]
+    constexpr sofa::type::MatSym<3, double> mat33 {1.,2.,3.,4.,5.,6.};
+
+    {
+        sofa::type::Mat<2, 2, double> mat22;
+        mat33.getsub(0, 0, mat22);
+
+        EXPECT_DOUBLE_EQ(mat22(0, 0), 1_sreal);
+        EXPECT_DOUBLE_EQ(mat22(0, 1), 2_sreal);
+        EXPECT_DOUBLE_EQ(mat22(1, 0), 2_sreal);
+        EXPECT_DOUBLE_EQ(mat22(1, 1), 3_sreal);
+    }
+    {
+
+        sofa::type::Mat<2, 2, double> mat22;
+        mat33.getsub(1, 1, mat22);
+
+        EXPECT_DOUBLE_EQ(mat22(0, 0), 3_sreal);
+        EXPECT_DOUBLE_EQ(mat22(0, 1), 5_sreal);
+        EXPECT_DOUBLE_EQ(mat22(1, 0), 5_sreal);
+        EXPECT_DOUBLE_EQ(mat22(1, 1), 6_sreal);
+    }
+    {
+
+        sofa::type::MatSym<2, double> mat22;
+        mat33.getsub(1, mat22);
+
+        EXPECT_DOUBLE_EQ(mat22(0, 0), 3_sreal);
+        EXPECT_DOUBLE_EQ(mat22(0, 1), 5_sreal);
+        EXPECT_DOUBLE_EQ(mat22(1, 0), 5_sreal);
+        EXPECT_DOUBLE_EQ(mat22(1, 1), 6_sreal);
+    }
+}
+
+
+
 }