AxioDL
/
zeus
mirror of https://github.com/AxioDL/zeus.git
2
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rewrite Matrix to Quaternion conversion

This commit is contained in:
Jack Andersen 2017-11-29 15:22:14 -10:00
parent 5b3ab7ac22
commit 9a22f0c5ed
1 changed files with 49 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
include/zeus/CQuaternion.hpp
View File

@ -93,39 +93,67 @@ public:
CQuaternion(const CMatrix3f& mat) CQuaternion(const CMatrix3f& mat)
{ {
float trace = mat[0][0] + mat[1][1] + mat[2][2]; float trace = mat[0][0] + mat[1][1] + mat[2][2];
if (trace > 0) if (trace >= 0.f)
{ {
float s = 0.5f / std::sqrt(trace + 1.0f); float st = std::sqrt(trace + 1.0f);
w = 0.25f / s; float s = 0.5f / st;
w = 0.5f * st;
x = (mat[1][2] - mat[2][1]) * s; x = (mat[1][2] - mat[2][1]) * s;
y = (mat[2][0] - mat[0][2]) * s; y = (mat[2][0] - mat[0][2]) * s;
z = (mat[0][1] - mat[1][0]) * s; z = (mat[0][1] - mat[1][0]) * s;
} }
else else
{ {
if (mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) int idx = 0;
if (mat[1][1] > mat[0][0])
{ {
float s = 2.0f * std::sqrt(1.0f + mat[0][0] - mat[1][1] - mat[2][2]); idx = 1;
w = (mat[1][2] - mat[2][1]) / s; if (mat[2][2] > mat[1][1])
x = 0.25f * s; idx = 2;
y = (mat[1][0] + mat[0][1]) / s;
z = (mat[2][0] + mat[0][2]) / s;
} }
else if (mat[1][1] > mat[2][2]) else if (mat[2][2] > mat[0][0])
{ {
float s = 2.0f * std::sqrt(1.0f + mat[1][1] - mat[0][0] - mat[2][2]); idx = 2;
w = (mat[2][0] - mat[0][2]) / s;
x = (mat[1][0] + mat[0][1]) / s;
y = 0.25f * s;
z = (mat[2][1] + mat[1][2]) / s;
} }
else
switch (idx)
{ {
float s = 2.0f * std::sqrt(1.0f + mat[2][2] - mat[0][0] - mat[1][1]); case 0:
w = (mat[0][1] - mat[1][0]) / s; {
x = (mat[2][0] + mat[0][2]) / s; float st = std::sqrt(mat[0][0] - (mat[1][1] + mat[2][2]) + 1.f);
y = (mat[2][1] + mat[1][2]) / s; float s = 0.5f / st;
z = 0.25f * s; w = (mat[1][2] - mat[2][1]) * s;
x = 0.5f * st;
y = (mat[1][0] + mat[0][1]) * s;
z = (mat[2][0] + mat[0][2]) * s;
break;
}
case 1:
{
float st = std::sqrt(mat[1][1] - (mat[2][2] + mat[0][0]) + 1.f);
float s = 0.5f / st;
w = (mat[2][0] - mat[0][2]) * s;
x = (mat[1][0] + mat[0][1]) * s;
y = 0.5f * st;
z = (mat[2][1] + mat[1][2]) * s;
break;
}
case 2:
{
float st = std::sqrt(mat[2][2] - (mat[0][0] + mat[1][1]) + 1.f);
float s = 0.5f / st;
w = (mat[0][1] - mat[1][0]) * s;
x = (mat[2][0] + mat[0][2]) * s;
y = (mat[2][1] + mat[1][2]) * s;
z = 0.5f * st;
break;
}
default:
w = 0.f;
x = 0.f;
y = 0.f;
z = 0.f;
break;
} }
} }
} }