kindred/create
1
1
Fork 0
Code Issues 53 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

Fix create Rotation from scaled matrix

Browse Source 
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
This commit is contained in:
Jolbas
2023-02-17 17:05:12 +01:00
committed by Chris Hennes
parent bdb6e1bd55
commit 6c3efbdb3b
3 changed files with 88 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
src/Base/Matrix.cpp
View File

@@ -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;
}

45
src/Base/Rotation.cpp
View File

@@ -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 <http://www.geometrictools.com>
//
// 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];

34
src/Mod/Test/BaseTests.py
View File

@@ -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))