1// Copyright (C) 2015 Konstantin Ritt.
2// SPDX-License-Identifier: LicenseRef-Qt-Commercial OR LGPL-3.0-only OR GPL-2.0-only OR GPL-3.0-only
3
4#ifndef QT3DCORE_QMATH3D_P_H
5#define QT3DCORE_QMATH3D_P_H
6
7//
8// W A R N I N G
9// -------------
10//
11// This file is not part of the Qt3D API. It exists purely as an
12// implementation detail. This header file may change from version to
13// version without notice, or even be removed.
14//
15// We mean it.
16//
17#include <QtGui/qmatrix4x4.h>
18#include <QtGui/qquaternion.h>
19#include <QtGui/qvector3d.h>
20#include <Qt3DCore/private/sqt_p.h>
21
22#include <cmath>
23
24QT_BEGIN_NAMESPACE
25
26inline void composeQMatrix4x4(const QVector3D &position, const QQuaternion &orientation, const QVector3D &scale, QMatrix4x4 &m)
27{
28 const QMatrix3x3 rot3x3(orientation.toRotationMatrix());
29
30 // set up final matrix with scale, rotation and translation
31 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();
32 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();
33 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();
34 // no projection term
35 m(3, 0) = 0.0f; m(3, 1) = 0.0f; m(3, 2) = 0.0f; m(3, 3) = 1.0f;
36}
37
38inline void decomposeQMatrix3x3(const QMatrix3x3 &m, QMatrix3x3 &Q, QVector3D &D, QVector3D &U)
39{
40 // Factor M = QR = QDU where Q is orthogonal, D is diagonal,
41 // and U is upper triangular with ones on its diagonal.
42 // Algorithm uses Gram-Schmidt orthogonalization (the QR algorithm).
43 //
44 // If M = [ m0 | m1 | m2 ] and Q = [ q0 | q1 | q2 ], then
45 // q0 = m0/|m0|
46 // q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
47 // q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
48 //
49 // where |V| indicates length of vector V and A*B indicates dot
50 // product of vectors A and B. The matrix R has entries
51 //
52 // r00 = q0*m0 r01 = q0*m1 r02 = q0*m2
53 // r10 = 0 r11 = q1*m1 r12 = q1*m2
54 // r20 = 0 r21 = 0 r22 = q2*m2
55 //
56 // so D = diag(r00,r11,r22) and U has entries u01 = r01/r00,
57 // u02 = r02/r00, and u12 = r12/r11.
58
59 // Q = rotation
60 // D = scaling
61 // U = shear
62
63 // D stores the three diagonal entries r00, r11, r22
64 // U stores the entries U[0] = u01, U[1] = u02, U[2] = u12
65
66 // build orthogonal matrix Q
67 float invLen = 1.0f / std::sqrt(x: m(0, 0) * m(0, 0) + m(1, 0) * m(1, 0) + m(2, 0) * m(2, 0));
68 Q(0, 0) = m(0, 0) * invLen;
69 Q(1, 0) = m(1, 0) * invLen;
70 Q(2, 0) = m(2, 0) * invLen;
71
72 float dot = Q(0, 0) * m(0, 1) + Q(1, 0) * m(1, 1) + Q(2, 0) * m(2, 1);
73 Q(0, 1) = m(0, 1) - dot * Q(0, 0);
74 Q(1, 1) = m(1, 1) - dot * Q(1, 0);
75 Q(2, 1) = m(2, 1) - dot * Q(2, 0);
76 invLen = 1.0f / std::sqrt(x: Q(0, 1) * Q(0, 1) + Q(1, 1) * Q(1, 1) + Q(2, 1) * Q(2, 1));
77 Q(0, 1) *= invLen;
78 Q(1, 1) *= invLen;
79 Q(2, 1) *= invLen;
80
81 dot = Q(0, 0) * m(0, 2) + Q(1, 0) * m(1, 2) + Q(2, 0) * m(2, 2);
82 Q(0, 2) = m(0, 2) - dot * Q(0, 0);
83 Q(1, 2) = m(1, 2) - dot * Q(1, 0);
84 Q(2, 2) = m(2, 2) - dot * Q(2, 0);
85 dot = Q(0, 1) * m(0, 2) + Q(1, 1) * m(1, 2) + Q(2, 1) * m(2, 2);
86 Q(0, 2) -= dot * Q(0, 1);
87 Q(1, 2) -= dot * Q(1, 1);
88 Q(2, 2) -= dot * Q(2, 1);
89 invLen = 1.0f / std::sqrt(x: Q(0, 2) * Q(0, 2) + Q(1, 2) * Q(1, 2) + Q(2, 2) * Q(2, 2));
90 Q(0, 2) *= invLen;
91 Q(1, 2) *= invLen;
92 Q(2, 2) *= invLen;
93
94 // guarantee that orthogonal matrix has determinant 1 (no reflections)
95 const float det = Q(0, 0) * Q(1, 1) * Q(2, 2) + Q(0, 1) * Q(1, 2) * Q(2, 0) +
96 Q(0, 2) * Q(1, 0) * Q(2, 1) - Q(0, 2) * Q(1, 1) * Q(2, 0) -
97 Q(0, 1) * Q(1, 0) * Q(2, 2) - Q(0, 0) * Q(1, 2) * Q(2, 1);
98 if (det < 0.0f)
99 Q *= -1.0f;
100
101 // build "right" matrix R
102 QMatrix3x3 R(Qt::Uninitialized);
103 R(0, 0) = Q(0, 0) * m(0, 0) + Q(1, 0) * m(1, 0) + Q(2, 0) * m(2, 0);
104 R(0, 1) = Q(0, 0) * m(0, 1) + Q(1, 0) * m(1, 1) + Q(2, 0) * m(2, 1);
105 R(1, 1) = Q(0, 1) * m(0, 1) + Q(1, 1) * m(1, 1) + Q(2, 1) * m(2, 1);
106 R(0, 2) = Q(0, 0) * m(0, 2) + Q(1, 0) * m(1, 2) + Q(2, 0) * m(2, 2);
107 R(1, 2) = Q(0, 1) * m(0, 2) + Q(1, 1) * m(1, 2) + Q(2, 1) * m(2, 2);
108 R(2, 2) = Q(0, 2) * m(0, 2) + Q(1, 2) * m(1, 2) + Q(2, 2) * m(2, 2);
109
110 // the scaling component
111 D[0] = R(0, 0);
112 D[1] = R(1, 1);
113 D[2] = R(2, 2);
114
115 // the shear component
116 U[0] = R(0, 1) / D[0];
117 U[1] = R(0, 2) / D[0];
118 U[2] = R(1, 2) / D[1];
119}
120
121inline bool hasScale(const QMatrix4x4 &m)
122{
123 // If the columns are orthonormal and form a right-handed system, then there is no scale
124 float t(m.determinant());
125 if (!qFuzzyIsNull(f: t - 1.0f))
126 return true;
127 t = m(0, 0) * m(0, 0) + m(1, 0) * m(1, 0) + m(2, 0) * m(2, 0);
128 if (!qFuzzyIsNull(f: t - 1.0f))
129 return true;
130 t = m(0, 1) * m(0, 1) + m(1, 1) * m(1, 1) + m(2, 1) * m(2, 1);
131 if (!qFuzzyIsNull(f: t - 1.0f))
132 return true;
133 t = m(0, 2) * m(0, 2) + m(1, 2) * m(1, 2) + m(2, 2) * m(2, 2);
134 if (!qFuzzyIsNull(f: t - 1.0f))
135 return true;
136 return false;
137}
138
139inline void decomposeQMatrix4x4(const QMatrix4x4 &m, QVector3D &position, QQuaternion &orientation, QVector3D &scale)
140{
141 Q_ASSERT(m.isAffine());
142
143 const QMatrix3x3 m3x3(m.toGenericMatrix<3, 3>());
144
145 QMatrix3x3 rot3x3(Qt::Uninitialized);
146 if (hasScale(m)) {
147 decomposeQMatrix3x3(m: m3x3, Q&: rot3x3, D&: scale, U&: position);
148 } else {
149 // we know there is no scaling part; no need for QDU decomposition
150 scale = QVector3D(1.0f, 1.0f, 1.0f);
151 rot3x3 = m3x3;
152 }
153 orientation = QQuaternion::fromRotationMatrix(rot3x3);
154 position = QVector3D(m(0, 3), m(1, 3), m(2, 3));
155}
156
157inline void decomposeQMatrix4x4(const QMatrix4x4 &m, Qt3DCore::Sqt &sqt)
158{
159 Q_ASSERT(m.isAffine());
160
161 const QMatrix3x3 m3x3(m.toGenericMatrix<3, 3>());
162
163 QMatrix3x3 rot3x3(Qt::Uninitialized);
164 if (hasScale(m)) {
165 decomposeQMatrix3x3(m: m3x3, Q&: rot3x3, D&: sqt.scale, U&: sqt.translation);
166 } else {
167 // we know there is no scaling part; no need for QDU decomposition
168 sqt.scale = QVector3D(1.0f, 1.0f, 1.0f);
169 rot3x3 = m3x3;
170 }
171 sqt.rotation = QQuaternion::fromRotationMatrix(rot3x3);
172 sqt.translation = QVector3D(m(0, 3), m(1, 3), m(2, 3));
173}
174
175QT_END_NAMESPACE
176
177#endif // QT3DCORE_QMATH3D_P_H
178

source code of qt3d/src/core/transforms/qmath3d_p.h