XNA将旋转矩阵转换回Yaw,Pitch和Roll的方法

XNA中能通过Matrix.CreateFromYawPitchRoll创建旋转矩阵，众所周知，Yaw是绕Y轴旋转，Pitch是绕X轴旋转，Roll是绕Z轴旋转

因为方便,大地无敌一直用旋转矩阵的转化而不用yaw,pitch和roll来表示物体的旋转。
但是，有些时候需要获得物体的旋转数据，以及避免只使用矩阵旋转的累积的误差（嗯，比如矩阵变得不只是旋转矩阵了），该如何是好呢？当然是反求出yaw,pitch和roll啦~~~~
在网上查了众多资料后，参考了http://planning.cs.uiuc.edu/node103.html
解决了旋转系统的差别后，大地无敌写了一个将矩阵退回Yaw Pitch Roll的方法！！！

        >
        /// 

        /// 将旋转矩阵转换回Yaw,Pitch,Roll
        /// 
        /// 旋转矩阵
        /// X是Yaw，Y是Pitch，Z是Roll
        public static Vector3 MatrixToYawPitchRoll(Matrix rotation)
        {
            float yaw = 0;
            float pitch = 0;
            float roll = 0;

            //求yaw
            if (rotation.M11 != 0)
            {

                roll = -(float)Math.Atan2(rotation.M21 , rotation.M11);
            }
            else
            {
                roll = -Math.Sign(rotation.M21) * MathHelper.PiOver2;
            }

            //求Pitch
            if (rotation.M32 != 0 || rotation.M33 != 0)
            {
                double x = Math.Sqrt(Math.Pow(rotation.M32, 2) + Math.Pow(rotation.M33, 2));
                yaw = -(float)Math.Atan2(-rotation.M31, x);
            }
            else
            {
                yaw = -Math.Sign(rotation.M31) * MathHelper.PiOver2 * (-1);
            }

            //求roll
            if (rotation.M33 != 0)
            {
                pitch = -(float)Math.Atan2(rotation.M32, rotation.M33);
            }
            else
            {
                pitch = -Math.Sign(rotation.M32) * MathHelper.PiOver2;
            }

            return new Vector3(yaw, pitch, roll);
        }

、、

 

然后，就可以得到Yaw,Pitch和Roll进行其它操作，或用该Yaw,Pitch和Roll创建一个新的矩阵来避免误差！！