public static Quaternion FromMatrix(Matrix4 m) { Quaternion result; /* From http://www.euclideanspace.com/ */ float trace = m.M00 + m.M11 + m.M22 + 1.0f; if (trace > 0.00001f) { float s = 0.5f / (float)Math.Sqrt(trace); result.W = 0.25f / s; result.X = (m.M21 - m.M12) * s; result.Y = (m.M02 - m.M20) * s; result.Z = (m.M10 - m.M01) * s; } else { if (m.M00 > m.M11 && m.M00 > m.M22) { float s = 2.0f * (float)Math.Sqrt(1.0f + m.M00 - m.M11 - m.M22); result.X = 0.25f * s; result.Y = (m.M01 + m.M10) / s; result.Z = (m.M02 + m.M20) / s; result.W = (m.M12 - m.M21) / s; } else if (m.M11 > m.M22) { float s = 2.0f * (float)Math.Sqrt(1.0f + m.M11 - m.M00 - m.M22); result.X = (m.M01 + m.M10) / s; result.Y = 0.25f * s; result.Z = (m.M12 + m.M21) / s; result.W = (m.M02 - m.M20) / s; } else { float s = 2.0f * (float)Math.Sqrt(1.0f + m.M22 - m.M00 - m.M11); result.X = (m.M02 + m.M20) / s; result.Y = (m.M12 + m.M21) / s; result.Z = 0.25f * s; result.W = (m.M01 - m.M10) / s; } } return result; }
public static void SetMatrix(GraphicsMatrix which, Matrix4 matrix) { if (!Toolkit.utSetRenderMatrix((Toolkit.utRenderMatrix)which, ref matrix.M00)) throw new FrameworkException(); }