diff --git a/src/Base/Matrix.cpp b/src/Base/Matrix.cpp index 040ff8dc6a..5d7e45a9f3 100644 --- a/src/Base/Matrix.cpp +++ b/src/Base/Matrix.cpp @@ -26,6 +26,7 @@ # include # include #endif +# include #include "Matrix.h" #include "Converter.h" @@ -897,3 +898,70 @@ ScaleType Matrix4D::hasScale(double tol) const return ScaleType::NoScaling; } + +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() + }; + 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); + } + } 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]; + // To keep signs of the scale factors equal + if (SZRT[0].determinant() < 0) { + SZRT[2].rotZ(D_PI); + SZRT[0].rotZ(D_PI); + } + SZRT[2].inverse(); + // 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; + // The remaining shear + SZRT[0].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)); + // 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 (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; + } + } + return SZRT; +} diff --git a/src/Base/Matrix.h b/src/Base/Matrix.h index 5569dea636..163dee1a19 100644 --- a/src/Base/Matrix.h +++ b/src/Base/Matrix.h @@ -25,6 +25,7 @@ #define BASE_MATRIX_H #include +#include #include "Vector3D.h" #ifndef FC_GLOBAL_H @@ -175,6 +176,8 @@ public: { scale(Vector3d(scalexyz, scalexyz, scalexyz)); } /// Check for scaling factor ScaleType hasScale(double tol=0.0) const; + /// Decompose matrix into pure shear, scale, rotation and move + std::array decompose() const; /// Rotate around the X axis (in transformed space) for the given value in radians void rotX (double fAngle); /// Rotate around the Y axis (in transformed space) for the given value in radians diff --git a/src/Base/MatrixPy.xml b/src/Base/MatrixPy.xml index 4f06bf99fa..c2dca83498 100644 --- a/src/Base/MatrixPy.xml +++ b/src/Base/MatrixPy.xml @@ -97,6 +97,13 @@ if it's not a scale matrix. tol : float + + + decompose() -> Base.Matrix, Base.Matrix, Base.Matrix, Base.Matrix\n +Return a tuple of matrices representing shear, scale, rotation and move. +So that matrix = move * rotation * scale * shear. + + nullify() -> None diff --git a/src/Base/MatrixPyImp.cpp b/src/Base/MatrixPyImp.cpp index 505b77ab3e..8bacece2a8 100644 --- a/src/Base/MatrixPyImp.cpp +++ b/src/Base/MatrixPyImp.cpp @@ -22,6 +22,7 @@ #include "PreCompiled.h" +//#include // inclusion of the generated files (generated out of MatrixPy.xml) #include "RotationPy.h" @@ -351,6 +352,18 @@ PyObject* MatrixPy::hasScale(PyObject * args) Py::Module mod("FreeCAD"); return Py::new_reference_to(mod.callMemberFunction("ScaleType", Py::TupleN(Py::Int(static_cast(type))))); } +PyObject* MatrixPy::decompose(PyObject * args) +{ + if (!PyArg_ParseTuple(args, "")) + return nullptr; + + auto ms = getMatrixPtr()->decompose(); + Py::Tuple tuple(4); + for (int i=0; i<4; i++) { + tuple.setItem(i, Py::Matrix(ms[i])); + } + return Py::new_reference_to(tuple); +} PyObject* MatrixPy::nullify() { diff --git a/src/Base/Rotation.cpp b/src/Base/Rotation.cpp index 204885147d..e7a6d40890 100644 --- a/src/Base/Rotation.cpp +++ b/src/Base/Rotation.cpp @@ -22,6 +22,7 @@ #include "PreCompiled.h" +#include #include #include "Base/Exception.h" @@ -220,28 +221,8 @@ void Rotation::setValue(const double q[4]) void Rotation::setValue(const Matrix4D & m) { - - auto type = m.hasScale(); - if (type == Base::ScaleType::Other) { - THROWM(Base::ValueError, "setValue(matrix): Could not determine the rotation."); - } - Matrix4D mc(m); - if (type != Base::ScaleType::NoScaling) { - mc.setCol(3, Vector3d(0.0, 0.0, 0.0)); - if (type == Base::ScaleType::NonUniformRight) { - mc.transpose(); - } - double sx = 1.0 / mc.getRow(0).Length(); - double sy = 1.0 / mc.getRow(1).Length(); - double sz = 1.0 / mc.getRow(2).Length(); - mc.scale(sx, sy, sz); - if (type == Base::ScaleType::NonUniformRight) { - mc.transpose(); - } - if (mc.determinant3() < 0.0) { - mc.scale(-1.0, -1.0, -1.0); - } - } + // Get the rotation part matrix + Matrix4D mc = m.decompose()[2]; // Extract quaternion double trace = (mc[0][0] + mc[1][1] + mc[2][2]); if (trace > 0.0) { diff --git a/src/Mod/Test/BaseTests.py b/src/Mod/Test/BaseTests.py index 30ec380436..0e9d72db63 100644 --- a/src/Mod/Test/BaseTests.py +++ b/src/Mod/Test/BaseTests.py @@ -266,31 +266,20 @@ class AlgebraTestCase(unittest.TestCase): self.assertNotEqual(m2, m4 * m3, "Wrong multiplication order") def testRotationFromMatrix(self): - m1 = FreeCAD.Matrix() - m1.rotateZ(0.2) - m1.scale(0.5) - m1.rotateY(0.2) - m1.scale(3) - m1.move(10, 5, -3) - r1 = FreeCAD.Rotation(m1) - m2 = FreeCAD.Matrix() - m2.rotateZ(0.2) - m2.rotateY(0.2) - m2.move(10, 5, -3) - r2 = FreeCAD.Rotation(m2) - self.assertTrue(r1.isSame(r2, 1e-7), "Scale on matrix influenced rotation") - m3 = FreeCAD.Matrix() - m3.scale(-1) - r3 = FreeCAD.Rotation(m3) - r0 = FreeCAD.Rotation() - self.assertTrue(r3.isSame(r0, 1e-7), "Scale on matrix influenced rotation") - m4 = FreeCAD.Matrix() - m4.scale(1.25, 1.0, 0.25) - m4.move(4, 5, 6) - r24 = FreeCAD.Rotation(m2 * m4) - r42 = FreeCAD.Rotation(m4 * m2) - self.assertTrue(r2.isSame(r24, 1e-7), "Scale on matrix influenced rotation") - self.assertTrue(r2.isSame(r42, 1e-7), "Scale on matrix influenced rotation") + rot = FreeCAD.Rotation(45,30,0) + m_r = rot.toMatrix() + 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) + 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) + m_r.scale(-2) + err = "Uniform scale must not affect rotation" + 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) @@ -374,24 +363,6 @@ class AlgebraTestCase(unittest.TestCase): p.Rotation.Angle = math.pi / 2 self.assertEqual(abs(p.inverse().Rotation.Angle), p.Rotation.Angle) - def testMatrixToRotationFailure(self): - mat = FreeCAD.Matrix() - mat.A21 = 1.0 - with self.assertRaises(ValueError): - FreeCAD.Placement(mat) - with self.assertRaises(ValueError): - FreeCAD.Rotation(mat) - with self.assertRaises(ValueError): - FreeCAD.Rotation(*mat.A) - with self.assertRaises(ValueError): - FreeCAD.Rotation(1, 1, 0, 0, 1, 0, 0, 0, 1) - with self.assertRaises(ValueError): - rot = FreeCAD.Rotation() - rot.Matrix = FreeCAD.Matrix(1, 1, 0, 0, 1, 0, 0, 0, 1) - with self.assertRaises(ValueError): - plm = FreeCAD.Placement() - rot.Matrix = FreeCAD.Matrix(1, 1, 0, 0, 1, 0, 0, 0, 1) - def testYawPitchRoll(self): def getYPR1(yaw, pitch, roll): r = FreeCAD.Rotation() diff --git a/tests/src/Base/Rotation.cpp b/tests/src/Base/Rotation.cpp index c5cb73f4a4..d8d6676a9e 100644 --- a/tests/src/Base/Rotation.cpp +++ b/tests/src/Base/Rotation.cpp @@ -8,16 +8,17 @@ TEST(Rotation, TestNonUniformScaleLeft) { Base::Rotation rot; - rot.setYawPitchRoll(20.0, 0.0, 0.0); + rot.setYawPitchRoll(45.0, 0.0, 0.0); Base::Matrix4D mat; rot.getValue(mat); Base::Matrix4D scale; - scale.scale(2.0, 3.0, 4.0); + scale.scale(1.0, std::sqrt(3.0), 1.0); Base::Rotation scaled_rot(scale * mat); + rot.setYawPitchRoll(60.0, 0.0, 0.0); EXPECT_EQ(scaled_rot.isSame(rot, 1.0e-7), true); EXPECT_EQ(rot.isSame(scaled_rot, 1.0e-7), true); } @@ -66,13 +67,4 @@ TEST(Rotation, TestUniformScaleLT1) EXPECT_EQ(scaled_rot.isSame(scaled_rot, 1.0e-7), true); } -TEST(Rotation, TestRotationFailure) -{ - Base::Matrix4D mat; - mat.setCol(0, Base::Vector3d {1, 0, 0}); - mat.setCol(1, Base::Vector3d {1, 1, 0}); - mat.setCol(2, Base::Vector3d {0, 0, 1}); - - EXPECT_THROW(Base::Rotation {mat}, Base::ValueError); -} // NOLINTEND(cppcoreguidelines-avoid-magic-numbers,readability-magic-numbers)