コード例 #1
0
            public static EulerRot ToEulerAngles(double x, double y, double z, double w)
            {
                double[,] matRot = Matrix.MakeRotationFromQuaternion(x, y, z, w);

                EulerRot result = new EulerRot();

                result.y = Math.Asin(Clamp(matRot[0, 2], -1, 1));

                if (Math.Abs(matRot[0, 2]) < 0.99999999)
                {
                    result.x = Math.Atan2(-matRot[1, 2], matRot[2, 2]);
                    result.z = Math.Atan2(-matRot[0, 1], matRot[0, 0]);
                }
                else
                {
                    result.x = Math.Atan2(matRot[2, 1], matRot[2, 2]);
                    result.z = 0;
                }

                result.x = RadianToDegree(result.x);
                result.y = RadianToDegree(result.y);
                result.z = RadianToDegree(result.z);

                return(result);
            }
コード例 #2
0
            static public EulerRot GetEulerAnglesFromMatrix(double[,] matrix)
            {
                double nz1, nz2, nz3;

                nz3 = Math.Sqrt(matrix[1, 0] * matrix[1, 0] + matrix[1, 1] * matrix[1, 1]);
                nz1 = (-matrix[1, 0] * matrix[1, 2]) / nz3;
                nz2 = (-matrix[1, 1] * matrix[1, 2]) / nz3;

                double vx = nz1 * matrix[0, 0] + nz2 * matrix[0, 1] + nz3 * matrix[0, 2];
                double vz = nz1 * matrix[2, 0] + nz2 * matrix[2, 1] + nz3 * matrix[2, 2];

                EulerRot retval = new EulerRot();

                retval.x = RadianToDegree(Math.Asin(Clamp(matrix[1, 2], -1, 1)));
                retval.y = -RadianToDegree(Math.Atan2(vx, vz));
                retval.z = -RadianToDegree(Math.Atan2(matrix[1, 0], matrix[1, 1]));

                return(retval);
            }