434 lines
11 KiB
C++
434 lines
11 KiB
C++
/*
|
|
QGLView
|
|
Copyright (C) 2019 Ivan Pelipenko peri4ko@yandex.ru
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "gltransform.h"
|
|
//#include <Qt3DCore/private/sqt_p.h>
|
|
|
|
#include "gltypes.h"
|
|
#include <cmath>
|
|
|
|
|
|
inline void composeQMatrix4x4(const QVector3D & position, const QVector3D & orientation, const QVector3D & scale, QMatrix4x4 &m) {
|
|
const QMatrix3x3 rot3x3(Transform::toRotationMatrix(orientation));
|
|
|
|
// 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, QVector3D & 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 = Transform::fromRotationMatrix(rot3x3);
|
|
position = QVector3D(m(0, 3), m(1, 3), m(2, 3));
|
|
}
|
|
|
|
|
|
|
|
|
|
Transform::Transform(): m_scale(1.0f, 1.0f, 1.0f),
|
|
m_translation(), m_eulerRotationAngles(), m_matrixDirty(false) {
|
|
}
|
|
|
|
|
|
void Transform::setMatrix(const QMatrix4x4 & m) {
|
|
if (m != matrix()) {
|
|
m_matrix = m;
|
|
m_matrixDirty = false;
|
|
QVector3D s;
|
|
QVector3D t;
|
|
QVector3D r;
|
|
decomposeQMatrix4x4(m, t, r, s);
|
|
m_scale = s;
|
|
m_translation = t;
|
|
m_eulerRotationAngles = r;
|
|
}
|
|
}
|
|
|
|
|
|
void Transform::setRotationX(float r) {
|
|
if (m_eulerRotationAngles.x() == r)
|
|
return;
|
|
m_eulerRotationAngles.setX(r);
|
|
m_matrixDirty = true;
|
|
}
|
|
|
|
|
|
void Transform::setRotationY(float r) {
|
|
if (m_eulerRotationAngles.y() == r)
|
|
return;
|
|
m_eulerRotationAngles.setY(r);
|
|
m_matrixDirty = true;
|
|
}
|
|
|
|
|
|
void Transform::setRotationZ(float r) {
|
|
if (m_eulerRotationAngles.z() == r)
|
|
return;
|
|
m_eulerRotationAngles.setZ(r);
|
|
m_matrixDirty = true;
|
|
}
|
|
|
|
|
|
QMatrix4x4 Transform::matrix() const {
|
|
buildMatrix();
|
|
return m_matrix;
|
|
}
|
|
|
|
|
|
QMatrix4x4 Transform::matrixRotate() const {
|
|
buildMatrix();
|
|
return m_matrixR;
|
|
}
|
|
|
|
|
|
QMatrix4x4 Transform::matrixScale() const {
|
|
buildMatrix();
|
|
return m_matrixS;
|
|
}
|
|
|
|
|
|
QMatrix4x4 Transform::matrixRotateScale() const {
|
|
buildMatrix();
|
|
return m_matrixWT;
|
|
}
|
|
|
|
|
|
QVector3D Transform::direction() const {
|
|
return matrixRotate().mapVector(QVector3D(0,0,-1)).normalized();
|
|
}
|
|
|
|
|
|
void Transform::buildMatrix() const {
|
|
if (m_matrixDirty) {
|
|
composeQMatrix4x4(m_translation, m_eulerRotationAngles, m_scale, m_matrix);
|
|
composeQMatrix4x4(QVector3D(), m_eulerRotationAngles, m_scale, m_matrixWT);
|
|
composeQMatrix4x4(QVector3D(), m_eulerRotationAngles, QVector3D(1,1,1), m_matrixR);
|
|
composeQMatrix4x4(QVector3D(), QVector3D(), m_scale, m_matrixS);
|
|
m_matrixDirty = false;
|
|
}
|
|
}
|
|
|
|
|
|
float Transform::rotationX() const {
|
|
return m_eulerRotationAngles.x();
|
|
}
|
|
|
|
|
|
float Transform::rotationY() const {
|
|
return m_eulerRotationAngles.y();
|
|
}
|
|
|
|
|
|
float Transform::rotationZ() const {
|
|
return m_eulerRotationAngles.z();
|
|
}
|
|
|
|
|
|
void Transform::setScale(const QVector3D & s) {
|
|
if (s != m_scale) {
|
|
m_scale = s;
|
|
m_matrixDirty = true;
|
|
}
|
|
}
|
|
|
|
|
|
void Transform::setScaleX(float s) {
|
|
if (s != m_scale.x()) {
|
|
m_scale.setX(s);
|
|
m_matrixDirty = true;
|
|
}
|
|
}
|
|
|
|
|
|
void Transform::setScaleY(float s) {
|
|
if (s != m_scale.y()) {
|
|
m_scale.setY(s);
|
|
m_matrixDirty = true;
|
|
}
|
|
}
|
|
|
|
|
|
void Transform::setScaleZ(float s) {
|
|
if (s != m_scale.z()) {
|
|
m_scale.setZ(s);
|
|
m_matrixDirty = true;
|
|
}
|
|
}
|
|
|
|
|
|
QVector3D Transform::scale3D() const {
|
|
return m_scale;
|
|
}
|
|
|
|
|
|
float Transform::scale() const {
|
|
return m_scale.x();
|
|
}
|
|
|
|
|
|
void Transform::setRotation(const QVector3D & r) {
|
|
if (r != m_eulerRotationAngles) {
|
|
m_eulerRotationAngles = r;
|
|
m_matrixDirty = true;
|
|
}
|
|
}
|
|
|
|
|
|
QVector3D Transform::rotation() const {
|
|
return m_eulerRotationAngles;
|
|
}
|
|
|
|
|
|
void Transform::setTranslation(const QVector3D & t) {
|
|
if (t != m_translation) {
|
|
m_translation = t;
|
|
m_matrixDirty = true;
|
|
}
|
|
}
|
|
|
|
|
|
void Transform::setTranslationX(float t) {
|
|
if (t != m_translation.x()) {
|
|
m_translation.setX(t);
|
|
m_matrixDirty = true;
|
|
}
|
|
}
|
|
|
|
|
|
void Transform::setTranslationY(float t) {
|
|
if (t != m_translation.y()) {
|
|
m_translation.setY(t);
|
|
m_matrixDirty = true;
|
|
}
|
|
}
|
|
|
|
|
|
void Transform::setTranslationZ(float t) {
|
|
if (t != m_translation.z()) {
|
|
m_translation.setZ(t);
|
|
m_matrixDirty = true;
|
|
}
|
|
}
|
|
|
|
|
|
QVector3D Transform::translation() const {
|
|
return m_translation;
|
|
}
|
|
|
|
|
|
QQuaternion Transform::fromAxisAndAngle(const QVector3D & axis, float angle) {
|
|
return QQuaternion::fromAxisAndAngle(axis, angle);
|
|
}
|
|
|
|
|
|
QQuaternion Transform::fromAxisAndAngle(float x, float y, float z, float angle) {
|
|
return QQuaternion::fromAxisAndAngle(x, y, z, angle);
|
|
}
|
|
|
|
|
|
QQuaternion Transform::fromAxesAndAngles(const QVector3D & axis1, float angle1,
|
|
const QVector3D & axis2, float angle2) {
|
|
const QQuaternion q1 = QQuaternion::fromAxisAndAngle(axis1, angle1);
|
|
const QQuaternion q2 = QQuaternion::fromAxisAndAngle(axis2, angle2);
|
|
return q2 * q1;
|
|
}
|
|
|
|
|
|
QQuaternion Transform::fromAxesAndAngles(const QVector3D & axis1, float angle1,
|
|
const QVector3D & axis2, float angle2,
|
|
const QVector3D & axis3, float angle3) {
|
|
const QQuaternion q1 = QQuaternion::fromAxisAndAngle(axis1, angle1);
|
|
const QQuaternion q2 = QQuaternion::fromAxisAndAngle(axis2, angle2);
|
|
const QQuaternion q3 = QQuaternion::fromAxisAndAngle(axis3, angle3);
|
|
return q3 * q2 * q1;
|
|
}
|
|
|
|
|
|
QQuaternion Transform::fromAxes(const QVector3D & xAxis, const QVector3D & yAxis, const QVector3D & zAxis) {
|
|
return QQuaternion::fromAxes(xAxis, yAxis, zAxis);
|
|
}
|
|
|
|
|
|
QVector3D Transform::fromDirection(QVector3D d, float pitch) {
|
|
QVector3D ret;
|
|
d.normalize();
|
|
ret[0] = M_PI - acos(d.z());
|
|
ret[1] = pitch * deg2rad;
|
|
ret[2] = -atan2(d.x(), d.y());
|
|
normalizeAngleRad(ret[0]);
|
|
normalizeAngleRad(ret[2]);
|
|
return ret * rad2deg;
|
|
}
|
|
|
|
|
|
QVector3D Transform::fromRotationMatrix(const QMatrix3x3 & m) {
|
|
//return QQuaternion::fromRotationMatrix(m);
|
|
float sy = sqrt(m(0,0) * m(0,0) + m(1,0) * m(1,0));
|
|
bool singular = sy < 1.E-6; // If
|
|
float x, y, z;
|
|
if (!singular) {
|
|
x = atan2( m(2,1), m(2,2));
|
|
y = atan2(-m(2,0), sy);
|
|
z = atan2( m(1,0), m(0,0));
|
|
} else {
|
|
x = atan2(-m(1,2), m(1,1));
|
|
y = atan2(-m(2,0), sy);
|
|
z = 0.;
|
|
}
|
|
return QVector3D(x, y, z) * rad2deg;
|
|
}
|
|
|
|
|
|
QMatrix3x3 Transform::toRotationMatrix(const QVector3D & r) {
|
|
QMatrix4x4 m;
|
|
if (r.z() != 0.f) m.rotate(r.z(), 0., 0., 1.);
|
|
if (r.y() != 0.f) m.rotate(r.y(), 0., 1., 0.);
|
|
if (r.x() != 0.f) m.rotate(r.x(), 1., 0., 0.);
|
|
return m.toGenericMatrix<3,3>();
|
|
}
|
|
|
|
|
|
QMatrix4x4 Transform::rotateAround(const QVector3D & point, float angle, const QVector3D & axis) {
|
|
QMatrix4x4 m;
|
|
m.translate(point);
|
|
m.rotate(angle, axis);
|
|
m.translate(-point);
|
|
return m;
|
|
}
|
|
|
|
|
|
QMatrix4x4 Transform::rotateFromAxes(const QVector3D & xAxis, const QVector3D & yAxis, const QVector3D & zAxis) {
|
|
return QMatrix4x4(xAxis.x(), yAxis.x(), zAxis.x(), 0.0f,
|
|
xAxis.y(), yAxis.y(), zAxis.y(), 0.0f,
|
|
xAxis.z(), yAxis.z(), zAxis.z(), 0.0f,
|
|
0.0f, 0.0f, 0.0f, 1.0f);
|
|
}
|