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;
        }
Esempio n. 2
0
 public static void SetMatrix(GraphicsMatrix which, Matrix4 matrix)
 {
     if (!Toolkit.utSetRenderMatrix((Toolkit.utRenderMatrix)which, ref matrix.M00))
         throw new FrameworkException();
 }