bool Matrix4_InverseT(Type *out16, Type const *in16)
{
Type det = Matrix4_DeterminantT(in16);
if(de::abs(det) < .0005f)
{
Matrix4<Type> identity;
identity.data().get(0, reinterpret_cast<IByteArray::Byte *>(out16),
identity.data().size());
return false;
}
for(int i = 0; i < 4; i++)
{
for(int j = 0; j < 4; j++)
{
Type sub[3*3];
int sign = 1 - ((i + j) % 2) * 2;
Matrix4_SubmatrixT(in16, sub, i, j);
out16[i + j*4] = (Matrix3_DeterminantT(sub) * sign) / det;
}
}
return true;
}
Quaternion Quaternion::matrixToQuaternion(Matrix4 matrix) {
float t, x, y, z;
float *m = matrix.data();
Quaternion q;
float trace = m[0] + m[5] + m[10];
if (trace > 0) {// I changed M_EPSILON to 0
float s = 0.5f / sqrtf(trace + 1.0f);
q._t = 0.25f / s;
q._x = (m[6] - m[9]) * s;
q._y = (m[8] - m[2]) * s;
q._z = (m[1] - m[4]) * s;
} else {
if (m[0] > m[5] && m[0] > m[10]) {
float s = 2.0f * sqrtf(1.0f + m[0] - m[5] - m[10]);
q._t = (m[6] - m[9]) / s;
q._x = 0.25f * s;
q._y = (m[4] + m[1]) / s;
q._z = (m[8] + m[2]) / s;
} else if (m[5] > m[10]) {
float s = 2.0f * sqrtf(1.0f + m[5] - m[0] - m[10]);
q._t = (m[8] - m[2]) / s;
q._x = (m[4] + m[1]) / s;
q._y = 0.25f * s;
q._z = (m[9] + m[6]) / s;
} else {
float s = 2.0f * sqrtf(1.0f + m[10] - m[0] - m[5]);
q._t = (m[1] - m[4]) / s;
q._x = (m[8] + m[2]) / s;
q._y = (m[9] + m[6]) / s;
q._z = 0.25f * s;
}
}
return q;
}