From 74a22df884f1fc440bf6bde65c91a98f4a4c2020 Mon Sep 17 00:00:00 2001 From: wmayer Date: Tue, 13 May 2025 13:07:44 +0200 Subject: [PATCH] Base: Simplify Base::Matrix4D As discussed in https://forum.freecad.org/viewtopic.php?t=65959 reduce code duplications --- src/Base/Matrix.cpp | 12 +-- src/Base/Matrix.h | 67 ++++----------- tests/src/Base/Matrix.cpp | 172 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 190 insertions(+), 61 deletions(-) diff --git a/src/Base/Matrix.cpp b/src/Base/Matrix.cpp index a619fc7354..8bd35ec1ab 100644 --- a/src/Base/Matrix.cpp +++ b/src/Base/Matrix.cpp @@ -277,13 +277,9 @@ void Matrix4D::rotLine(const Vector3d& vec, double fAngle) double fsin {}; // set all entries to "0" - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - clMA.dMtrx4D[i][j] = 0; - clMB.dMtrx4D[i][j] = 0; - clMC.dMtrx4D[i][j] = 0; - } - } + clMA.nullify(); + clMB.nullify(); + clMC.nullify(); // ** normalize the rotation axis clRotAxis.Normalize(); @@ -623,7 +619,7 @@ void Matrix4D::inverseOrthogonal() { Base::Vector3d vec(dMtrx4D[0][3], dMtrx4D[1][3], dMtrx4D[2][3]); transpose(); - vec = this->operator*(vec); + multVec(vec, vec); dMtrx4D[0][3] = -vec.x; dMtrx4D[3][0] = 0; dMtrx4D[1][3] = -vec.y; diff --git a/src/Base/Matrix.h b/src/Base/Matrix.h index 41a9d52df2..665c69376a 100644 --- a/src/Base/Matrix.h +++ b/src/Base/Matrix.h @@ -239,15 +239,8 @@ private: inline Matrix4D Matrix4D::operator+(const Matrix4D& mat) const { - Matrix4D clMat; - - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - clMat.dMtrx4D[i][j] = dMtrx4D[i][j] + mat[i][j]; - } - } - - return clMat; + Matrix4D newMat(*this); + return newMat += mat; } inline Matrix4D& Matrix4D::operator+=(const Matrix4D& mat) @@ -263,15 +256,8 @@ inline Matrix4D& Matrix4D::operator+=(const Matrix4D& mat) inline Matrix4D Matrix4D::operator-(const Matrix4D& mat) const { - Matrix4D clMat; - - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - clMat.dMtrx4D[i][j] = dMtrx4D[i][j] - mat[i][j]; - } - } - - return clMat; + Matrix4D newMat(*this); + return newMat -= mat; } inline Matrix4D& Matrix4D::operator-=(const Matrix4D& mat) @@ -287,19 +273,7 @@ inline Matrix4D& Matrix4D::operator-=(const Matrix4D& mat) inline Matrix4D& Matrix4D::operator*=(const Matrix4D& mat) { - Matrix4D clMat; - - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - clMat.dMtrx4D[i][j] = 0; - for (int e = 0; e < 4; e++) { - clMat.dMtrx4D[i][j] += dMtrx4D[i][e] * mat.dMtrx4D[e][j]; - } - } - } - - (*this) = clMat; - + (*this) = (*this) * mat; return *this; } @@ -336,23 +310,16 @@ inline Matrix4D& Matrix4D::operator=(const Matrix4D& mat) inline Vector3f Matrix4D::operator*(const Vector3f& vec) const { - // clang-format off - double sx = static_cast(vec.x); - double sy = static_cast(vec.y); - double sz = static_cast(vec.z); - return Vector3f(static_cast(dMtrx4D[0][0] * sx + dMtrx4D[0][1] * sy + dMtrx4D[0][2] * sz + dMtrx4D[0][3]), - static_cast(dMtrx4D[1][0] * sx + dMtrx4D[1][1] * sy + dMtrx4D[1][2] * sz + dMtrx4D[1][3]), - static_cast(dMtrx4D[2][0] * sx + dMtrx4D[2][1] * sy + dMtrx4D[2][2] * sz + dMtrx4D[2][3])); - // clang-format on + Vector3f dst; + multVec(vec, dst); + return dst; } inline Vector3d Matrix4D::operator*(const Vector3d& vec) const { - // clang-format off - return Vector3d((dMtrx4D[0][0] * vec.x + dMtrx4D[0][1] * vec.y + dMtrx4D[0][2] * vec.z + dMtrx4D[0][3]), - (dMtrx4D[1][0] * vec.x + dMtrx4D[1][1] * vec.y + dMtrx4D[1][2] * vec.z + dMtrx4D[1][3]), - (dMtrx4D[2][0] * vec.x + dMtrx4D[2][1] * vec.y + dMtrx4D[2][2] * vec.z + dMtrx4D[2][3])); - // clang-format on + Vector3d dst; + multVec(vec, dst); + return dst; } inline void Matrix4D::multVec(const Vector3d& src, Vector3d& dst) const @@ -379,14 +346,8 @@ inline void Matrix4D::multVec(const Vector3f& src, Vector3f& dst) const inline Matrix4D Matrix4D::operator*(double scalar) const { - Matrix4D matrix; - for (unsigned int i = 0; i < 4; i++) { - for (unsigned int j = 0; j < 4; j++) { - matrix.dMtrx4D[i][j] = dMtrx4D[i][j] * scalar; - } - } - - return matrix; + Matrix4D newMat(*this); + return newMat *= scalar; } inline Matrix4D& Matrix4D::operator*=(double scalar) @@ -421,7 +382,7 @@ inline bool Matrix4D::operator!=(const Matrix4D& mat) const inline Vector3f& operator*=(Vector3f& vec, const Matrix4D& mat) { - vec = mat * vec; + mat.multVec(vec, vec); return vec; } diff --git a/tests/src/Base/Matrix.cpp b/tests/src/Base/Matrix.cpp index 63b3bb0a01..6d1e280627 100644 --- a/tests/src/Base/Matrix.cpp +++ b/tests/src/Base/Matrix.cpp @@ -1,6 +1,7 @@ #include #include #include +#include // NOLINTBEGIN(cppcoreguidelines-*,readability-magic-numbers) // clang-format off @@ -155,6 +156,18 @@ TEST(Matrix, TestMultVec) Base::Vector3d vec2 {1, 1, 1}; mat.multVec(vec2, vec2); EXPECT_EQ(vec2, Base::Vector3d(6.0, 7.0, 8.0)); + + Base::Vector3f vec3{1.0F,1.0F,3.0F}; + vec3 = mat * vec3; + EXPECT_EQ(vec3, Base::Vector3f(12.0F, 9.0F, 12.0F)); + + Base::Vector3f vec4 {1.0F, 1.0F, 1.0F}; + mat.multVec(vec4, vec4); + EXPECT_EQ(vec4, Base::Vector3f(6.0F, 7.0F, 8.0F)); + + Base::Vector3f vec5 {1.0F, 1.0F, 1.0F}; + vec5 *= mat; + EXPECT_EQ(vec5, Base::Vector3f(6.0F, 7.0F, 8.0F)); } TEST(Matrix, TestMult) @@ -173,6 +186,7 @@ TEST(Matrix, TestMult) 10.0, 13.0, 13.0, 7.0, 0.0, 0.0, 0.0, 1.0}; EXPECT_EQ(mat3, mat4); + EXPECT_NE(mat3, Base::Matrix4D()); } TEST(Matrix, TestMultAssign) @@ -280,6 +294,21 @@ TEST(Matrix, TestHatOperator) EXPECT_EQ(mat1, mat2); } +TEST(Matrix, TestHatOperatorFloat) +{ + Base::Vector3f vec{1.0, 2.0, 3.0}; + + Base::Matrix4D mat1; + mat1.Hat(vec); + + Base::Matrix4D mat2{0.0F, -vec.z, vec.y, 0.0F, + vec.z, 0.0F, -vec.x, 0.0F, + -vec.y, vec.x, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 1.0F}; + + EXPECT_EQ(mat1, mat2); +} + TEST(Matrix, TestDyadic) { Base::Vector3d vec{1.0, 2.0, 3.0}; @@ -295,6 +324,21 @@ TEST(Matrix, TestDyadic) EXPECT_EQ(mat1, mat2); } +TEST(Matrix, TestDyadicFloat) +{ + Base::Vector3f vec{1.0F, 2.0F, 3.0F}; + + Base::Matrix4D mat1; + mat1.Outer(vec, vec); + + Base::Matrix4D mat2{1.0, 2.0, 3.0, 0.0, + 2.0, 4.0, 6.0, 0.0, + 3.0, 6.0, 9.0, 0.0, + 0.0, 0.0, 0.0, 1.0}; + + EXPECT_EQ(mat1, mat2); +} + TEST(Matrix, TestDecomposeScale) { Base::Matrix4D mat; @@ -333,6 +377,18 @@ TEST(Matrix, TestDecomposeMove) EXPECT_EQ(res[3], mat); } +TEST(Matrix, TestDecomposeMoveFloat) +{ + Base::Matrix4D mat; + mat.move(Base::Vector3f(1.0F, 2.0F, 3.0F)); + auto res = mat.decompose(); + + EXPECT_TRUE(res[0].isUnity()); + EXPECT_TRUE(res[1].isUnity()); + EXPECT_TRUE(res[2].isUnity()); + EXPECT_EQ(res[3], mat); +} + TEST(Matrix, TestDecompose) { Base::Matrix4D mat; @@ -401,5 +457,121 @@ TEST(Matrix, TestRotAxisFormula) //NOLINT EXPECT_DOUBLE_EQ(mat1[2][1], mat2[2][1]); EXPECT_DOUBLE_EQ(mat1[2][2], mat2[2][2]); } + +TEST(Matrix, TestTransform) +{ + Base::Matrix4D mat; + mat.rotZ(Base::toRadians(90.0)); + + Base::Matrix4D unity; + unity.transform(Base::Vector3d(10.0, 0.0, 0.0), mat); + + Base::Matrix4D mov{0.0, -1.0, 0.0, 10.0, + 1.0, 0.0, 0.0, -10.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0}; + EXPECT_EQ(unity, mov); +} + +TEST(Matrix, TestTransformFloat) +{ + Base::Matrix4D mat; + mat.rotZ(Base::toRadians(90.0)); + + Base::Matrix4D mat2; + mat2.transform(Base::Vector3f(10.0F, 0.0F, 0.0F), mat); + + Base::Matrix4D mov{0.0, -1.0, 0.0, 10.0, + 1.0, 0.0, 0.0, -10.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0}; + EXPECT_EQ(mat2, mov); +} + +TEST(Matrix, TestInverseOrthogonal) +{ + Base::Matrix4D mat; + mat.rotZ(Base::toRadians(90.0)); + + Base::Matrix4D mat2; + mat2.transform(Base::Vector3d(10.0, 0.0, 0.0), mat); + mat2.inverseOrthogonal(); + + Base::Matrix4D mov{0.0, 1.0, 0.0, 10.0, + -1.0, 0.0, 0.0, 10.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0}; + EXPECT_EQ(mat2, mov); +} + +TEST(Matrix, TestTranspose) +{ + Base::Matrix4D mat{1.0, 2.0, 3.0, 4.0, + 5.0, 6.0, 7.0, 8.0, + 9.0, 1.0, 2.0, 3.0, + 4.0, 5.0, 6.0, 7.0}; + + Base::Matrix4D trp{1.0, 5.0, 9.0, 4.0, + 2.0, 6.0, 1.0, 5.0, + 3.0, 7.0, 2.0, 6.0, + 4.0, 8.0, 3.0, 7.0}; + + mat.transpose(); + EXPECT_EQ(mat, trp); +} + +TEST(Matrix, TestTrace) +{ + Base::Matrix4D mat{1.0, 2.0, 3.0, 4.0, + 5.0, 6.0, 7.0, 8.0, + 9.0, 1.0, 2.0, 3.0, + 4.0, 5.0, 6.0, 7.0}; + EXPECT_DOUBLE_EQ(mat.trace(), 16.0); + EXPECT_DOUBLE_EQ(mat.trace3(), 9.0); +} + +TEST(Matrix, TestSetAndGetMatrix) +{ + Base::Matrix4D mat{1.0, 2.0, 3.0, 0.0, + 2.0, 4.0, 6.0, 0.0, + 3.0, 6.0, 9.0, 0.0, + 0.0, 0.0, 0.0, 1.0}; + + std::array values; + mat.getMatrix(values.data()); + Base::Matrix4D inp; + inp.setMatrix(values.data()); + + EXPECT_EQ(mat, inp); +} + +TEST(Matrix, TestSetAndGetGLMatrix) +{ + Base::Matrix4D mat{1.0, 2.0, 3.0, 0.0, + 2.0, 4.0, 6.0, 0.0, + 3.0, 6.0, 9.0, 0.0, + 0.0, 0.0, 0.0, 1.0}; + + std::array values; + mat.getGLMatrix(values.data()); + Base::Matrix4D inp; + inp.setGLMatrix(values.data()); + + EXPECT_EQ(mat, inp); +} + +TEST(Matrix, TestToAndFromString) +{ + Base::Matrix4D mat{1.0, 2.0, 3.0, 0.0, + 2.0, 4.0, 6.0, 0.0, + 3.0, 6.0, 9.0, 0.0, + 0.0, 0.0, 0.0, 1.0}; + + std::string str = mat.toString(); + Base::Matrix4D inp; + inp.fromString(str); + + EXPECT_EQ(mat, inp); +} // clang-format on // NOLINTEND(cppcoreguidelines-*,readability-magic-numbers)