ChronoEngine_SwAddin.SWTaskpaneHost.GetQuaternionFromMatrix C# (CSharp) Method

        public double[] GetQuaternionFromMatrix(ref MathTransform trasf)
        {
            double[] q = new double[4];
            double[] amatr = (double[])trasf.ArrayData;
            double s, tr;
                                        // for speed reasons: ..
            double m00 = amatr[0];
            double m01 = amatr[3];
            double m02 = amatr[6];
            double m10 = amatr[1];
            double m11 = amatr[4];
            double m12 = amatr[7];
            double m20 = amatr[2];
            double m21 = amatr[5];
            double m22 = amatr[8];

            tr=m00 + m11 + m22;		// diag sum

            if (tr >= 0)
            {
                s = Math.Sqrt(tr + 1.0);
                q[0] = 0.5 * s;
                s = 0.5 / s;
                q[1] = (m21 - m12) * s;
                q[2] = (m02 - m20) * s;
                q[3] = (m10 - m01) * s;
            }
            else
            {
                int i = 0;

                if (m11 > m00)
                {
                    i = 1;
                    if (m22 > m11)	i = 2;
                }
                else
                {
                    if (m22 > m00)  i = 2;
                }

                switch (i)
                {
                case 0:
                    s = Math.Sqrt(m00 - m11 - m22 + 1);
                    q[1] = 0.5 * s;
                    s = 0.5 / s;
                    q[2] = (m01 + m10) * s;
                    q[3] = (m20 + m02) * s;
                    q[0] = (m21 - m12) * s;
                    break;
                case 1:
                    s = Math.Sqrt(m11 - m22 - m00 + 1);
                    q[2] = 0.5 * s;
                    s = 0.5 / s;
                    q[3] = (m12 + m21) * s;
                    q[1] = (m01 + m10) * s;
                    q[0] = (m02 - m20) * s;
                    break;
                case 2:
                    s = Math.Sqrt(m22 - m00 - m11 + 1);
                    q[3] = 0.5 * s;
                    s = 0.5 / s;
                    q[1] = (m20 + m02) * s;
                    q[2] = (m12 + m21) * s;
                    q[0] = (m10 - m01) * s;
                    break;
                }
            }
            return q;
        }