thirdparty/scummvm
2
0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

GRIM: Fix matrix to quaternion conversion. Fixes #1132

Browse source
This commit is contained in:
Joni Vähämäki 2014-12-25 22:35:30 +02:00
parent b8bddf64da
commit 2c1ae44b5c
1 changed files with 36 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

56
math/quat.cpp
View file

@ -111,29 +111,45 @@ Quaternion& Quaternion::normalize() {
} }
void Quaternion::fromMatrix(const Matrix3 &m) { void Quaternion::fromMatrix(const Matrix3 &m) {
float qx, qy, qz; float qx, qy, qz, qw;
float qw = 0.25f * (m.getValue(0, 0) + m.getValue(1, 1) + m.getValue(2, 2) + 1.0f); float tr = m.getValue(0, 0) + m.getValue(1, 1) + m.getValue(2, 2);
float s;
if (qw != 0.0f) { if (tr > 0.0f) {
qw = sqrtf(qw); s = sqrtf(tr + 1.0f);
qx = (m.getValue(2, 1) - m.getValue(1, 2)) / (4 * qw); qw = s * 0.5f;
qy = (m.getValue(0, 2) - m.getValue(2, 0)) / (4 * qw); s = 0.5f / s;
qz = (m.getValue(1, 0) - m.getValue(0, 1)) / (4 * qw); qx = (m.getValue(2, 1) - m.getValue(1, 2)) * s;
qy = (m.getValue(0, 2) - m.getValue(2, 0)) * s;
qz = (m.getValue(1, 0) - m.getValue(0, 1)) * s;
} else { } else {
float sqx = -0.5f * (m.getValue(1, 1) + m.getValue(2, 2)); int h = 0;
qx = sqrt(sqx); if (m.getValue(1, 1) > m.getValue(0, 0))
if (sqx > 0.0f) { h = 1;
qy = m.getValue(0, 1) / (2.0f * qx); if (m.getValue(2, 2) > m.getValue(h, h))
qz = m.getValue(0, 2) / (2.0f * qx); h = 2;
if (h == 0) {
s = sqrt(m.getValue(0, 0) - (m.getValue(1,1) + m.getValue(2, 2)) + 1.0f);
qx = s * 0.5f;
s = 0.5f / s;
qy = (m.getValue(0, 1) + m.getValue(1, 0)) * s;
qz = (m.getValue(2, 0) + m.getValue(0, 2)) * s;
qw = (m.getValue(2, 1) - m.getValue(1, 2)) * s;
} else if (h == 1) {
s = sqrt(m.getValue(1, 1) - (m.getValue(2,2) + m.getValue(0, 0)) + 1.0f);
qy = s * 0.5f;
s = 0.5f / s;
qz = (m.getValue(1, 2) + m.getValue(2, 1)) * s;
qx = (m.getValue(0, 1) + m.getValue(1, 0)) * s;
qw = (m.getValue(0, 2) - m.getValue(2, 0)) * s;
} else { } else {
float sqy = 0.5f * (1.0f - m.getValue(2, 2)); s = sqrt(m.getValue(2, 2) - (m.getValue(0,0) + m.getValue(1, 1)) + 1.0f);
if (sqy > 0.0f) { qz = s * 0.5f;
qy = sqrtf(sqy); s = 0.5f / s;
qz = m.getValue(1, 2) / (2.0f * qy); qx = (m.getValue(2, 0) + m.getValue(0, 2)) * s;
} else { qy = (m.getValue(1, 2) + m.getValue(2, 1)) * s;
qy = 0.0f; qw = (m.getValue(1, 0) - m.getValue(0, 1)) * s;
qz = 1.0f;
}
} }
} }
set(qx, qy, qz, qw); set(qx, qy, qz, qw);