C# Quaternion.GetRotatedVector方法代码示例

        private void RotateCameraAroundLookDir(Point currentPosition, Vector3D currentPosition3D)
        {
            Vector3D curPos3DFlat = currentPosition3D;
            curPos3DFlat.Z = 0;
            curPos3DFlat.Normalize();

            Vector3D prevPos3DFlat = _previousPosition3D;
            prevPos3DFlat.Z = 0;
            prevPos3DFlat.Normalize();

            Vector3D axis = Vector3D.CrossProduct(prevPos3DFlat, curPos3DFlat);
            double angle = Vector3D.AngleBetween(prevPos3DFlat, curPos3DFlat);

            // 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);

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

            deltaRotation.GetRotatedVector(vectors);

            // Apply the changes
            _camera.UpDirection = vectors[0];
            _camera.LookDirection = vectors[1];
        }

        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);

            #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 * _orbitRadius.Value;

            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];
        }

        private void RandomCamera_Click(object sender, RoutedEventArgs e)
        {
            try
            {
                // Position
                Vector3D position = Math3D.GetRandomVector_Spherical(CAMERADISTANCE / 2, CAMERADISTANCE * 2);

                // Look Direction
                Vector3D lookDirection = position * -1;

                Vector3D rotateAxis = Math3D.GetRandomVector_Cone(Math3D.GetArbitraryOrhonganal(lookDirection), 20);
                Quaternion rotate = new Quaternion(rotateAxis, Math1D.GetNearZeroValue(20));

                lookDirection = rotate.GetRotatedVector(lookDirection);

                // Up Vector
                Vector3D up = Math3D.GetArbitraryOrhonganal(lookDirection);

                // Commit
                _camera.Position = position.ToPoint();
                _camera.LookDirection = lookDirection;
                _camera.UpDirection = up;
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.ToString(), this.Title, MessageBoxButton.OK, MessageBoxImage.Error);
            }
        }

        private void Update_Direction_Constant(double elapsedTime)
        {
            Quaternion delta = new Quaternion(_angVel.Axis, _angVel.Angle * this.Speed_Orientation * elapsedTime);

            this.Direction = delta.GetRotatedVector(this.Direction);
        }

        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
        }

        private void addJoined1_AddJoint(object sender, AddJoinedBodiesArgs e)
        {
            try
            {
                // Figure out the centerpoint
                Point3D centerPoint = Math3D.GetRandomVector_Spherical(CREATEOBJECTBOUNDRY).ToPoint();

                // Get a random rotation
                //TODO:  Figure out why it fails when I give it a rotation
                //Quaternion finalRotation = new Quaternion(Math3D.GetRandomVectorSpherical(_rand, 10), Math3D.GetNearZeroValue(_rand, 360d));
                Quaternion finalRotation = new Quaternion(new Vector3D(0, 0, 1), 0);

                // All bodies will be the same color
                Color color = UtilityWPF.GetRandomColor(64, 192);

                Body[] bodies = null;

                #region Get Bodies

                switch (e.JointType)
                {
                    case AddJointType.BallAndSocket:
                    case AddJointType.Hinge:
                    case AddJointType.Slider:
                    case AddJointType.Corkscrew:
                    case AddJointType.UniversalJoint:
                        bodies = new Body[2];
                        GetJointBodyPair(out bodies[0], out bodies[1], e.Body1Type, e.Body2Type, centerPoint, finalRotation, e.SeparationDistance, color);
                        break;

                    case AddJointType.UpVector:
                    case AddJointType.Multi_BallAndChain:
                    case AddJointType.Multi_Tetrahedron:
                        MessageBox.Show("finish this");
                        return;

                    default:
                        throw new ApplicationException("Unknown AddJointType: " + e.JointType.ToString());
                }

                #endregion

                _bodySets.Add(bodies);

                #region Setup Joint

                Vector3D directionAlong = finalRotation.GetRotatedVector(new Vector3D(1, 0, 0));
                Vector3D directionOrth1 = finalRotation.GetRotatedVector(new Vector3D(0, 1, 0));
                Vector3D directionOrth2 = finalRotation.GetRotatedVector(new Vector3D(0, 0, 1));

                switch (e.JointType)
                {
                    case AddJointType.BallAndSocket:
                        #region BallAndSocket

                        JointBallAndSocket ballAndSocket = JointBallAndSocket.CreateBallAndSocket(_world, centerPoint, bodies[0], bodies[1]);
                        ballAndSocket.ShouldLinkedBodiesCollideEachOther = true;

                        //TODO:  Let the user define these limits
                        //ballAndSocket.SetConeLimits();

                        #endregion
                        break;

                    case AddJointType.Hinge:
                        #region Hinge

                        JointHinge hinge = JointHinge.CreateHinge(_world, centerPoint, directionOrth1, bodies[0], bodies[1]);
                        hinge.ShouldLinkedBodiesCollideEachOther = true;

                        #endregion
                        break;

                    case AddJointType.Slider:
                        #region Slider

                        JointSlider slider = JointSlider.CreateSlider(_world, centerPoint, directionAlong, bodies[0], bodies[1]);
                        slider.ShouldLinkedBodiesCollideEachOther = true;

                        #endregion
                        break;

                    case AddJointType.Corkscrew:
                        #region Corkscrew

                        JointCorkscrew corkscrew = JointCorkscrew.CreateCorkscrew(_world, centerPoint, directionAlong, bodies[0], bodies[1]);
                        corkscrew.ShouldLinkedBodiesCollideEachOther = true;

                        #endregion
                        break;

                    case AddJointType.UniversalJoint:
                        #region UniversalJoint

                        JointUniversal uJoint = JointUniversal.CreateUniversal(_world, centerPoint, directionOrth1, directionOrth2, bodies[0], bodies[1]);
                        uJoint.ShouldLinkedBodiesCollideEachOther = true;

                        #endregion
                        break;

//.........这里部分代码省略.........

        private static Quaternion GetDelta(Quaternion from, Quaternion to)
        {
            DoubleVector fromVect = from.GetRotatedVector(new DoubleVector(1, 0, 0, 0, 1, 0));
            DoubleVector toVect = to.GetRotatedVector(new DoubleVector(1, 0, 0, 0, 1, 0));

            return Math3D.GetRotation(fromVect, toVect);
        }