63 lines
1.8 KiB
C
63 lines
1.8 KiB
C
#include <c3d/maths.h>
|
|
|
|
C3D_FQuat Quat_FromMtx(const C3D_Mtx* m)
|
|
{
|
|
// Original algorithm taken from here (with some optimizations):
|
|
// https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf
|
|
// Layout of the algorithm:
|
|
// First, we select a large (non-zero!) component "P" in the output quaternion (q)
|
|
// (we can test this just by looking at the diagonal of the matrix)
|
|
// Second, we calculate q' which is our desired output quaternion (q) scaled by 4P
|
|
// (this can be done with simple additions; the 4P factor is large and non-zero thanks to above)
|
|
// Third, we normalize q' to finally obtain q
|
|
// (this will work because normalize(kq) = q for any k scalar and q unit quaternion)
|
|
|
|
C3D_FQuat q;
|
|
C3D_FVec diagonal = FVec4_New(m->r[0].x, m->r[1].y, m->r[2].z, 1.0f);
|
|
|
|
// Check if x^2 + y^2 >= z^2 + w^2
|
|
if (diagonal.z <= 0.0f)
|
|
{
|
|
// Check if |x| >= |y|
|
|
if (diagonal.x >= diagonal.y)
|
|
{
|
|
// X case
|
|
q.x = diagonal.w + diagonal.x - diagonal.y - diagonal.z;
|
|
q.y = m->r[1].x + m->r[0].y;
|
|
q.z = m->r[2].x + m->r[0].z;
|
|
q.w = m->r[2].y - m->r[1].z;
|
|
}
|
|
else
|
|
{
|
|
// Y case
|
|
q.x = m->r[1].x + m->r[0].y;
|
|
q.y = diagonal.w - diagonal.x + diagonal.y - diagonal.z;
|
|
q.z = m->r[2].y + m->r[1].z;
|
|
q.w = m->r[0].z - m->r[2].x;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Check if |z| >= |w|
|
|
if (-diagonal.x >= diagonal.y)
|
|
{
|
|
// Z case
|
|
q.x = m->r[2].x + m->r[0].z;
|
|
q.y = m->r[2].y + m->r[1].z;
|
|
q.z = diagonal.w - diagonal.x - diagonal.y + diagonal.z;
|
|
q.w = m->r[1].x - m->r[0].y;
|
|
}
|
|
else
|
|
{
|
|
// W case
|
|
q.x = m->r[2].y - m->r[1].z;
|
|
q.y = m->r[0].z - m->r[2].x;
|
|
q.z = m->r[1].x - m->r[0].y;
|
|
q.w = diagonal.w + diagonal.x + diagonal.y + diagonal.z;
|
|
}
|
|
}
|
|
|
|
// Normalize the quaternion
|
|
return Quat_Normalize(q);
|
|
}
|