diff --git a/Sofa/framework/Type/src/sofa/type/Mat.h b/Sofa/framework/Type/src/sofa/type/Mat.h
index 3067a701727..8a82d419889 100644
--- a/Sofa/framework/Type/src/sofa/type/Mat.h
+++ b/Sofa/framework/Type/src/sofa/type/Mat.h
@@ -528,17 +528,17 @@ class Mat
 
     bool isDiagonal() const noexcept
     {
-        for (Size i=0; i<L; i++)
+        for (Size i=0; i<L; ++i)
         {
-            for (Size j=0; j<i-1; j++)
-                if( rabs( (*this)(i,j) ) > EQUALITY_THRESHOLD ) return false;
-            for (Size j=i+1; j<C; j++)
+            for (Size j=0; j<C; ++j)
+            {
+                if (j == i) continue;
                 if( rabs( (*this)(i,j) ) > EQUALITY_THRESHOLD ) return false;
+            }
         }
         return true;
     }
 
-
     /// @}
 
     // LINEAR ALGEBRA
diff --git a/Sofa/framework/Type/test/MatTypes_test.cpp b/Sofa/framework/Type/test/MatTypes_test.cpp
index 312cf438592..4fd3decb4d6 100644
--- a/Sofa/framework/Type/test/MatTypes_test.cpp
+++ b/Sofa/framework/Type/test/MatTypes_test.cpp
@@ -422,7 +422,7 @@ TEST(MatTypesTest, determinant1x1)
 {
     EXPECT_DOUBLE_EQ(sofa::type::determinant(sofa::type::Mat<1,1,SReal>::Identity()), 1_sreal);
 
-    sofa::type::Mat<1,1,SReal> a{{{4.}}};
+    sofa::type::Mat<1,1,SReal> a{{4.}};
     EXPECT_DOUBLE_EQ(sofa::type::determinant(a), 4_sreal);
 }
 
@@ -443,3 +443,136 @@ 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, 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, fill)
+{
+    Matrix3 M;
+    M.fill(5.0);
+    for (sofa::Size i = 0; i < 3; ++i)
+        for (sofa::Size j = 0; j < 3; ++j)
+            EXPECT_EQ(M(i,j), 5.0);
+}
+
+TEST(MatTypesTest, clear)
+{
+    Matrix3 M;
+    M.fill(1.0);
+    M.clear();
+    for (sofa::Size i = 0; i < 3; ++i)
+        for (sofa::Size j = 0; j < 3; ++j)
+            EXPECT_EQ(M(i,j), 0.0);
+}
+
+TEST(MatTypesTest, col)
+{
+    const Matrix3 M(Matrix3::Line(1., 2., 3.), Matrix3::Line(4., 5., 6.), Matrix3::Line(7., 8., 9.));
+    const auto c = M.col(1);
+    EXPECT_EQ(c[0], 2.);
+    EXPECT_EQ(c[1], 5.);
+    EXPECT_EQ(c[2], 8.);
+}
+
+TEST(MatTypesTest, operatorsEqual)
+{
+    const Matrix3 A(Matrix3::Line(1., 2., 3.), Matrix3::Line(4., 5., 6.), Matrix3::Line(7., 8., 9.));
+    const Matrix3 B(A);
+    EXPECT_TRUE(A == B);
+    EXPECT_FALSE(A != B);
+
+    const Matrix3 C(Matrix3::Line(1., 2., 3.), Matrix3::Line(4., 5., 6.), Matrix3::Line(7., 8., 10.));
+    EXPECT_FALSE(A == C);
+    EXPECT_TRUE(A != C);
+}
+
+TEST(MatTypesTest, isSymmetric)
+{
+    Matrix3 A;
+    A.identity();
+    EXPECT_TRUE(A.isSymmetric());
+
+    Matrix3 B(Matrix3::Line(1., 2., 3.), Matrix3::Line(2., 5., 6.), Matrix3::Line(3., 6., 9.));
+    EXPECT_TRUE(B.isSymmetric());
+
+    Matrix3 C(Matrix3::Line(1., 2., 3.), Matrix3::Line(4., 5., 6.), Matrix3::Line(7., 8., 9.));
+    EXPECT_FALSE(C.isSymmetric());
+}
+
+TEST(MatTypesTest, isDiagonal)
+{
+    Matrix3 A;
+    A.identity();
+    EXPECT_TRUE(A.isDiagonal());
+
+    Matrix3 B{{1., 0., 0.}, {0., 2., 0.}, {0., 0., 3.}};
+    EXPECT_TRUE(B.isDiagonal());
+
+    Matrix3 C(Matrix3::Line(1., 2., 0.), Matrix3::Line(0., 5., 0.), Matrix3::Line(0., 0., 9.));
+    EXPECT_FALSE(C.isDiagonal());
+
+    Mat<2,4,SReal> M;
+    for (sofa::Size i = 0; i < 2; ++i)
+        for (sofa::Size j = 0; j < 4; ++j)
+            M(i,j) = (i == j && i < 2) ? SReal{1} : SReal{0};
+    EXPECT_TRUE(M.isDiagonal());
+
+    Mat<3,2,SReal> N;
+    for (sofa::Size i = 0; i < 3; ++i)
+        for (sofa::Size j = 0; j < 2; ++j)
+            N(i,j) = (i == j && i < 2) ? SReal{1} : SReal{0};
+    EXPECT_TRUE(N.isDiagonal());
+}
+
+TEST(MatTypesTest, multDiagonal)
+{
+    const Matrix3 A(Matrix3::Line(1., 2., 3.), Matrix3::Line(4., 5., 6.), Matrix3::Line(7., 8., 9.));
+    const Vec3 d(2., 3., 4.);
+    const auto R = A.multDiagonal(d);
+    EXPECT_EQ(R(0,0), 2.); EXPECT_EQ(R(0,1), 6.); EXPECT_EQ(R(0,2), 12.);
+    EXPECT_EQ(R(1,0), 8.); EXPECT_EQ(R(1,1),15.); EXPECT_EQ(R(1,2),24.);
+}
+
+TEST(MatTypesTest, plusMinusTransposed)
+{
+    const Matrix3 A(Matrix3::Line(1., 2., 3.), Matrix3::Line(4., 5., 6.), Matrix3::Line(7., 8., 9.));
+    const auto AT = A.transposed();
+    const Matrix3 B = A.plusTransposed(A);
+    EXPECT_EQ(B, A + AT);
+
+    const Matrix3 C = A.minusTransposed(A);
+    EXPECT_EQ(C, A - AT);
+}
+
+TEST(MatTypesTest, addSubTransposed)
+{
+    Matrix3 M;
+    M.identity();
+    const Matrix3 A(Matrix3::Line(1., 2., 3.), Matrix3::Line(4., 5., 6.), Matrix3::Line(7., 8., 9.));
+    M.addTransposed(A);
+    EXPECT_EQ(M, Matrix3::Identity() + A.transposed());
+}
+
+TEST(MatTypesTest, symmetrize)
+{
+    Matrix3 A(Matrix3::Line(1., 2., 3.), Matrix3::Line(4., 5., 6.), Matrix3::Line(7., 8., 9.));
+    A.symmetrize();
+    EXPECT_EQ(A, A.transposed());
+}
+
+TEST(MatTypesTest, transformVec)
+{
+    const Matrix4 T = Matrix4::transformTranslation(Vec3(1., 2., 3.));
+    const Vec3 v(1., 0., 0.);
+    const auto result = T.transform(v);
+    EXPECT_EQ(result[0], 2.);
+    EXPECT_EQ(result[1], 2.);
+    EXPECT_EQ(result[2], 3.);
+}