public FixQuaternion(ref FixTrans3 matrix)
{
Fix determinant = (matrix.M11 * (matrix.M22 * matrix.M33 - matrix.M32 * matrix.M23)) -
(matrix.M12 * (matrix.M21 * matrix.M33 - matrix.M31 * matrix.M23)) +
(matrix.M13 * (matrix.M21 * matrix.M32 - matrix.M31 * matrix.M22));
Fix scale = FixMath.Pow(determinant, Fix.One / 3);
Fix x, y, z;
w = (FixMath.Sqrt(FixMath.Max(0, scale + matrix.M11 + matrix.M22 + matrix.M33)) / 2);
x = (FixMath.Sqrt(FixMath.Max(0, scale + matrix.M11 - matrix.M22 - matrix.M33)) / 2);
y = (FixMath.Sqrt(FixMath.Max(0, scale - matrix.M11 + matrix.M22 - matrix.M33)) / 2);
z = (FixMath.Sqrt(FixMath.Max(0, scale - matrix.M11 - matrix.M22 + matrix.M33)) / 2);
xyz = new FixVec3(x, y, z);
if (matrix.M32 - matrix.M23 < 0)
{
X = -X;
}
if (matrix.M13 - matrix.M31 < 0)
{
Y = -Y;
}
if (matrix.M21 - matrix.M12 < 0)
{
Z = -Z;
}
}
//https://gamedev.stackexchange.com/questions/50963/how-to-extract-euler-angles-from-transformation-matrix
public FixVec3 EulerAngle()
{
FixVec3 ea = new FixVec3();
ea.x = FixMath.Atan2(-m[1, 2], m[2, 2]);
Fix cosYangle = FixMath.Sqrt(FixMath.Pow(m[0, 0], 2) + FixMath.Pow(m[0, 1], 2));
ea.y = FixMath.Atan2(m[0, 2], cosYangle);
Fix sinXangle = FixMath.Sin(ea.x);
Fix cosXangle = FixMath.Cos(ea.x);
ea.z = FixMath.Atan2((cosXangle * m[1, 0]) + (sinXangle * m[2, 0]), (cosXangle * m[1, 1]) + (sinXangle * m[2, 1]));
return(ea);
}
