Revision control
Copy as Markdown
// Copyright (C) 2015 Konstantin Ritt.
// SPDX-License-Identifier: LicenseRef-Qt-Commercial OR LGPL-3.0-only OR GPL-2.0-only OR GPL-3.0-only
#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