From 40228d9ef274c78cf84c9efd3714fd2961eaa4ff Mon Sep 17 00:00:00 2001 From: Jolbas <39026960+Jolbas@users.noreply.github.com> Date: Thu, 31 Aug 2023 14:50:36 +0200 Subject: [PATCH] Matrix.decompose() fix Fixes some errors when matrix has zero scale in one or two directions --- src/Base/Matrix.cpp | 124 +++++++++++++++++----------- src/Mod/Test/BaseTests.py | 14 ++-- tests/src/Base/CoordinateSystem.cpp | 12 +-- 3 files changed, 87 insertions(+), 63 deletions(-) diff --git a/src/Base/Matrix.cpp b/src/Base/Matrix.cpp index 5d7e45a9f3..7f0ac424fe 100644 --- a/src/Base/Matrix.cpp +++ b/src/Base/Matrix.cpp @@ -899,69 +899,93 @@ ScaleType Matrix4D::hasScale(double tol) const return ScaleType::NoScaling; } -std::array Matrix4D::decompose() const{ +std::array Matrix4D::decompose() const { // decompose the matrix to shear, scale, rotation and move // so that matrix = move * rotation * scale * shear // return an array of matrices - std::array SZRT = { - Matrix4D(*this), Matrix4D(), Matrix4D(), Matrix4D() + Matrix4D moveMatrix; + Matrix4D rotationMatrix; + Matrix4D scaleMatrix; + Matrix4D residualMatrix(*this); + // extract transform + moveMatrix.move(residualMatrix.getCol(3)); + residualMatrix.setCol(3, Vector3d()); + // find and extract rotation + int prim_dir = -1; + std::array dirs = { + Vector3d(1., 0., 0.),Vector3d(0., 1., 0.),Vector3d(0., 0., 1.) }; - SZRT[3].move(SZRT[0].getCol(3)); - SZRT[0].setCol(3, Vector3d()); - // find rotation - Vector3d xDir = SZRT[0].getCol(0); - if (xDir.IsNull()) xDir = Vector3d(1.,0.,0.); - Vector3d yDir = SZRT[0].getCol(1); - if (yDir.IsNull()) yDir = Vector3d(0.,1.,0.); - Vector3d zDir = xDir.Cross(yDir); - // check for parallel directions - if (zDir.IsNull()) { - zDir = SZRT[0].getCol(2); - if (zDir.IsNull()) zDir = Vector3d(0.,0.,1.); - yDir = zDir.Cross(xDir); - if (yDir.IsNull()) { - zDir = xDir.Cross(xDir.y ? Vector3d(1.,0.,0.) : Vector3d(0.,1.,0.)); - yDir = zDir.Cross(xDir); + int i; + for (i = 0; i < 3; i++) { + if (residualMatrix.getCol(i).IsNull()) { + continue; + } + if (prim_dir < 0) { + dirs[i] = residualMatrix.getCol(i); + dirs[i].Normalize(); + prim_dir = i; + continue; + } else { + Vector3d cross = dirs[prim_dir].Cross(residualMatrix.getCol(i)); + if (cross.IsNull()) { + continue; + } + cross.Normalize(); + int last_dir = 3-i-prim_dir; + if (i - prim_dir == 1) { + dirs[last_dir] = cross; + dirs[i] = cross.Cross(dirs[prim_dir]); + } else { + dirs[last_dir] = -cross; + dirs[i] = dirs[prim_dir].Cross(-cross); + } + prim_dir = -2; // done + break; } - } else { - yDir = zDir.Cross(xDir); } - xDir.Normalize(); - yDir.Normalize(); - zDir.Normalize(); - - SZRT[2].setCol(0, xDir); - SZRT[2].setCol(1, yDir); - SZRT[2].setCol(2, zDir); - SZRT[2].inverse(); - SZRT[0] = SZRT[2] * SZRT[0]; + if (prim_dir >= 0) { + // handle case with only one valid direction + Vector3d cross = dirs[prim_dir].Cross(Vector3d(0., 0., 1.)); + if (cross.IsNull()) { + cross = dirs[prim_dir].Cross(Vector3d(0., 1., 0.)); + } + dirs[(prim_dir+1)%3] = cross; + dirs[(prim_dir+2)%3] = dirs[prim_dir].Cross(cross); + } + rotationMatrix.setCol(0, dirs[0]); + rotationMatrix.setCol(1, dirs[1]); + rotationMatrix.setCol(2, dirs[2]); + rotationMatrix.inverseGauss(); + residualMatrix = rotationMatrix * residualMatrix; // To keep signs of the scale factors equal - if (SZRT[0].determinant() < 0) { - SZRT[2].rotZ(D_PI); - SZRT[0].rotZ(D_PI); + if (residualMatrix.determinant() < 0) { + rotationMatrix.rotZ(D_PI); + residualMatrix.rotZ(D_PI); } - SZRT[2].inverse(); + rotationMatrix.inverseGauss(); // extract scale - double xScale = SZRT[0].dMtrx4D[0][0]; - double yScale = SZRT[0].dMtrx4D[1][1]; - double zScale = SZRT[0].dMtrx4D[2][2]; - SZRT[1].dMtrx4D[0][0] = xScale; - SZRT[1].dMtrx4D[1][1] = yScale; - SZRT[1].dMtrx4D[2][2] = zScale; + double xScale = residualMatrix.dMtrx4D[0][0]; + double yScale = residualMatrix.dMtrx4D[1][1]; + double zScale = residualMatrix.dMtrx4D[2][2]; + scaleMatrix.dMtrx4D[0][0] = xScale; + scaleMatrix.dMtrx4D[1][1] = yScale; + scaleMatrix.dMtrx4D[2][2] = zScale; // The remaining shear - SZRT[0].scale(xScale ? 1.0 / xScale : 1.0, yScale ? 1.0 / yScale : 1.0, zScale ? 1.0 / zScale : 1.0); + residualMatrix.scale(xScale ? 1.0 / xScale : 1.0, yScale ? 1.0 / yScale : 1.0, zScale ? 1.0 / zScale : 1.0); // Restore trace in shear matrix - SZRT[0].setTrace(Vector3d(1.0, 1.0, 1.0)); + residualMatrix.setDiagonal(Vector3d(1.0, 1.0, 1.0)); // Remove values close to zero - for (int i = 0; i < 3; i++) { - if (std::abs(SZRT[1].dMtrx4D[i][i]) < 1e-15) - SZRT[1].dMtrx4D[i][i] = 0.0; + for (i = 0; i < 3; i++) { + if (std::abs(scaleMatrix.dMtrx4D[i][i]) < 1e-15) + scaleMatrix.dMtrx4D[i][i] = 0.0; for (int j = 0; j < 3; j++) { - if (std::abs(SZRT[0].dMtrx4D[i][j]) < 1e-15) - SZRT[0].dMtrx4D[i][j] = 0.0; - if (std::abs(SZRT[2].dMtrx4D[i][j]) < 1e-15) - SZRT[2].dMtrx4D[i][j] = 0.0; + if (std::abs(residualMatrix.dMtrx4D[i][j]) < 1e-15) + residualMatrix.dMtrx4D[i][j] = 0.0; + if (std::abs(rotationMatrix.dMtrx4D[i][j]) < 1e-15) + rotationMatrix.dMtrx4D[i][j] = 0.0; } } - return SZRT; + return std::array{ + residualMatrix, scaleMatrix, rotationMatrix, moveMatrix + }; } diff --git a/src/Mod/Test/BaseTests.py b/src/Mod/Test/BaseTests.py index 0e9d72db63..3e32767447 100644 --- a/src/Mod/Test/BaseTests.py +++ b/src/Mod/Test/BaseTests.py @@ -266,20 +266,20 @@ class AlgebraTestCase(unittest.TestCase): self.assertNotEqual(m2, m4 * m3, "Wrong multiplication order") def testRotationFromMatrix(self): - rot = FreeCAD.Rotation(45,30,0) + rot = FreeCAD.Rotation(45, 30, 0) m_r = rot.toMatrix() - m_r.move(5,6,7) + m_r.move(5, 6, 7) m_s = FreeCAD.Matrix() - m_s.scale(1,1,3) - m_s.move(7,3,2) - target_rot = FreeCAD.Rotation(45,60,0) + m_s.scale(1, 1, 3) + m_s.move(7, 3, 2) + target_rot = FreeCAD.Rotation(45, 60, 0) err = "Non uniform scale has wrong affect non orthogonal rotation" self.assertTrue(FreeCAD.Rotation(m_s * m_r).isSame(target_rot, 1e-12), err) err = "Right multiplication with non uniform scale must not affect rotation" - self.assertTrue(FreeCAD.Rotation(m_r * m_s).isSame(rot,1e-12), err) + self.assertTrue(FreeCAD.Rotation(m_r * m_s).isSame(rot, 1e-12), err) m_r.scale(-2) err = "Uniform scale must not affect rotation" - self.assertTrue(FreeCAD.Rotation(m_r).isSame(rot,1e-12), err) + self.assertTrue(FreeCAD.Rotation(m_r).isSame(rot, 1e-12), err) def testRotation(self): r = FreeCAD.Rotation(1, 0, 0, 0) # 180 deg around (1,0,0) diff --git a/tests/src/Base/CoordinateSystem.cpp b/tests/src/Base/CoordinateSystem.cpp index 3a22aca6f0..1706ffbcc0 100644 --- a/tests/src/Base/CoordinateSystem.cpp +++ b/tests/src/Base/CoordinateSystem.cpp @@ -73,10 +73,10 @@ TEST(CoordinateSystem, TestTransformPlacement) csT.transform(plm); Base::Placement dis = cs.displacement(csT); - EXPECT_EQ(plm, dis); + EXPECT_EQ(plm.isSame(dis, 1e-15), true); Base::Placement disT = csT.displacement(cs); - EXPECT_EQ(plm.inverse(), disT); + EXPECT_EQ(plm.inverse().isSame(disT, 1e-15), true); } TEST(CoordinateSystem, TestMultTransformPlacement) @@ -106,10 +106,10 @@ TEST(CoordinateSystem, TestTransformRotation) csT.transform(rot); Base::Placement dis = cs.displacement(csT); - EXPECT_EQ(rot, dis.getRotation()); + EXPECT_EQ(rot.isSame(dis.getRotation(), 1e-15), true); Base::Placement disT = csT.displacement(cs); - EXPECT_EQ(rot.inverse(), disT.getRotation()); + EXPECT_EQ(rot.inverse().isSame(disT.getRotation(), 1e-15), true); } TEST(CoordinateSystem, TestTransformPoint) @@ -141,10 +141,10 @@ TEST(CoordinateSystem, TestSetPlacement) csT.setPlacement(plm); Base::Placement dis = cs.displacement(csT); - EXPECT_EQ(plm, dis); + EXPECT_EQ(plm.isSame(dis, 1e-15), true); Base::Placement disT = csT.displacement(cs); - EXPECT_EQ(plm.inverse(), disT); + EXPECT_EQ(plm.inverse().isSame(disT, 1e-15), true); } // NOLINTEND