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的方法!!!

        >
        /// <summary>
        /// 将旋转矩阵转换回Yaw,Pitch,Roll
        /// </summary>
        /// <param name="rotation">旋转矩阵</param>
        /// <returns>X是Yaw,Y是Pitch,Z是Roll</returns>
        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创建一个新的矩阵来避免误差!!

Leave a Reply

Your email address will not be published. Required fields are marked *