Exemple #1
0
        public static Quaternion GetQuaternion(MatrixByArr mat, double scale)
        {
            //   [a b c]   [q0*q0 + q1*q1 - q2*q2 - q3*q3,	2*(q1*q2 - q0*q3),				2*(q1*q3 + q0*q2)            ]
            // s*[d e f] = [2*(q1*q2 + q0*q3),				q0*q0 + q2*q2 - q1*q1 - q3*q3,	2*(q2*q3 - q0*q1)            ]
            //   [g h i]   [2*(q1*q3 - q0*q2),				2*(q2*q3 + q0*q1),				q0*q0 + q3*q3 - q1*q1 - q2*q2]
            //
            // q0*q0 + q1*q1 + q2*q2 + q3*q3 = 1
            // s(a+e) = 2*(q0*q0 - q3*q3)
            // s(a+i) = 2*(q0*q0 - q2*q2)
            // s(e+i) = 2*(q0*q0 - q1*q1)
            // s(d+b) = 4 * q1*q2
            // s(d-b) = 4 * q0*q3
            // s(c+g) = 4 * q1*q3
            // s(c-g) = 4 * q0*q2
            // s(h+f) = 4 * q2*q3
            // s(h-f) = 4 * q0*q1
            //
            // if we assume 's == 1',
            //    then (a+e+i) = 3*q0*q0 - (q1*q1 + q2*q2 + q3*q3) = 3*q0*q0 - (1-q0*q0) = 4*q0*q0 - 1
            //         q0 = sqrt((a + e + i + 4)/4)
            //         q1 = (h - f)/(4 * q0)
            //         q2 = (c - g)/(4 * q0)
            //         q3 = (d - b)/(4 * q0)
            HDebug.Assert(scale == 1);
            HDebug.Assert(mat.ColSize == 3, mat.RowSize == 3);
            double q0 = Math.Sqrt((mat[0, 0] + mat[1, 1] + mat[2, 2] + 4) / 4);

            HDebug.Assert(double.IsNaN(q0) == false);
            HDebug.Assert(q0 != 0);
            double q1 = (mat[2, 1] - mat[1, 2]) / (4 * q0);
            double q2 = (mat[0, 2] - mat[2, 0]) / (4 * q0);
            double q3 = (mat[1, 0] - mat[0, 1]) / (4 * q0);

            HDebug.AssertSimilar((mat[0, 0] + mat[1, 1]), 2 * (q0 * q0 - q3 * q3), 0.001);
            HDebug.AssertSimilar((mat[0, 0] + mat[2, 2]), 2 * (q0 * q0 - q2 * q2), 0.001);
            HDebug.AssertSimilar((mat[1, 1] + mat[2, 2]), 2 * (q0 * q0 - q1 * q1), 0.001);
            HDebug.AssertSimilar((mat[1, 0] + mat[0, 1]), (4 * q1 * q2), 0.001);
            HDebug.AssertSimilar((mat[1, 0] - mat[0, 1]), (4 * q0 * q3), 0.001);
            HDebug.AssertSimilar((mat[0, 2] + mat[2, 0]), (4 * q1 * q3), 0.001);
            HDebug.AssertSimilar((mat[0, 2] - mat[2, 0]), (4 * q0 * q2), 0.001);
            HDebug.AssertSimilar((mat[2, 1] + mat[1, 2]), (4 * q2 * q3), 0.001);
            HDebug.AssertSimilar((mat[2, 1] - mat[1, 2]), (4 * q0 * q1), 0.001);
            Quaternion quater = new Quaternion(q0, q1, q2, q3);

            if (HDebug.IsDebuggerAttached)
            {
                MatrixByArr  rotmat    = quater.RotationMatrix;
                const double tolerance = 0.001;
                HDebug.Assert(Math.Abs(rotmat[0, 0] - mat[0, 0]) < tolerance);
                HDebug.Assert(Math.Abs(rotmat[0, 1] - mat[0, 1]) < tolerance);
                HDebug.Assert(Math.Abs(rotmat[0, 2] - mat[0, 2]) < tolerance);
                HDebug.Assert(Math.Abs(rotmat[1, 0] - mat[1, 0]) < tolerance);
                HDebug.Assert(Math.Abs(rotmat[1, 1] - mat[1, 1]) < tolerance);
                HDebug.Assert(Math.Abs(rotmat[1, 2] - mat[1, 2]) < tolerance);
                HDebug.Assert(Math.Abs(rotmat[2, 0] - mat[2, 0]) < tolerance);
                HDebug.Assert(Math.Abs(rotmat[2, 1] - mat[2, 1]) < tolerance);
                HDebug.Assert(Math.Abs(rotmat[2, 2] - mat[2, 2]) < tolerance);
            }
            return(quater);
            //////////////////////////////////////////////////////////////////////////
            // (sqrt(3)*q0 + q1 + q2 + q3) * (sqrt(3)*q0 - q1 - q2 - q3)
            //   = (3*q0*q0 - q1*q1 - q2*q2 - q3*q3) - 2*(q1*q2 + q2*q3 + q1*q3)
            //   = (a+e+i)                           - (d+b + c+g + h+f)/2
            //
            // (q0 - q1 + q2 - q3) * (a0 - q1 - q2 + q3)
            //   = (q0*q0 + q1*q1 - q2*q2 - q3*q3) + 2*(q0*q1 + q2*q3)
            //   = a + h/2
        }