歐拉角和旋轉矩陣的相互轉換

歐拉角-->旋轉矩陣

旋轉矩陣-->歐拉角

 

 

 //由旋轉矩陣計算歐拉角
        private double[] rotationMatrixToEulerAngles(double[] M)
        {
            double R00 = M[0], R01 = M[1], R02 = M[2];
            double R10 = M[4], R11 = M[5], R12 = M[6];
            double R20 = M[8], R21 = M[9], R22 = M[10];

            double sy = Math.Sqrt(R00 * R00 + R10 * R10);

            bool singular = sy < 1e-6; // If

            double x, y, z;
            if (!singular)
            {
                x = Math.Atan2(R21, R22);
                y = Math.Atan2(-R20, sy);
                z = Math.Atan2(R10, R00);
            }
            else
            {
                x = Math.Atan2(-R12, R11);
                y = Math.Atan2(-R20, sy);
                z = 0;
            }
            x = x * 180.0 / Math.PI;
            y = y * 180.0 / Math.PI;
            z = z * 180.0 / Math.PI;
            double[] angle = new double[3] { x, y, z };
            return angle;
        }