C# Quaternion.ToUnit方法代码示例

        private void OrbitCamera(Point currentPosition, Vector3D currentPosition3D)
        {
            #region Get Mouse Movement - Spherical

            // Figure out a rotation axis and angle
            Vector3D axis = Vector3D.CrossProduct(_previousPosition3D, currentPosition3D);
            double angle = Vector3D.AngleBetween(_previousPosition3D, currentPosition3D);

            // Quaterion will throw if this happens - sometimes we can get 3D positions that are very similar, so we
            // avoid the throw by doing this check and just ignoring the event 
            if (axis.Length == 0)
            {
                return;
            }

            // Now need to rotate the axis into the camera's coords
            // Get the camera's current view matrix.
            Matrix3D viewMatrix = MathUtils.GetViewMatrix(_camera);
            viewMatrix.Invert();

            // Transform the trackball rotation axis relative to the camera orientation.
            axis = viewMatrix.Transform(axis);

            Quaternion deltaRotation = new Quaternion(axis, -angle);
            Quaternion deltaRotationExternal = new Quaternion(axis, angle);

            #endregion

            // This can't be calculated each mose move.  It causes a wobble when the look direction isn't pointed directly at the origin
            //if (_orbitRadius == null)
            //{
            //    _orbitRadius = OnGetOrbitRadius();
            //}

            // Figure out the offset in world coords
            Vector3D lookLine = _camera.LookDirection;
            lookLine.Normalize();
            lookLine = lookLine * _camera.Position.ToVector().Length;		//_orbitRadius.Value;		// the camera is always pointed to the origin, so this shortcut works

            Point3D orbitPointWorld = _camera.Position + lookLine;

            // Get the opposite of the look line (the line from the orbit center to the camera's position)
            Vector3D lookLineOpposite = lookLine * -1d;

            // Rotate
            Vector3D[] vectors = new Vector3D[] { lookLineOpposite, _camera.UpDirection, _camera.LookDirection };

            deltaRotation.GetRotatedVector(vectors);

            // Apply the changes
            _camera.Position = orbitPointWorld + vectors[0];
            _camera.UpDirection = vectors[1];
            _camera.LookDirection = vectors[2];

            _quaternion = _quaternion.ToUnit() * deltaRotationExternal.ToUnit();
            _transform = new RotateTransform3D(new QuaternionRotation3D(_quaternion));

            OnRotationChanged();
        }

 /// <summary>
 /// This returns the current quaternion rotated by the delta
 /// </summary>
 /// <remarks>
 /// This method is really simple, but I'm tired of trial and error with multiplication order every time I need
 /// to rotate quaternions
 /// </remarks>
 public static Quaternion RotateBy(this Quaternion quaternion, Quaternion delta)
 {
     //return delta.ToUnit() * quaternion.ToUnit();		// this is the one that's backward (I think)
     return quaternion.ToUnit() * delta.ToUnit();
 }

        private void GetJointBodyPair(out Body body1, out Body body2, AddJointBodyType bodyType1, AddJointBodyType bodyType2, Point3D centerPoint, Quaternion rotation, double separationDistance, Color color)
        {
            #region Body 1

            double distanceToPermiter;
            Quaternion localRotation;
            GetJointBodyPairSprtOffset(out distanceToPermiter, out localRotation, bodyType1);

            Vector3D offset = new Vector3D(distanceToPermiter + (separationDistance / 2d), 0, 0);		// adding to the separation distance because I don't want centerpoint to centerpoint, I want edge to edge
            offset = rotation.GetRotatedVector(offset);

            Point3D shiftedCenter = centerPoint + offset;


            localRotation = rotation.ToUnit() * localRotation.ToUnit();
            RotateTransform3D finalRotation = new RotateTransform3D(new QuaternionRotation3D(localRotation));


            body1 = GetJointBodyPairSprtBody(GetJointBodyPairSprtHullType(bodyType1), shiftedCenter, finalRotation, color);

            #endregion

            #region Body 2

            GetJointBodyPairSprtOffset(out distanceToPermiter, out localRotation, bodyType2);
            offset = new Vector3D(distanceToPermiter + (separationDistance / 2d), 0, 0);
            offset = rotation.GetRotatedVector(offset);
            shiftedCenter = centerPoint - offset;		// subtracting instead of adding


            localRotation = new Quaternion(new Vector3D(0, 0, 1), 180d).ToUnit() * rotation.ToUnit() * localRotation.ToUnit();		// throwing in an extra 180 degrees of spin
            finalRotation = new RotateTransform3D(new QuaternionRotation3D(localRotation));


            body2 = GetJointBodyPairSprtBody(GetJointBodyPairSprtHullType(bodyType2), shiftedCenter, finalRotation, color);

            #endregion
        }