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

Create rotation from any matrix

Browse Source 
To help find a matrix components a decompose method is added to Matrix class
This commit is contained in:
Jolbas
2023-03-23 01:03:07 +01:00
committed by Chris Hennes
parent d7fcbc79a8
commit 2d8c280528
7 changed files with 111 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
src/Base/Matrix.cpp
View File

@@ -26,6 +26,7 @@
# include <cstring>
# include <sstream>
#endif
# include <array>
#include "Matrix.h"
#include "Converter.h"
@@ -897,3 +898,70 @@ ScaleType Matrix4D::hasScale(double tol) const
return ScaleType::NoScaling;
}
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()
};
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;
}

3
src/Base/Matrix.h
View File

@@ -25,6 +25,7 @@
#define BASE_MATRIX_H
#include <string>
#include <array>
#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<Matrix4D, 4> 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

7
src/Base/MatrixPy.xml
View File

@@ -97,6 +97,13 @@ if it's not a scale matrix.
tol : float</UserDocu>
</Documentation>
</Methode>
<Methode Name="decompose" Const="true">
<Documentation>
<UserDocu>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.</UserDocu>
</Documentation>
</Methode>
<Methode Name="nullify" NoArgs="true">
<Documentation>
<UserDocu>nullify() -> None

13
src/Base/MatrixPyImp.cpp
View File

@@ -22,6 +22,7 @@
#include "PreCompiled.h"
//#include <array>
// 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<int>(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()
{

25
src/Base/Rotation.cpp
View File

@@ -22,6 +22,7 @@
#include "PreCompiled.h"
#include <array>
#include <boost/algorithm/string/predicate.hpp>
#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) {

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

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

14
tests/src/Base/Rotation.cpp
View File

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