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); }