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)
{
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);
w = 0.25f / s;
float st = std::sqrt(trace + 1.0f);
float s = 0.5f / st;
w = 0.5f * st;
x = (mat[1][2] - mat[2][1]) * s;
y = (mat[2][0] - mat[0][2]) * s;
z = (mat[0][1] - mat[1][0]) * s;
}
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]);
w = (mat[1][2] - mat[2][1]) / s;
x = 0.25f * s;
y = (mat[1][0] + mat[0][1]) / s;
z = (mat[2][0] + mat[0][2]) / s;
idx = 1;
if (mat[2][2] > mat[1][1])
idx = 2;
}
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]);
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;
idx = 2;
}
else
switch (idx)
{
float s = 2.0f * std::sqrt(1.0f + mat[2][2] - mat[0][0] - mat[1][1]);
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.25f * s;
case 0:
{
float st = std::sqrt(mat[0][0] - (mat[1][1] + mat[2][2]) + 1.f);
float s = 0.5f / st;
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;
}
}
}