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

Matrix.decompose() fix

Browse Source 
Fixes some errors when matrix has zero scale in one or two directions
This commit is contained in:
Jolbas
2023-08-31 14:50:36 +02:00
committed by Chris Hennes
parent 2d8c280528
commit 40228d9ef2
3 changed files with 87 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

124
src/Base/Matrix.cpp
View File

@@ -899,69 +899,93 @@ ScaleType Matrix4D::hasScale(double tol) const
return ScaleType::NoScaling;
}
std::array<Matrix4D, 4> Matrix4D::decompose() const{
std::array<Matrix4D, 4> 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<Matrix4D, 4> 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<Vector3d, 3> 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<Matrix4D, 4>{
residualMatrix, scaleMatrix, rotationMatrix, moveMatrix
};
}

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

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

12
tests/src/Base/CoordinateSystem.cpp
View File

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