C# Quaternion.ToAxisAngle方法代码示例

        public Cube3D()
            : base(600, 400, GraphicsMode.Default, "1-2:Camera")
        {
            VSync = VSyncMode.On;

            Keyboard.KeyUp += (sender, e) =>
            {
                //Escapeキーで終了
                if (e.Key == Key.Escape)
                {
                    this.Exit();
                }
            };

            RenderFrame += (sender, e) =>
            {
                GL.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit);

                GL.MatrixMode(MatrixMode.Modelview);
                Vector3 eyepoint = new Vector3(0, 0, -3);
                Matrix4 modelview = Matrix4.LookAt(eyepoint, Vector3.UnitZ, Vector3.UnitY);
                GL.LoadMatrix(ref modelview);

                Quaternion q = new Quaternion(Form1.AHRS.Quaternion[1],
                    Form1.AHRS.Quaternion[2],
                    Form1.AHRS.Quaternion[3],
                    Form1.AHRS.Quaternion[0]);
                q.ToAxisAngle(out vec3, out angle);
                GL.Rotate((float)(angle * 180 / Math.PI), vec3);

                //GL.Rotate(deg, Vector3d.UnitY);
                //deg += 1;

                DrawCube();

                SwapBuffers();
            };
        }

		static public Vector3 QuaternionToEuler(Quaternion rotation)
		{
			Vector4 v = rotation.ToAxisAngle();
			return new Vector3(v.X, v.Y, v.Z);
		}

        //Mouse drag, calculate rotation
        private Matrix4 Drag(Point newPosition)
        {
            //Only allows rotation around x and y, z is not allowed
            //Map the point to the sphere
            MapToSphere(newPosition, ref _endVector);
            //Calculate x-axis rotate angle
            Vector3 xStartVector = new Vector3(0, _startVector.Y, _startVector.Z);
            Vector3 xEndVector = new Vector3(0, _endVector.Y, _endVector.Z);

            Vector3 xPerp;
            Vector3.Cross(ref xStartVector, ref xEndVector, out xPerp);
            Quaternion xRotation = new Quaternion();
            //Compute the length of the perpendicular vector
            if (xPerp.Length > Epsilon)
            //if its non-zero
            {
                //We're ok, so return the perpendicular vector as the transform after all
                xRotation.X = xPerp.X;
                xRotation.Y = xPerp.Y;
                xRotation.Z = xPerp.Z;
                //In the quaternion values, w is cosine (theta / 2), where theta is the rotation angle
                xRotation.W = Vector3.Dot(xStartVector, xEndVector);
            }
            //if it is zero
            else
            {
                //The begin and end vectors coincide, so return an identity transform
                xRotation.X = xRotation.Y = xRotation.Z = 0.0f;
                xRotation.W = 1.0f;
            }
            float xAngle;
            Vector3 xAxis;
            xRotation.ToAxisAngle(out xAxis, out xAngle);
            float xDegree = MathHelper.RadiansToDegrees(xAngle);
            xDegree = xDegree *0.01f;
            xAngle = MathHelper.DegreesToRadians(xDegree);
            var xMatrix = Matrix4.CreateFromAxisAngle(xAxis, xAngle); //Matrix4.Rotate(rotation);

            //float xAngle = (float)Math.Acos(Vector3.Dot(xStartVector, xEndVector));
            //float xdegree = MathHelper.RadiansToDegrees(xAngle);
            //xdegree = xdegree / 10;
            //xAngle = MathHelper.DegreesToRadians(xdegree);
            //Matrix4 xMatrix = Matrix4.CreateFromAxisAngle(Vector3.UnitX, xAngle);
            //Calculate y_axis rotate angle
            Vector3 yStartVector = new Vector3(_startVector.X, 0, _startVector.Z);
            Vector3 yEndVector = new Vector3(_endVector.X, 0, _endVector.Z);

            Vector3 yPerp;
            Vector3.Cross(ref yStartVector, ref yEndVector, out yPerp);
            Quaternion yRotation = new Quaternion();
            //Compute the length of the perpendicular vector
            if (yPerp.Length > Epsilon)
            //if its non-zero
            {
                //We're ok, so return the perpendicular vector as the transform after all
                yRotation.X = yPerp.X;
                yRotation.Y = yPerp.Y;
                yRotation.Z = yPerp.Z;
                //In the quaternion values, w is cosine (theta / 2), where theta is the rotation angle
                yRotation.W = Vector3.Dot(yStartVector, yEndVector);
            }
            //if it is zero
            else
            {
                //The begin and end vectors coincide, so return an identity transform
                yRotation.X = yRotation.Y = yRotation.Z = 0.0f;
                yRotation.W = 1.0f;
            }
            float yAngle;
            Vector3 yAxis;
            yRotation.ToAxisAngle(out yAxis, out yAngle);
            float yDegree = MathHelper.RadiansToDegrees(yAngle);
            yDegree = yDegree *0.01f ;
            yAngle = MathHelper.DegreesToRadians(yDegree);
            var yMatrix = Matrix4.CreateFromAxisAngle(yAxis, yAngle); //Matrix4.Rotate(rotation);

            //float yAngle = (float)Math.Acos(Vector3.Dot(yStartVector, yEndVector));
            //float ydegree = MathHelper.RadiansToDegrees(yAngle);
            //ydegree = ydegree / 10;
            //yAngle = MathHelper.DegreesToRadians(ydegree);
            //Matrix4 yMatrix = Matrix4.CreateFromAxisAngle(Vector3.UnitY, yAngle);
            //z_axis is not allowed for rotation
            ////////////////////////////////
            return yMatrix;
            //return rotateMatrix;
            //return Matrix4.Mult(yMatrix, xMatrix);

            ////Return the quaternion equivalent to the rotation

            //Vector3 perp;

            ////Compute the vector perpendicular to the begin and end vectors
            //Vector3.Cross(ref _startVector, ref _endVector, out perp);
            //Quaternion rotation = new Quaternion();
            ////Compute the length of the perpendicular vector
            //if (perp.Length > Epsilon)
            ////if its non-zero
            //{
            //    //We're ok, so return the perpendicular vector as the transform after all
//.........这里部分代码省略.........

 /// <summary>
 /// Build a rotation matrix from a quaternion
 /// </summary>
 /// <param name="q">the quaternion</param>
 /// <returns>A rotation matrix</returns>
 public static Matrix4 Rotate(Quaternion q)
 {
     float3 axis;
     float angle;
     q.ToAxisAngle(out axis, out angle);
     return Rotate(axis, angle);
 }

        public void Small_Changes_Single_Axis()
        {
            var q1 = Quaternion.FromAxisAngle(new AxisAngle { Axis = Vector3.UnitX, Angle = Angle.FromDegrees(0.1) });

            var q2 = new Quaternion();

            for (var i = 0; i < 2700; i++)
            {
                q2 *= q1;
            }

            var a = q2.ToAxisAngle();

            Assert.Equal(270, a.Angle.RawDegrees.SigFig(3));
            Assert.Equal(Vector3.UnitX, a.Axis.SigFig(3));
        }