C# Quaternion.Normalize方法代碼示例

        private Quaternion GetRotation(Vector3 src, Vector3 dest)
        {
            src.Normalize();
            dest.Normalize();

            float d = Vector3.Dot(src, dest);

            if (d >= 1f)
            {
                return Quaternion.Identity;
            }
            else if (d < (1e-6f - 1.0f))
            {
                Vector3 axis = Vector3.Cross(Vector3.UnitX, src);

                if (axis.LengthSquared() == 0)
                {
                    axis = Vector3.Cross(Vector3.UnitY, src);
                }

                axis.Normalize();
                return Quaternion.CreateFromAxisAngle(axis, MathHelper.Pi);
            }
            else
            {
                float s = (float)Math.Sqrt((1 + d) * 2);
                float invS = 1 / s;

                Vector3 c = Vector3.Cross(src, dest);
                Quaternion q = new Quaternion(invS * c, 0.5f * s);
                q.Normalize();

                return q;
            }
        }

		/// <summary>
		/// Construct a new transform.
		/// </summary>
		/// <param name="scale">The change in scale multiplier.</param>
		/// <param name="position">The change in position.</param>
		/// <param name="orientation">The transform's orientation.</param>
		public Transform(float scale, ref Vector3 position, ref Quaternion orientation)
		{
			Scale = scale;
			Position = position;
			Orientation = orientation;
			Orientation.Normalize();
			Combine(Scale, ref Position, ref Orientation, out Combined);
		}

        public void RotateGameObject(ref GameObject toBeRotated)
        {
            Vector3 axis = toBeRotated.RotationAxis;
            float angle = MathHelper.ToRadians(toBeRotated.RotationAngleInDegrees);
            float angleHalf = angle / 2;
            float sinAngleHalf = (float)Math.Sin(angleHalf);
            Quaternion rotationQuaternion = new Quaternion(axis.X * sinAngleHalf, axis.Y * sinAngleHalf, axis.Z * sinAngleHalf, (float)Math.Cos(angleHalf));
            rotationQuaternion.Normalize();

            Matrix rotationMatrix = Matrix.CreateFromQuaternion(rotationQuaternion);

            toBeRotated.World *= rotationMatrix;
        }

        public PlayerShip(List<Collidable> collidableRef, CollidableType type, float boundingRadius, Model model, Vector3 shipPosition, GraphicsDevice device, Vector3 cameraFollowPosition, float scale, float minShipSpeed, float maxShipSpeed, float acceleration, float turningSpeed,
            float maxTurningSpeed, float minTurningSpeed, WeaponSystem2D weaponSystem2D, MissleSystem2D missleSystem2D, SpawnerManager spawnerManager, PlayerConfig[] playerConfigurations)
        {
            CollidableReference = collidableRef;

            _shipModel = new BasicModel(model, scale);

            ShipWorld = Matrix.CreateTranslation(shipPosition) * Matrix.CreateScale(scale);
            _dockingPosition = shipPosition;
            ShipPosition = shipPosition;

            _shipRotation = Quaternion.Identity;

            Camera = new Camera(device, cameraFollowPosition);

            _keyboard = new KeyboardInput();

            _minShipSpeed = minShipSpeed;
            _maxShipSpeed = maxShipSpeed;
            _acceleration = acceleration;
            _turningSpeed = turningSpeed;
            _gamePadInput = new GamePadInput();

            _weaponSystem2D = weaponSystem2D;
            _device = device;
            _shipRotation.Normalize();

            //Collidable Properties
            this.CollidableType = type;
            this.BoundingSphereRadius = boundingRadius;

            _maxTurningSpeed = maxTurningSpeed;
            _minTurningSpeed = minTurningSpeed;

            Life = 0;

            _soundDump = new List<Sound>();

            spawnerManager.Selected += (object sender, EventArgs args) =>
                {
                    SpawnerManager SM = sender as SpawnerManager;
                    Spawn(SM._selected);
                };

            _missleSystem2D = missleSystem2D;

            _playerConfigurations = playerConfigurations;
        }

        /// <summary>
        /// Computes an arc rotation from two vectors
        /// </summary>
        /// <param name="origin">Origin</param>
        /// <param name="destination">Destination</param>
        /// <param name="fallback"></param>
        /// <param name="rotation">Quaternion containing an arc rotation</param>
        public static void GetArcRotation(ref Vector3 origin, ref Vector3 destination, ref Vector3 fallback, out Quaternion rotation)
        {
            Vector3 start = origin;
            Vector3 end = destination;
            start.Normalize();
            end.Normalize();

            float dot;
            Vector3.Dot(ref start, ref end, out dot);
            if (dot >= 1.0f)
            {
                rotation = Quaternion.Identity;
                return;
            }

            if (dot < (1e-06f - 1.0f))
            {
                if (fallback != Vector3.Zero)
                    Quaternion.CreateFromAxisAngle(ref fallback, MathHelper.ToRadians(MathHelper.Pi), out rotation);
                else
                {
                    Vector3 axis = Vector3.Cross(Vector3.UnitZ, start);
                    if (axis.IsZeroLength()) axis = Vector3.Cross(Vector3.UnitY, start);
                    axis.Normalize();
                    Quaternion.CreateFromAxisAngle(ref axis, MathHelper.ToRadians(MathHelper.Pi), out rotation);
                }
            }
            else
            {
                float s = (float)Math.Sqrt((1.0f + dot) * 2.0f);
                float invS = 1.0f / s;
                Vector3 cross;
                Vector3.Cross(ref start, ref end, out cross);
            #if XBOX || XBOX360
                rotation = new Quaternion();
            #endif
                rotation.X = cross.X * invS;
                rotation.Y = cross.Y * invS;
                rotation.Z = cross.Z * invS;
                rotation.W = s * 0.5f;
                rotation.Normalize();
            }
        }

        private void HandleUserInput(float dt)
        {
            Translation = Vector3.Zero;

            float deltaX = -_input.Mouse.DeltaX;
            float deltaY = -_input.Mouse.DeltaY;
            float scale = RotationSpeed * dt;

            _rotation = Quaternion.Multiply(Quaternion.CreateFromAxisAngle(Vector3.Up,deltaX * scale),Quaternion.Multiply(_rotation, Quaternion.CreateFromYawPitchRoll(0, deltaY * scale,0)));
            _rotation.Normalize();
            //m_rotationMat = Matrix.Multiply(Matrix.CreateFromYawPitchRoll(deltaX * scale, deltaY * scale, 0.0f),m_rotationMat);

            if (_input.Keyboard.QPressedAndHeld) Translation += Vector3.Left;
            if (_input.Keyboard.WPressedAndHeld) Translation += Vector3.Forward;
            if (_input.Keyboard.EPressedAndHeld) Translation += Vector3.Right;
            if (_input.Keyboard.SPressedAndHeld) Translation += Vector3.Backward;
            if (_input.Keyboard.SpacePressedAndHeld) Translation += _input.Keyboard.ShiftPressed ? Vector3.Down : Vector3.Up;
            Translation = Translation * TranslationSpeed * dt;
        }

        protected virtual void createViewMatrix()
        {
            Quaternion qPitch = Quaternion.CreateFromAxisAngle(Vector3.UnitX, MathHelper.ToRadians(_pitch));
            Quaternion qYaw = Quaternion.CreateFromAxisAngle(Vector3.UnitY, MathHelper.ToRadians(_yaw));
            _orientation = qYaw*qPitch;

            _orientation.Normalize();
            _view = Matrix.CreateFromQuaternion(_orientation);
            _view.Translation = _position;
            Matrix.Invert(ref _view, out _view);
        }

 /// <summary>
 /// QuaternionAngle returns the amount of rotation in the given quaternion, in radians.
 /// </summary>
 /// <param name="rotation">Input quaternion.</param>
 /// <returns>Returns rotation angle in radians</returns>
 public static float QuaternionAngle(Quaternion rotation)
 {
     rotation.Normalize();
     float angle = 2.0f * (float)Math.Acos(rotation.W);
     return angle;
 }

        /// <summary>
        /// Performs the DES algorithm.
        /// </summary>
        protected void ComputeDESMatrix(ref Vector3 p, ref Quaternion q, out Matrix result)
        {
            // If translational alpha is 1, then no need to perform smoothing
            if (transAlpha == 1)
                transSt = p;
            else
            {
                // Compute the translational smoothing
                transSt = transAlpha * p + (1 - transAlpha) * (transPrevSt + transPrevBt);
                transBt = transGamma * (transSt - transPrevSt) + (1 - transGamma) * transPrevBt;

                transPrevSt = transSt;
                transPrevBt = transBt;
            }

            // If rotational alpha is 1, then no need to perform smoothing
            if (rotAlpha == 1)
                rotSt = q;
            else
            {
                rotSt = Quaternion.Slerp(rotPrevBt, q, rotAlpha);
                rotBt = Quaternion.Slerp(rotPrevBt, rotSt, rotGamma);

                rotSt.Normalize();
                rotBt.Normalize();

                rotPrevSt = rotSt;
                rotPrevBt = rotBt;
            }

            Matrix.CreateFromQuaternion(ref rotSt, out tmpMat1);
            Matrix.CreateTranslation(ref transSt, out tmpMat2);
            Matrix.Multiply(ref tmpMat1, ref tmpMat2, out result);
        }

        public void Normalize()
        {
            Quaternion q = new Quaternion(1, 2, 3, 4);
            Quaternion expected = new Quaternion(0.1825742f,0.3651484f,0.5477226f,0.7302967f);

            Compare(expected, Quaternion.Normalize(q));

            Quaternion result;
            Quaternion.Normalize(ref q, out result);
            Compare(expected, result);


            q.Normalize();
            Compare(expected, q);
        }

        /// <summary>
        /// Call this method once per frame to update the internal state of the
        /// Entity object.
        /// </summary>
        /// <param name="gameTime">The elapsed game time.</param>
        public void Update(GameTime gameTime)
        {
            float elapsedTimeSec = (float)gameTime.ElapsedGameTime.TotalSeconds;

            Vector3 velocityElapsed;
            Vector3 eulerOrientElapsed;
            Vector3 eulerRotateElapsed;
            Vector3 oldPos;
            Vector3 heading;
            Quaternion temp;

            velocityElapsed = velocity * elapsedTimeSec;
            eulerOrientElapsed = eulerOrient * elapsedTimeSec;
            eulerRotateElapsed = eulerRotate * elapsedTimeSec;

            // Update the entity's position.

            ExtractAxes();

            oldPos = position;

            position += right * velocityElapsed.X;
            position += up * velocityElapsed.Y;
            position += forward * velocityElapsed.Z;

            heading = position - oldPos;
            heading.Normalize();

            // Update the entity's orientation.

            temp = EulerToQuaternion(orientation, eulerOrientElapsed.Y,
                    eulerOrientElapsed.X, eulerOrientElapsed.Z);

            // When moving backwards invert rotations to match direction of travel.

            if (Vector3.Dot(heading, forward) < 0.0f)
                temp = Quaternion.Inverse(temp);

            orientation = Quaternion.Concatenate(orientation, temp);
            orientation.Normalize();

            // Update the entity's free rotation.

            temp = EulerToQuaternion(rotation, eulerRotateElapsed.Y,
                    eulerRotateElapsed.X, eulerRotateElapsed.Z);

            rotation = Quaternion.Concatenate(rotation, temp);
            rotation.Normalize();

            // Update the entity's world matrix.

            temp = Quaternion.Concatenate(rotation, orientation);
            temp.Normalize();

            Matrix.CreateFromQuaternion(ref temp, out worldMatrix);
            worldMatrix.M41 = position.X;
            worldMatrix.M42 = position.Y;
            worldMatrix.M43 = position.Z;

            // Clear the cached Euler rotations and velocity for this frame.

            velocity.X = velocity.Y = velocity.Z = 0.0f;
            eulerOrient.X = eulerOrient.Y = eulerOrient.Z = 0.0f;
            eulerRotate.X = eulerRotate.Y = eulerRotate.Z = 0.0f;
        }

        /// <summary>
        /// Sets the world matrix
        /// </summary>
        /// <param name="time">The game time variable</param>
        /// <param name="m">Rotation matrix</param>
        public virtual void setWorldMatrix(float time, Matrix m)
        {
            #region "Rotation"
            Quaternion pitch = Quaternion.CreateFromAxisAngle(m.Right, MathHelper.ToRadians(shipData.pitch) * time * pitchSpeed);
            Quaternion roll = Quaternion.CreateFromAxisAngle(m.Backward, MathHelper.ToRadians(shipData.roll) * time * rollSpeed);
            Quaternion yaw = Quaternion.CreateFromAxisAngle(m.Up, MathHelper.ToRadians(shipData.yaw) * time * yawSpeed);

            rotate = yaw * pitch * roll * rotate;
            rotate.Normalize();
            #endregion

            world = Matrix.CreateScale(shipData.scale) * Matrix.CreateFromQuaternion(rotate);
            world.Translation = Position;

            shipData.resetAngles();
        }

        private void Rotate(Quaternion qrot)
        {
            q = Quaternion.Multiply(qrot, q);
            q.Normalize();

            focus = Vector3.Transform(focusConst, q);
            up = Vector3.Transform(upConst, q);
        }

        private void timer1_Tick(object sender, EventArgs e)
        {
            if (port2.IsOpen)
            {
                pictureBox1.BackColor = System.Drawing.Color.SpringGreen;
            }
            else
            {
                pictureBox1.BackColor = System.Drawing.Color.Red;
            }
            if (port.IsOpen)
            {
                pictureBox2.BackColor = System.Drawing.Color.SpringGreen;
            }
            else
            {
                pictureBox2.BackColor = System.Drawing.Color.Red;
            }

            int pom;
            char[] ch = new char[8];
            int[] serv = new int[6];
            Vector3 eul;
            Quaternion qu = new Quaternion();
            qu.W = 1;
            qu.X = 0;
            qu.Y = -1;
            qu.Z = 0;
            qu.Normalize();

            serv[0] = (int)(ad[0] * 0.5 + ad[1] * 0.5);
            serv[1] = ad[2];
            serv[2] = ad[4];
            serv[3] = ad[6];
            serv[4] = ad[8];

            quat_out = Quaternion.Multiply(Quaternion.Conjugate(quat2), Quaternion.Multiply(quat1, qu)); //Quaternion.Multiply(Quaternion.Conjugate(quat2), quat1);

            eul = FromQ2(quat_out);
            if (eul.Z > 200)
            {
                serv[5] = (int)((eul.Z) / (double)0.990);
                if (serv[5] < 0)
                { serv[5] = 0; }
                if (serv[5] > 100)
                { serv[5] = 100; }
            }

            richTextBox5.Text = serv[0].ToString();
            richTextBox6.Text = serv[1].ToString();
            richTextBox7.Text = serv[2].ToString();
            richTextBox8.Text = serv[3].ToString();
            richTextBox9.Text = serv[4].ToString();
            richTextBox10.Text = serv[5].ToString();

            ch[0] = (char)101;
            for (pom = 1; pom < 7; pom++)
            {
                ch[pom] = Convert.ToChar(serv[(pom - 1)]);
            }
            ch[7] = (char)101;
            if (port2.IsOpen)
            {
                port2.Write(ch, 0, 8);
            }

            richTextBox1.Text = quat_out.W.ToString();
            richTextBox2.Text = quat_out.X.ToString();
            richTextBox3.Text = quat_out.Y.ToString();
            richTextBox4.Text = quat_out.Z.ToString();

            richTextBox11.Text = eul.Z.ToString();
            richTextBox12.Text = eul.Y.ToString();
            richTextBox13.Text = eul.X.ToString();
        }

        // helper method to convert quaternions to Euler angles
        protected void QuaternionToEulerAngles(
            ref Quaternion rotation, ref float pitch,
            ref float yaw, ref float roll)
        {
            // following code assumes normalized quat
            rotation.Normalize();

            // temp variables to save some typing
            float q0 = rotation.X;
            float q1 = rotation.Y;
            float q2 = rotation.Z;
            float q3 = rotation.W;

            // test for naughty cases (anomalies at north and south poles)
            float check = q0 * q1 + q2 * q3;

            if (Math.Abs(check) >= 0.499)
            {
                // special case to avoid unreliable data near poles
                check /= Math.Abs(check); // determine sign (1 or -1)

                // calculate angles for special case
                pitch = check * (float)Math.PI / 2.0f;
                yaw = check * 2.0f * (float)Math.Atan(q0 * q3);
                roll = check * 0.0f;
            }
            else
            {
                // looks good; calculate angles for typical case
                pitch = (float)Math.Asin(-2 * (q1 * q3 - q0 * q2));
                yaw = (float)Math.Atan2(2 * (q1 * q2 + q0 * q3),
                    q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3);
                roll = (float)Math.Atan2(2 * (q2 * q3 + q0 * q1),
                    q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3);
            }

            // we're done; convert back to degrees
            pitch = MathHelper.ToDegrees(pitch);
            yaw = MathHelper.ToDegrees(yaw);
            roll = MathHelper.ToDegrees(roll);
        }