| /**************************************************************************** |
| ** |
| ** Copyright (C) 2015 Konstantin Ritt. |
| ** Contact: https://www.qt.io/licensing/ |
| ** |
| ** This file is part of the Qt3D module of the Qt Toolkit. |
| ** |
| ** $QT_BEGIN_LICENSE:LGPL$ |
| ** Commercial License Usage |
| ** Licensees holding valid commercial Qt licenses may use this file in |
| ** accordance with the commercial license agreement provided with the |
| ** Software or, alternatively, in accordance with the terms contained in |
| ** a written agreement between you and The Qt Company. For licensing terms |
| ** and conditions see https://www.qt.io/terms-conditions. For further |
| ** information use the contact form at https://www.qt.io/contact-us. |
| ** |
| ** GNU Lesser General Public License Usage |
| ** Alternatively, this file may be used under the terms of the GNU Lesser |
| ** General Public License version 3 as published by the Free Software |
| ** Foundation and appearing in the file LICENSE.LGPL3 included in the |
| ** packaging of this file. Please review the following information to |
| ** ensure the GNU Lesser General Public License version 3 requirements |
| ** will be met: https://www.gnu.org/licenses/lgpl-3.0.html. |
| ** |
| ** GNU General Public License Usage |
| ** Alternatively, this file may be used under the terms of the GNU |
| ** General Public License version 2.0 or (at your option) the GNU General |
| ** Public license version 3 or any later version approved by the KDE Free |
| ** Qt Foundation. The licenses are as published by the Free Software |
| ** Foundation and appearing in the file LICENSE.GPL2 and LICENSE.GPL3 |
| ** included in the packaging of this file. Please review the following |
| ** information to ensure the GNU General Public License requirements will |
| ** be met: https://www.gnu.org/licenses/gpl-2.0.html and |
| ** https://www.gnu.org/licenses/gpl-3.0.html. |
| ** |
| ** $QT_END_LICENSE$ |
| ** |
| ****************************************************************************/ |
| |
| #ifndef QT3DCORE_QMATH3D_P_H |
| #define QT3DCORE_QMATH3D_P_H |
| |
| // |
| // W A R N I N G |
| // ------------- |
| // |
| // This file is not part of the Qt3D API. It exists purely as an |
| // implementation detail. This header file may change from version to |
| // version without notice, or even be removed. |
| // |
| // We mean it. |
| // |
| #include <QtGui/qmatrix4x4.h> |
| #include <QtGui/qquaternion.h> |
| #include <QtGui/qvector3d.h> |
| #include <Qt3DCore/private/sqt_p.h> |
| |
| #include <cmath> |
| |
| QT_BEGIN_NAMESPACE |
| |
| inline void composeQMatrix4x4(const QVector3D &position, const QQuaternion &orientation, const QVector3D &scale, QMatrix4x4 &m) |
| { |
| const QMatrix3x3 rot3x3(orientation.toRotationMatrix()); |
| |
| // set up final matrix with scale, rotation and translation |
| m(0, 0) = scale.x() * rot3x3(0, 0); m(0, 1) = scale.y() * rot3x3(0, 1); m(0, 2) = scale.z() * rot3x3(0, 2); m(0, 3) = position.x(); |
| m(1, 0) = scale.x() * rot3x3(1, 0); m(1, 1) = scale.y() * rot3x3(1, 1); m(1, 2) = scale.z() * rot3x3(1, 2); m(1, 3) = position.y(); |
| m(2, 0) = scale.x() * rot3x3(2, 0); m(2, 1) = scale.y() * rot3x3(2, 1); m(2, 2) = scale.z() * rot3x3(2, 2); m(2, 3) = position.z(); |
| // no projection term |
| m(3, 0) = 0.0f; m(3, 1) = 0.0f; m(3, 2) = 0.0f; m(3, 3) = 1.0f; |
| } |
| |
| inline void decomposeQMatrix3x3(const QMatrix3x3 &m, QMatrix3x3 &Q, QVector3D &D, QVector3D &U) |
| { |
| // Factor M = QR = QDU where Q is orthogonal, D is diagonal, |
| // and U is upper triangular with ones on its diagonal. |
| // Algorithm uses Gram-Schmidt orthogonalization (the QR algorithm). |
| // |
| // If M = [ m0 | m1 | m2 ] and Q = [ q0 | q1 | q2 ], then |
| // q0 = m0/|m0| |
| // q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0| |
| // q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1| |
| // |
| // where |V| indicates length of vector V and A*B indicates dot |
| // product of vectors A and B. The matrix R has entries |
| // |
| // r00 = q0*m0 r01 = q0*m1 r02 = q0*m2 |
| // r10 = 0 r11 = q1*m1 r12 = q1*m2 |
| // r20 = 0 r21 = 0 r22 = q2*m2 |
| // |
| // so D = diag(r00,r11,r22) and U has entries u01 = r01/r00, |
| // u02 = r02/r00, and u12 = r12/r11. |
| |
| // Q = rotation |
| // D = scaling |
| // U = shear |
| |
| // D stores the three diagonal entries r00, r11, r22 |
| // U stores the entries U[0] = u01, U[1] = u02, U[2] = u12 |
| |
| // build orthogonal matrix Q |
| float invLen = 1.0f / std::sqrt(m(0, 0) * m(0, 0) + m(1, 0) * m(1, 0) + m(2, 0) * m(2, 0)); |
| Q(0, 0) = m(0, 0) * invLen; |
| Q(1, 0) = m(1, 0) * invLen; |
| Q(2, 0) = m(2, 0) * invLen; |
| |
| float dot = Q(0, 0) * m(0, 1) + Q(1, 0) * m(1, 1) + Q(2, 0) * m(2, 1); |
| Q(0, 1) = m(0, 1) - dot * Q(0, 0); |
| Q(1, 1) = m(1, 1) - dot * Q(1, 0); |
| Q(2, 1) = m(2, 1) - dot * Q(2, 0); |
| invLen = 1.0f / std::sqrt(Q(0, 1) * Q(0, 1) + Q(1, 1) * Q(1, 1) + Q(2, 1) * Q(2, 1)); |
| Q(0, 1) *= invLen; |
| Q(1, 1) *= invLen; |
| Q(2, 1) *= invLen; |
| |
| dot = Q(0, 0) * m(0, 2) + Q(1, 0) * m(1, 2) + Q(2, 0) * m(2, 2); |
| Q(0, 2) = m(0, 2) - dot * Q(0, 0); |
| Q(1, 2) = m(1, 2) - dot * Q(1, 0); |
| Q(2, 2) = m(2, 2) - dot * Q(2, 0); |
| dot = Q(0, 1) * m(0, 2) + Q(1, 1) * m(1, 2) + Q(2, 1) * m(2, 2); |
| Q(0, 2) -= dot * Q(0, 1); |
| Q(1, 2) -= dot * Q(1, 1); |
| Q(2, 2) -= dot * Q(2, 1); |
| invLen = 1.0f / std::sqrt(Q(0, 2) * Q(0, 2) + Q(1, 2) * Q(1, 2) + Q(2, 2) * Q(2, 2)); |
| Q(0, 2) *= invLen; |
| Q(1, 2) *= invLen; |
| Q(2, 2) *= invLen; |
| |
| // guarantee that orthogonal matrix has determinant 1 (no reflections) |
| const float det = Q(0, 0) * Q(1, 1) * Q(2, 2) + Q(0, 1) * Q(1, 2) * Q(2, 0) + |
| Q(0, 2) * Q(1, 0) * Q(2, 1) - Q(0, 2) * Q(1, 1) * Q(2, 0) - |
| Q(0, 1) * Q(1, 0) * Q(2, 2) - Q(0, 0) * Q(1, 2) * Q(2, 1); |
| if (det < 0.0f) |
| Q *= -1.0f; |
| |
| // build "right" matrix R |
| QMatrix3x3 R(Qt::Uninitialized); |
| R(0, 0) = Q(0, 0) * m(0, 0) + Q(1, 0) * m(1, 0) + Q(2, 0) * m(2, 0); |
| R(0, 1) = Q(0, 0) * m(0, 1) + Q(1, 0) * m(1, 1) + Q(2, 0) * m(2, 1); |
| R(1, 1) = Q(0, 1) * m(0, 1) + Q(1, 1) * m(1, 1) + Q(2, 1) * m(2, 1); |
| R(0, 2) = Q(0, 0) * m(0, 2) + Q(1, 0) * m(1, 2) + Q(2, 0) * m(2, 2); |
| R(1, 2) = Q(0, 1) * m(0, 2) + Q(1, 1) * m(1, 2) + Q(2, 1) * m(2, 2); |
| R(2, 2) = Q(0, 2) * m(0, 2) + Q(1, 2) * m(1, 2) + Q(2, 2) * m(2, 2); |
| |
| // the scaling component |
| D[0] = R(0, 0); |
| D[1] = R(1, 1); |
| D[2] = R(2, 2); |
| |
| // the shear component |
| U[0] = R(0, 1) / D[0]; |
| U[1] = R(0, 2) / D[0]; |
| U[2] = R(1, 2) / D[1]; |
| } |
| |
| inline bool hasScale(const QMatrix4x4 &m) |
| { |
| // If the columns are orthonormal and form a right-handed system, then there is no scale |
| float t(m.determinant()); |
| if (!qFuzzyIsNull(t - 1.0f)) |
| return true; |
| t = m(0, 0) * m(0, 0) + m(1, 0) * m(1, 0) + m(2, 0) * m(2, 0); |
| if (!qFuzzyIsNull(t - 1.0f)) |
| return true; |
| t = m(0, 1) * m(0, 1) + m(1, 1) * m(1, 1) + m(2, 1) * m(2, 1); |
| if (!qFuzzyIsNull(t - 1.0f)) |
| return true; |
| t = m(0, 2) * m(0, 2) + m(1, 2) * m(1, 2) + m(2, 2) * m(2, 2); |
| if (!qFuzzyIsNull(t - 1.0f)) |
| return true; |
| return false; |
| } |
| |
| inline void decomposeQMatrix4x4(const QMatrix4x4 &m, QVector3D &position, QQuaternion &orientation, QVector3D &scale) |
| { |
| Q_ASSERT(m.isAffine()); |
| |
| const QMatrix3x3 m3x3(m.toGenericMatrix<3, 3>()); |
| |
| QMatrix3x3 rot3x3(Qt::Uninitialized); |
| if (hasScale(m)) { |
| decomposeQMatrix3x3(m3x3, rot3x3, scale, position); |
| } else { |
| // we know there is no scaling part; no need for QDU decomposition |
| scale = QVector3D(1.0f, 1.0f, 1.0f); |
| rot3x3 = m3x3; |
| } |
| orientation = QQuaternion::fromRotationMatrix(rot3x3); |
| position = QVector3D(m(0, 3), m(1, 3), m(2, 3)); |
| } |
| |
| inline void decomposeQMatrix4x4(const QMatrix4x4 &m, Qt3DCore::Sqt &sqt) |
| { |
| Q_ASSERT(m.isAffine()); |
| |
| const QMatrix3x3 m3x3(m.toGenericMatrix<3, 3>()); |
| |
| QMatrix3x3 rot3x3(Qt::Uninitialized); |
| if (hasScale(m)) { |
| decomposeQMatrix3x3(m3x3, rot3x3, sqt.scale, sqt.translation); |
| } else { |
| // we know there is no scaling part; no need for QDU decomposition |
| sqt.scale = QVector3D(1.0f, 1.0f, 1.0f); |
| rot3x3 = m3x3; |
| } |
| sqt.rotation = QQuaternion::fromRotationMatrix(rot3x3); |
| sqt.translation = QVector3D(m(0, 3), m(1, 3), m(2, 3)); |
| } |
| |
| QT_END_NAMESPACE |
| |
| #endif // QT3DCORE_QMATH3D_P_H |