// Multiplication
Quat4& Quat4::operator *= (Quat4& B)
{
Quat4 A = Quat4(this);
q[0] = A[0]*B[0] - A[1]*B[1] - A[2]*B[2] - A[3]*B[3];
q[1] = A[0]*B[1] + A[1]*B[0] + A[2]*B[3] - A[3]*B[2];
q[2] = A[0]*B[2] - A[1]*B[3] + A[2]*B[0] + A[3]*B[1];
q[3] = A[0]*B[3] + A[1]*B[2] - A[2]*B[1] + A[3]*B[0];
return *this;
}
Quat4 Quat4::fromMatrix(float m00, float m01, float m02,
float m10, float m11, float m12,
float m20, float m21, float m22) {
// taken from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
float tr = m00 + m11 + m22;
float w,x,y,z;
if (tr > 0) {
float S = (float)sqrt(tr + 1.0) * 2; // S=4*qw
w = 0.25f * S;
x = (m21 - m12) / S;
y = (m02 - m20) / S;
z = (m10 - m01) / S;
} else if ((m00 > m11) & (m00 > m22)) {
float S = (float)sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx
w = (m21 - m12) / S;
x = 0.25f * S;
y = (m01 + m10) / S;
z = (m02 + m20) / S;
} else if (m11 > m22) {
float S = (float)sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy
w = (m02 - m20) / S;
x = (m01 + m10) / S;
y = 0.25f * S;
z = (m12 + m21) / S;
} else {
float S = (float)sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz
w = (m10 - m01) / S;
x = (m02 + m20) / S;
y = (m12 + m21) / S;
z = 0.25f * S;
}
return Quat4(w,x,y,z);
}
Quat4 Quat4::operator * (Quat4& B)
{
Quat4 A = Quat4(this);
A *= B;
return A;
}
