IGL_INLINE void igl::mat3_to_quat(const Q_type * mat, Q_type * q)
{
Q_type trace;
Q_type s;
Q_type t;
int i;
int j;
int k;
static int next[3] = { 1, 2, 0 };
trace = mat[0 * 3 + 0] + mat[1 * 3 + 1] + mat[2 * 3 + 2];
if ( trace > 0.0f ) {
t = trace + 1.0f;
s = ReciprocalSqrt( t ) * 0.5f;
q[3] = s * t;
q[0] = ( mat[1 * 3 + 2] - mat[2 * 3 + 1] ) * s;
q[1] = ( mat[2 * 3 + 0] - mat[0 * 3 + 2] ) * s;
q[2] = ( mat[0 * 3 + 1] - mat[1 * 3 + 0] ) * s;
} else {
i = 0;
if ( mat[1 * 3 + 1] > mat[0 * 3 + 0] ) {
i = 1;
}
if ( mat[2 * 3 + 2] > mat[i * 3 + i] ) {
i = 2;
}
j = next[i];
k = next[j];
t = ( mat[i * 3 + i] - ( mat[j * 3 + j] + mat[k * 3 + k] ) ) + 1.0f;
s = ReciprocalSqrt( t ) * 0.5f;
q[i] = s * t;
q[3] = ( mat[j * 3 + k] - mat[k * 3 + j] ) * s;
q[j] = ( mat[i * 3 + j] + mat[j * 3 + i] ) * s;
q[k] = ( mat[i * 3 + k] + mat[k * 3 + i] ) * s;
}
//// Unused translation
//jq.t[0] = mat[0 * 4 + 3];
//jq.t[1] = mat[1 * 4 + 3];
//jq.t[2] = mat[2 * 4 + 3];
}
void CMatrix::RotationPart(CQuater& q) {
float fT = m[E_11] + m[E_22] + m[E_33];
float fS;
if (fT > 0.0) {
fT += 1.0;
fS = (float)(ReciprocalSqrt(fT) * 0.5);
q.w = fS * fT;
q.x = (m[E_32] - m[E_23]) * fS;
q.y = (m[E_13] - m[E_31]) * fS;
q.z = (m[E_21] - m[E_12]) * fS;
} else if (m[E_11] > m[E_22] && m[E_11] > m[E_33]) {
fT = m[E_11] - m[E_22] - m[E_33] + (float)1.0;
fS = (float)(ReciprocalSqrt(fT) * 0.5);
q.w = (m[E_32] - m[E_23]) * fS;
q.x = fS * fT;
q.y = (m[E_21] + m[E_12]) * fS;
q.z = (m[E_13] + m[E_31]) * fS;
} else if (m[E_22] > m[E_33]) {
fT = -m[E_11] + m[E_22] - m[E_33] + (float)1.0;
fS = (float)(ReciprocalSqrt(fT) * 0.5);
q.w = (m[E_13] - m[E_31]) * fS;
q.x = (m[E_21] + m[E_12]) * fS;
q.y = fS * fT;
q.z = (m[E_32] + m[E_23]) * fS;
} else {
fT = -m[E_11] - m[E_22] + m[E_33] + (float)1.0;
fS = (float)(ReciprocalSqrt(fT) * 0.5);
q.w = (m[E_21] - m[E_12]) * fS;
q.x = (m[E_13] + m[E_31]) * fS;
q.y = (m[E_32] + m[E_23]) * fS;
q.z = fS * fT;
}
}
