Create rotation from any matrix

To help find a matrix components a decompose method is added to Matrix class

src/Base/Matrix.cpp

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

src/Base/Matrix.h

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

src/Base/MatrixPy.xml

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

src/Base/MatrixPyImp.cpp

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

src/Base/Rotation.cpp

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