C# Quaternion.Normalize方法代码示例

        private void CalculateCurrentQuaternion(double timeFactor)
        {
            if (LERPActivated)
            {
                var x = startQuaternion.X * (1 - timeFactor) + endQuaternion.X * timeFactor;
                var y = startQuaternion.Y * (1 - timeFactor) + endQuaternion.Y * timeFactor;
                var z = startQuaternion.Z * (1 - timeFactor) + endQuaternion.Z * timeFactor;
                var w = startQuaternion.W * (1 - timeFactor) + endQuaternion.W * timeFactor;
                currentQuaternion = new Quaternion(x, y, z, w);
                currentQuaternion.Normalize();
            }
            else
            {
                currentQuaternion = Quaternion.Slerp(startQuaternion, endQuaternion, timeFactor);

                double dotProduct = startQuaternion.X * endQuaternion.Y + startQuaternion.Y * endQuaternion.Y
                    + startQuaternion.Z * endQuaternion.Z + startQuaternion.W * endQuaternion.W;

                var theta = Math.Acos(dotProduct);
                if (theta < 0.0) theta = -theta;

                var startFactor = Math.Sin((1 - timeFactor) * theta) / Math.Sin(theta);
                var endFactor = Math.Sin(timeFactor * theta) / Math.Sin(theta);
                var x = startFactor * startQuaternion.X + endFactor * endQuaternion.X;
                var y = startFactor * startQuaternion.Y + endFactor * endQuaternion.Y;
                var z = startFactor * startQuaternion.Z + endFactor * endQuaternion.Z;
                var w = startFactor * startQuaternion.W + endFactor * endQuaternion.W;
                currentQuaternion = new Quaternion(x, y, z, w);
                currentQuaternion.Normalize();
            }
        }

        public static Quaternion getQuaternion(Vector3D v0, Vector3D v1)
        {
            Quaternion q = new Quaternion();
            // Copy, since cannot modify local
            v0.Normalize();
            v1.Normalize();

            double d = Vector3D.DotProduct(v0, v1);
            // If dot == 1, vectors are the same
            if (d >= 1.0f)
            {
                return Quaternion.Identity;
            }

            double s = Math.Sqrt((1 + d) * 2);
            double invs = 1 / s;

            Vector3D c = Vector3D.CrossProduct(v0, v1);

            q.X = c.X * invs;
            q.Y = c.Y * invs;
            q.Z = c.Z * invs;
            q.W = s * 0.5f;
            q.Normalize();

            return q;
        }

 public void CalculateCurrentQuaternion(ref Quaternion currentQuaternion, double timeFactor)
 {
     var x = startQuaternion.X * (1 - timeFactor) + endQuaternion.X * timeFactor;
     var y = startQuaternion.Y * (1 - timeFactor) + endQuaternion.Y * timeFactor;
     var z = startQuaternion.Z * (1 - timeFactor) + endQuaternion.Z * timeFactor;
     var w = startQuaternion.W * (1 - timeFactor) + endQuaternion.W * timeFactor;
     currentQuaternion = new Quaternion(x, y, z, w);
     currentQuaternion.Normalize();
 }

        /// <summary>
        /// Функция возвращает кватернион вращения по положению двух векторов.
        /// </summary>
        /// <param name="v0">Трехмерный вектор</param>
        /// <param name="v1">Трехмерный вектор</param>
        /// <returns>Объект кватерниона</returns>
        public static Quaternion GetQuaternion(Vector3D v0, Vector3D v1)
        {
            Quaternion q = new Quaternion();
            v0.Normalize();
            v1.Normalize();
            double d = Vector3D.DotProduct(v0, v1);
            if (d >= 1.0f)  return Quaternion.Identity;

            double s = Math.Sqrt((1 + d) * 2);
            double invs = 1 / s;

            Vector3D c = Vector3D.CrossProduct(v0, v1);
            q.X = c.X * invs;
            q.Y = c.Y * invs;
            q.Z = c.Z * invs;
            q.W = s * 0.5f;
            q.Normalize();
            return q;
        }

        private void ApplyBetaAngleToAxes(double OldAngle, double NewAngle)
        {
            double netAngle;
            Matrix3D transformation = Matrix3D.Identity;
            Quaternion betaAngleRotation = Quaternion.Identity;

            netAngle = NewAngle - OldAngle;

            if (netAngle == 0)
                return;

            // Compute the transformation
            betaAngleRotation = new Quaternion(this.LongitudinalAxis, netAngle);
            betaAngleRotation.Normalize();
            transformation.Rotate(betaAngleRotation);

            // Apply the transformation
            this.MajorAxis = transformation.Transform(this.MajorAxis);
            this.MinorAxis = transformation.Transform(this.MinorAxis);
        }

 private void ResetButton_Click(object sender, RoutedEventArgs e)
 {
     dispatcherTimer.Stop();
     ApplyChanges.IsEnabled = true;
     var quat = new Quaternion(CurrentQuaternionX, CurrentQuaternionY, CurrentQuaternionZ, CurrentQuaternionW);
     quat.Normalize();
     CurrentQuaternionX = quat.X;
     CurrentQuaternionY = quat.Y;
     CurrentQuaternionZ = quat.Z;
     CurrentQuaternionW = quat.W;
     CalculateMass();
     CalculateDiagonalLength();
     CalculateGravityForceVector();
     CalculateCubeTensor();
     time = 0;
     SetupBaseODEVector();
     SetupCube();
     TrajectoryPoints.Clear();
     if (HelixViewport.Children.IndexOf(FrameStartManipulator) < 0)
         HelixViewport.Children.Add(FrameStartManipulator);
 }

        private void DispatcherTimerTick(object sender, EventArgs e)
        {
            time += TimeDelta;
            var odeSolverData = new ODESolverData(cubeTensor, invertCubeTensor);
            var ODEVector = new double[] { time - TimeDelta, time };
            var yNew = SolveODESystem(yODEVector, ODEVector, odeSolverData);
            for (int i = 0; i < 3; i++)
                yODEVector[i] = yNew[1, i];

            var quat = new Quaternion(yNew[1, 3], yNew[1, 4], yNew[1, 5], yNew[1, 6]);
            quat.Normalize();
            //FrameStartCube.Transform = new RotateTransform3D(new QuaternionRotation3D(quat));
            var tg = new Transform3DGroup();
            tg.Children.Add(new RotateTransform3D(new QuaternionRotation3D(quat)));
            tg.Children.Add(new RotateTransform3D(new QuaternionRotation3D(
                new Quaternion(CurrentQuaternionX, CurrentQuaternionY, CurrentQuaternionZ, CurrentQuaternionW))));
            FrameStartCube.Transform = tg;
            diagonalArrow.Transform = FrameStartCube.Transform;

            var diagonalRotation = MatrixVectorMultiply(diagonalArrow.Transform.Value, diagonalArrow.Direction);
            diagonalRotation.Normalize();
            var endPoint = new Point3D(diagonalRotation.X * cubeDiagonalLength,
                diagonalRotation.Y * cubeDiagonalLength, diagonalRotation.Z * cubeDiagonalLength);
            trajectoryPoints.Add(endPoint);
            if(time - TimeDelta > 0)
                trajectoryPoints.Add(endPoint);
            if (trajectoryPoints.Count > TrajectoryLength * 2 - 1)
            {
                trajectoryPoints.RemoveAt(0);
                trajectoryPoints.RemoveAt(0);
            }

            yODEVector[3] = quat.X;
            yODEVector[4] = quat.Y;
            yODEVector[5] = quat.Z;
            yODEVector[6] = quat.W;
        }

        private static Quaternion FromToRotation(Vector3D from, Vector3D to)
        {
            from.Normalize();
            to.Normalize();

            var d = Vector3D.DotProduct(from, to);

            if (d >= 1.0f)
            {
                // In the case where the two vectors are pointing in the same
                // direction, we simply return the identity rotation.
                return Quaternion.Identity;
            }
            else if (d <= -1.0f)
            {
                // If the two vectors are pointing in opposite directions then we
                // need to supply a quaternion corresponding to a rotation of
                // PI-radians about an axis orthogonal to the fromDirection.
                var axis = Vector3D.CrossProduct(from, new Vector3D(1, 0, 0));
                if (axis.LengthSquared < 1e-6)
                {
                    // Bad luck. The x-axis and fromDirection are linearly
                    // dependent (colinear). We'll take the axis as the vector
                    // orthogonal to both the y-axis and fromDirection instead.
                    // The y-axis and fromDirection will clearly not be linearly
                    // dependent.
                    axis = Vector3D.CrossProduct(from, new Vector3D(0, 1, 0));
                }

                // Note that we need to normalize the axis as the cross product of
                // two unit vectors is not nececessarily a unit vector.
                axis.Normalize();
                return new Quaternion(axis, 180);
            }
            else
            {
                // Scalar component.
                var s = Math.Sqrt(from.LengthSquared * to.LengthSquared) + Vector3D.DotProduct(from, to);

                // Vector component.
                var v = Vector3D.CrossProduct(from, to);

                // Return the normalized quaternion rotation.
                var rotation = new Quaternion(v.X, v.Y, v.Z, s);
                rotation.Normalize();

                return rotation;
            }
        }

        public bool TickData(Vector lastMouse, bool isFlipped)
        {
            bool somethingChanged = false;

            //active means nothing in the "flipped" mode
            bool isActiveItem = IsMouseOver && !isFlipped;
            bool goodMouse = lastMouse.IsValid();

            #region rotation

            Quaternion rotationTarget = new Quaternion(new Vector3D(1, 0, 0), 0);

            //apply forces
            rotationTarget.Normalize();
            m_rotationCurrent.Normalize();

            double angle = 0;
            Vector3D axis = new Vector3D(0, 0, 1);
            if (lastMouse.IsValid() && !isFlipped)
            {
                Point3D mouse = new Point3D(lastMouse.X, lastMouse.Y, 1);
                Vector3D line = mouse - m_locationCurrent;
                Vector3D straight = new Vector3D(0, 0, 1);

                angle = Vector3D.AngleBetween(line, straight);
                axis = Vector3D.CrossProduct(line, straight);
            }
            Quaternion rotationForceTowardsMouse = new Quaternion(axis, -angle);

            Quaternion rotationForceToDesired = rotationTarget - m_rotationCurrent;

            Quaternion rotationForce = rotationForceToDesired + rotationForceTowardsMouse;

            m_rotationVelocity *= new Quaternion(rotationForce.Axis, rotationForce.Angle * .2);

            //dampenning
            m_rotationVelocity = new Quaternion(m_rotationVelocity.Axis, m_rotationVelocity.Angle * (m_weird - .3));

            //apply terminal velocity
            m_rotationVelocity = new Quaternion(m_rotationVelocity.Axis, m_rotationVelocity.Angle);

            m_rotationVelocity.Normalize();

            //apply to position
            m_rotationCurrent *= m_rotationVelocity;
            m_rotationCurrent.Normalize();

            //see if there is any real difference between what we calculated and what actually exists
            if (AnyDiff(m_quaternionRotation3D.Quaternion.Axis, m_rotationCurrent.Axis, c_diff) ||
                AnyDiff(m_quaternionRotation3D.Quaternion.Angle, m_rotationCurrent.Angle, c_diff))
            {
                //if the angles are both ~0, the axis may be way off but the result is basically the same
                //check for this and forget animating in this case
                if (AnyDiff(m_quaternionRotation3D.Quaternion.Angle, 0, c_diff) || AnyDiff(m_rotationCurrent.Angle, 0, c_diff))
                {
                    m_quaternionRotation3D.Quaternion = m_rotationCurrent;
                    somethingChanged = true;
                }
            }

            #endregion

            #region flip
            double verticalFlipTarget = isFlipped ? 180 : 0;
            double verticalFlipCurrent = m_verticalFlipRotation.Angle;

            //force
            double verticalFlipForce = verticalFlipTarget - verticalFlipCurrent;

            //velocity
            m_flipVerticalVelocity += .3 * verticalFlipForce;

            //dampening
            m_flipVerticalVelocity *= (m_weird - .3);

            //terminal velocity
            m_flipVerticalVelocity = limitDouble(m_flipVerticalVelocity, 10);

            //apply
            verticalFlipCurrent += m_flipVerticalVelocity;

            if (AnyDiff(verticalFlipCurrent, m_verticalFlipRotation.Angle, c_diff) && AnyDiff(m_flipVerticalVelocity, 0, c_diff))
            {
                m_verticalFlipRotation.Angle = verticalFlipCurrent;
            }

            #endregion

            #region scale
            if (isActiveItem && !isFlipped)
            {
                this.m_scaleDesired = 2;
            }
            else
            {
                this.m_scaleDesired = 1;
            }

            double scaleForce = this.m_scaleDesired - this.m_scaleCurrent;
            this.m_scaleVelocity += .1 * scaleForce;
//.........这里部分代码省略.........

        private Matrix3D findRotation(Vector3D u, Vector3D v)
        {
            Matrix3D rotationMatrix = new Matrix3D();
            Quaternion rotationQuat = new Quaternion();

            Vector3D cross = Vector3D.CrossProduct(u, v);
            rotationQuat.X = cross.X;
            rotationQuat.Y = cross.Y;
            rotationQuat.Z = cross.Z;
            rotationQuat.W = Math.Sqrt(u.LengthSquared * v.LengthSquared) + Vector3D.DotProduct(u, v);
            rotationQuat.Normalize();

            QuaternionRotation3D tempRotation = new QuaternionRotation3D(rotationQuat);
            RotateTransform3D tempTransform = new RotateTransform3D(tempRotation);
            rotationMatrix = tempTransform.Value;  //Going through RotateTransform3D is kind of a hacky way to do this...

            return rotationMatrix;
        }

 private void RotateWork()
 {
     currentQ = new Quaternion(SensorAFilter.Quaternion.X, SensorAFilter.Quaternion.Y, SensorAFilter.Quaternion.Z, SensorAFilter.Quaternion.W);
     currentQ.Normalize();
     imu.Transform = new RotateTransform3D(new QuaternionRotation3D(startQ - currentQ), CUBE_CENTER);
 }