From 6c3efbdb3bd339d112e53801d4ff2ea840c8cfc8 Mon Sep 17 00:00:00 2001 From: Jolbas <39026960+Jolbas@users.noreply.github.com> Date: Fri, 17 Feb 2023 17:05:12 +0100 Subject: [PATCH] Fix create Rotation from scaled matrix Support for creation of Rotation from matrices which is a combination of non uniform scale and a rotation Fixes according to review Scale -1 is Uniform, Not NoScaling Fix hasScale() when negative scale --- src/Base/Matrix.cpp | 29 ++++++++++++++++--------- src/Base/Rotation.cpp | 45 ++++++++++++++++++++++++++++++--------- src/Mod/Test/BaseTests.py | 34 +++++++++++++++++++++++++++++ 3 files changed, 88 insertions(+), 20 deletions(-) diff --git a/src/Base/Matrix.cpp b/src/Base/Matrix.cpp index c2f16a9fd8..70854e508d 100644 --- a/src/Base/Matrix.cpp +++ b/src/Base/Matrix.cpp @@ -833,34 +833,43 @@ ScaleType Matrix4D::hasScale(double tol) const if (tol == 0.0) tol = 1e-9; + // check if the absolute values are proportionally close or equal + auto closeAbs = [&](double a, double b) { + double c = fabs(a); + double d = fabs(b); + if (d>c) return (d-c)/d <= tol; + else if (c>d) return (c-d)/c <= tol; + return true; + }; + // get column vectors - double dx = Vector3d(dMtrx4D[0][0],dMtrx4D[1][0],dMtrx4D[2][0]).Sqr(); - double dy = Vector3d(dMtrx4D[0][1],dMtrx4D[1][1],dMtrx4D[2][1]).Sqr(); - double dz = Vector3d(dMtrx4D[0][2],dMtrx4D[1][2],dMtrx4D[2][2]).Sqr(); + double dx = getCol(0).Sqr(); + double dy = getCol(1).Sqr(); + double dz = getCol(2).Sqr(); double dxyz = sqrt(dx * dy * dz); // get row vectors - double du = Vector3d(dMtrx4D[0][0],dMtrx4D[0][1],dMtrx4D[0][2]).Sqr(); - double dv = Vector3d(dMtrx4D[1][0],dMtrx4D[1][1],dMtrx4D[1][2]).Sqr(); - double dw = Vector3d(dMtrx4D[2][0],dMtrx4D[2][1],dMtrx4D[2][2]).Sqr(); + double du = getRow(0).Sqr(); + double dv = getRow(1).Sqr(); + double dw = getRow(2).Sqr(); double duvw = sqrt(du * dv * dw); double d3 = determinant3(); // This could be e.g. a projection, a shearing,... matrix - if (fabs(dxyz - d3) > tol && fabs(duvw - d3) > tol) { + if (!closeAbs(dxyz, d3) && !closeAbs(duvw, d3)) { return ScaleType::Other; } - if (fabs(duvw - d3) <= tol && (fabs(du - dv) > tol || fabs(dv - dw) > tol)) { + if (closeAbs(duvw, d3) && (!closeAbs(du, dv) || !closeAbs(dv, dw))) { return ScaleType::NonUniformLeft; } - if (fabs(dxyz - d3) <= tol && (fabs(dx - dy) > tol || fabs(dy - dz) > tol)) { + if (closeAbs(dxyz, d3) && (!closeAbs(dx, dy) || !closeAbs(dy, dz))) { return ScaleType::NonUniformRight; } - if (fabs(dx - 1.0) > tol) { + if (fabs(d3 - 1.0) > tol) { return ScaleType::Uniform; } diff --git a/src/Base/Rotation.cpp b/src/Base/Rotation.cpp index ef1d491c9e..344d513c7b 100644 --- a/src/Base/Rotation.cpp +++ b/src/Base/Rotation.cpp @@ -220,32 +220,55 @@ void Rotation::setValue(const double q[4]) void Rotation::setValue(const Matrix4D & m) { - double trace = (m[0][0] + m[1][1] + m[2][2]); + + 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); + } + } + // Extract quaternion + double trace = (mc[0][0] + mc[1][1] + mc[2][2]); if (trace > 0.0) { double s = sqrt(1.0+trace); this->quat[3] = 0.5 * s; s = 0.5 / s; - this->quat[0] = ((m[2][1] - m[1][2]) * s); - this->quat[1] = ((m[0][2] - m[2][0]) * s); - this->quat[2] = ((m[1][0] - m[0][1]) * s); + this->quat[0] = ((mc[2][1] - mc[1][2]) * s); + this->quat[1] = ((mc[0][2] - mc[2][0]) * s); + this->quat[2] = ((mc[1][0] - mc[0][1]) * s); } else { // Described in RotationIssues.pdf from // // Get the max. element of the trace unsigned short i = 0; - if (m[1][1] > m[0][0]) i = 1; - if (m[2][2] > m[i][i]) i = 2; + if (mc[1][1] > mc[0][0]) i = 1; + if (mc[2][2] > mc[i][i]) i = 2; unsigned short j = (i+1)%3; unsigned short k = (i+2)%3; - double s = sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1.0); + double s = sqrt((mc[i][i] - (mc[j][j] + mc[k][k])) + 1.0); this->quat[i] = s * 0.5; s = 0.5 / s; - this->quat[3] = ((m[k][j] - m[j][k]) * s); - this->quat[j] = ((m[j][i] + m[i][j]) * s); - this->quat[k] = ((m[k][i] + m[i][k]) * s); + this->quat[3] = ((mc[k][j] - mc[j][k]) * s); + this->quat[j] = ((mc[j][i] + mc[i][j]) * s); + this->quat[k] = ((mc[k][i] + mc[i][k]) * s); } this->evaluateVector(); @@ -752,6 +775,8 @@ bool Rotation::isSame(const Rotation& q, double tol) const // This term can be simplified to // 2 - 2*(x1*y1 + ... + x4*y4) so that for the equality we have to check // 1 - tol/2 <= x1*y1 + ... + x4*y4 + // This simplification only work if both quats are normalized + // Is it safe to assume that? // Because a quaternion (x1,x2,x3,x4) is equal to (-x1,-x2,-x3,-x4) we use the // absolute value of the scalar product double dot = q.quat[0]*quat[0]+q.quat[1]*quat[1]+q.quat[2]*quat[2]+q.quat[3]*quat[3]; diff --git a/src/Mod/Test/BaseTests.py b/src/Mod/Test/BaseTests.py index 6cb265abc8..a5a3379d7a 100644 --- a/src/Mod/Test/BaseTests.py +++ b/src/Mod/Test/BaseTests.py @@ -253,6 +253,35 @@ class AlgebraTestCase(unittest.TestCase): self.assertEqual(m2, m3*m4 , "Wrong multiplication order") self.assertNotEqual(m2, m4*m3, "Wrong multiplication order") + + def testRotationFromMatrix(self): + m1 = FreeCAD.Matrix() + m1.rotateZ(.2) + m1.scale(0.5) + m1.rotateY(.2) + m1.scale(3) + m1.move(10,5,-3) + r1 = FreeCAD.Rotation(m1) + m2 = FreeCAD.Matrix() + m2.rotateZ(.2) + m2.rotateY(.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') + + def testRotation(self): r=FreeCAD.Rotation(1,0,0,0) # 180 deg around (1,0,0) self.assertEqual(r.Axis, FreeCAD.Vector(1,0,0)) @@ -526,6 +555,11 @@ class MatrixTestCase(unittest.TestCase): self.mat.setRow(1, FreeCAD.Vector(1,2,3)) self.mat.setRow(2, FreeCAD.Vector(1,2,3)) self.assertEqual(self.mat.hasScale(), FreeCAD.ScaleType.Other) + self.mat.unity() + self.mat.scale(-1.) + self.assertEqual(self.mat.hasScale(), FreeCAD.ScaleType.Uniform) + self.mat.scale(-2.) + self.assertEqual(self.mat.hasScale(), FreeCAD.ScaleType.Uniform) def testShearing(self): self.mat.setRow(1, FreeCAD.Vector(0,1,1))