示例#1
0
        public void AddAngles(Vector3 angles)
        {
            var a = MatrixMath.GetRotationMatrix(X, Y, Z);
            var b = MatrixMath.GetRotationMatrix(angles.X, angles.Y, angles.Z);

            decimal[,] angleMatrix = MatrixMath.Multiply3v3(b, a);
            double yaw = Math.Atan2((double)angleMatrix[1, 0], (double)angleMatrix[0, 0]) / Math.PI * 180.0;

            angleMatrix = MatrixMath.Multiply3v3(MatrixMath.GetZRotation(Convert.ToDecimal(-yaw)), angleMatrix);
            double pitch = Math.Atan2((double)-angleMatrix[2, 0], (double)angleMatrix[0, 0]) / Math.PI * 180.0;
            double roll  = Math.Atan2((double)-angleMatrix[1, 2], (double)angleMatrix[1, 1]) / Math.PI * 180.0;

            X = Convert.ToDecimal(pitch - Math.Floor(pitch / 360.0) * 360.0);
            Y = Convert.ToDecimal(yaw - Math.Floor(yaw / 360.0) * 360.0);
            Z = Convert.ToDecimal(roll - Math.Floor(roll / 360.0) * 360.0);
        }
 public static decimal[,] GetRotationMatrix(decimal Pitch, decimal Yaw, decimal Roll)
 {
     return(MatrixMath.Multiply3v3(MatrixMath.GetZRotation(Yaw), MatrixMath.Multiply3v3(MatrixMath.GetYRotation(Pitch), MatrixMath.GetXRotation(Roll))));
 }