IrrlichtNETCP.Quaternion.FromMatrix C# (CSharp) Method

        public Quaternion FromMatrix(Matrix4 m)
        {
            float diag = m.GetM(0,0) + m.GetM(1,1) + m.GetM(2,2) + 1;
            float scale = 0.0f;

            if (diag > 0.0f)
            {
                scale = (float)Math.Sqrt(diag) * 2.0f; // get scale from diagonal

                // TODO: speed this up
                m_x = (m.GetM(2,1) - m.GetM(1,2)) / scale;
                m_y = (m.GetM(0,2) - m.GetM(2,0)) / scale;
                m_z = (m.GetM(1,0) - m.GetM(0,1)) / scale;
                m_w = 0.25f * scale;
            }
            else
            {
                if (m.GetM(0,0) > m.GetM(1,1) && m.GetM(0,0) > m.GetM(2,2))
                {
                    // 1st element of diag is greatest value
                    // find scale according to 1st element, and double it
                    scale = (float)Math.Sqrt(1.0f + m.GetM(0,0) - m.GetM(1,1) - m.GetM(2,2)) * 2.0f;

                    // TODO: speed this up
                    m_x = 0.25f * scale;
                    m_y = (m.GetM(0,1) + m.GetM(1,0)) / scale;
                    m_z = (m.GetM(2,0) + m.GetM(0,2)) / scale;
                    m_w = (m.GetM(2,1) - m.GetM(1,2)) / scale;
                }
                else if (m.GetM(1,1) > m.GetM(2,2))
                {
                    // 2nd element of diag is greatest value
                    // find scale according to 2nd element, and double it
                    scale = (float)Math.Sqrt(1.0f + m.GetM(1,1) - m.GetM(0,0) - m.GetM(2,2)) * 2.0f;

                    // TODO: speed this up
                    m_x = (m.GetM(0,1) + m.GetM(1,0)) / scale;
                    m_y = 0.25f * scale;
                    m_z = (m.GetM(1,2) + m.GetM(2,1)) / scale;
                    m_w = (m.GetM(0,2) - m.GetM(2,0)) / scale;
                }
                else
                {
                    // 3rd element of diag is greatest value
                    // find scale according to 3rd element, and double it
                    scale = (float)Math.Sqrt(1.0f + m.GetM(2,2) - m.GetM(0,0) - m.GetM(1,1)) * 2.0f;

                    // TODO: speed this up
                    m_x = (m.GetM(0,2) + m.GetM(2,0)) / scale;
                    m_y = (m.GetM(1,2) + m.GetM(2,1)) / scale;
                    m_z = 0.25f * scale;
                    m_w = (m.GetM(1,0) - m.GetM(0,1)) / scale;
                }
            }

            Normalize();
            return this;
        }