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