C# Matrix3D.Rotate方法代码示例

        public static void GetRotatedVector(this Quaternion quaternion, Point3D[] points)
        {
            Matrix3D matrix = new Matrix3D();
            matrix.Rotate(quaternion);

            MatrixTransform3D transform = new MatrixTransform3D(matrix);

            transform.Transform(points);
        }

        public static Matrix3D RotateXYZ(Matrix3D matrix, double xRotationDegrees, double yRotationDegrees, double zRotationDegrees)
        {
            matrix.Rotate(new Quaternion(new Vector3D(1, 0, 0), xRotationDegrees));

            var localY = matrix.Transform(new Vector3D(0, 1, 0));
            matrix.Rotate(new Quaternion(localY, yRotationDegrees));

            var localZ = matrix.Transform(new Vector3D(0, 0, 1));
            matrix.Rotate(new Quaternion(localZ, zRotationDegrees));
            return matrix;
        }

        public static Matrix3D RotateXYZ(Matrix3D matrix, Vector3D rotationsXYZ)
        {
            matrix.Rotate(new Quaternion(new Vector3D(0, 0, 1), rotationsXYZ.Z));

            var localY = matrix.Transform(new Vector3D(0, 1, 0));
            matrix.Rotate(new Quaternion(localY, rotationsXYZ.Y));

            var localX = matrix.Transform(new Vector3D(1, 0, 0));
            matrix.Rotate(new Quaternion(localX, rotationsXYZ.X));
            return matrix;
        }

        public Transform3DGroup CreateTransformGroup(Halo3.ObjectChunk placedObject)
        {
            var transformGroup = new Transform3DGroup();
            float yaw, pitch, roll;
            Core.Helpers.VectorMath.Convert.ToYawPitchRoll(
                placedObject.SpawnPosition.Right,
                placedObject.SpawnPosition.Forward,
                placedObject.SpawnPosition.Up,
                out yaw,
                out pitch,
                out roll);

            // For some reason you have to swag the roll and yaw.
            var swag = Microsoft.Xna.Framework.Quaternion.CreateFromYawPitchRoll(roll, pitch, yaw);

            // Apply 3D Matrix
            var matrix = new Matrix3D();
            matrix.Rotate(new Quaternion(swag.X, swag.Y, swag.Z, swag.W));
            matrix.OffsetX = placedObject.SpawnCoordinates.X;
            matrix.OffsetY = placedObject.SpawnCoordinates.Y;
            matrix.OffsetZ = placedObject.SpawnCoordinates.Z;
            // TODO: FUCK THIS VALUE
            // TODO: AND FUCK BUNGIE
            //matrix.Prepend(new Matrix3D
            //					{
            //						OffsetX = 0,
            //						OffsetY = 0,
            //						OffsetZ = 0
            //					});
            transformGroup.Children.Add(new MatrixTransform3D(matrix));

            return transformGroup;
        }

        /// <summary>
        /// Rotates the double vector around the angle in degrees
        /// </summary>
        public DoubleVector GetRotatedVector(Vector3D axis, double angle)
        {
            Matrix3D matrix = new Matrix3D();
            matrix.Rotate(new Quaternion(axis, angle));

            MatrixTransform3D transform = new MatrixTransform3D(matrix);

            return new DoubleVector(transform.Transform(this.Standard), transform.Transform(this.Orth));
        }

        public void Rotate(Vector3D rotationAmount)
        {
            var rotationAroundX = new Matrix3D();
            rotationAroundX.Rotate(new Quaternion(Right, Trig.DegreeToRadian(rotationAmount.Y)));

            var rotationAroundY = new Matrix3D();
            rotationAroundY.Rotate(new Quaternion(Up, Trig.DegreeToRadian(rotationAmount.X)));

            var rotationMatrix = Matrix3D.Multiply(rotationAroundX, rotationAroundY);
            Direction = Vector3D.Multiply(Direction, rotationMatrix);
            Up = Vector3D.Multiply(Up, rotationMatrix);

            UpdateSettings();
        }

        /// <summary>
        /// Create a transform matrix that translates from skeleton space
        /// into a space whose origin is the center of the screen and has the
        /// same units as skeleton space.
        /// </summary>
        /// <param name="sensorOffset">vector, in meters, from the center of the display to the center of the Kinect sensor</param>
        /// <param name="sensorElevationAngle">elevation angle of the sensor in degrees</param>
        /// <returns>transform matrix</returns>
        public static Matrix3D SensorToScreenPositionTransform(Vector3D sensorOffset, double sensorElevationAngle)
        {
            var rotateIntoSensorSpace = new Matrix3D();
            rotateIntoSensorSpace.Rotate(new Quaternion(new Vector3D(1.0, 0.0, 0.0), sensorElevationAngle));

            var eye = -sensorOffset;
            eye = rotateIntoSensorSpace.Transform(eye);

            var normalFromEye = new Vector3D(0.0, 0.0, 1.0);
            normalFromEye = rotateIntoSensorSpace.Transform(normalFromEye);

            var up = new Vector3D(0.0, 1.0, 0.0);

            return LookAt(eye, normalFromEye, up);
        }

        Matrix3D CalculateRotationMatrix(double x, double y, double z)
        {
            Matrix3D matrix = new Matrix3D();

            matrix.Rotate(new Quaternion(new Vector3D(1, 0, 0), x));
            matrix.Rotate(new Quaternion(new Vector3D(0, 1, 0) * matrix, y));
            matrix.Rotate(new Quaternion(new Vector3D(0, 0, 1) * matrix, z));

            return matrix;
        }

        private void RotateCameraAroundLookDir(double curX, double curY, double dx, double dy)
        {
            const double ROTATESPEED = .005d;

            // Calculate angle
            //double radians = ROTATESPEED * Math.Sqrt(Math.Pow(dx, 2) + Math.Pow(dy, 2));
            double radians = ROTATESPEED * Math.Sqrt((dx * dx) + (dy * dy));
            double degrees = Math1D.RadiansToDegrees(radians);

            if (radians < 0)
            {
                MessageBox.Show("ya");
            }

            // See if I should negate the angle
            //NOTE:  This logic is flawed.  I fixed this later by taking curXY cross prevXY
            if (curX >= 0 && curY >= 0)
            {
                // Q1
                if (dx > 0 || dy < 0)
                {
                    degrees *= -1;
                }
            }
            else if (curX <= 0 && curY >= 0)
            {
                // Q2
                if (dx > 0 || dy > 0)
                {
                    degrees *= -1;
                }
            }
            else if (curX <= 0 && curY <= 0)
            {
                // Q3
                if (dx < 0 || dy > 0)
                {
                    degrees *= -1;
                }
            }
            else if (curX >= 0 && curY <= 0)
            {
                // Q4
                if (dx < 0 || dy < 0)
                {
                    degrees *= -1;
                }
            }

            // Create a matrix that will perform the rotation
            Matrix3D matrix = new Matrix3D();
            matrix.Rotate(new Quaternion(_camera1.LookDirection, degrees));

            // Rotate the camera
            _camera1.UpDirection = matrix.Transform(_camera1.UpDirection);
        }

        /// <summary>
        /// Rotate around three axes.
        /// </summary>
        /// <param name="p1">
        /// The previous mouse position.
        /// </param>
        /// <param name="p2">
        /// The current mouse position.
        /// </param>
        /// <param name="rotateAround">
        /// The point to rotate around.
        /// </param>
        public void RotateTurnball(Point p1, Point p2, Point3D rotateAround)
        {
            this.InitTurnballRotationAxes(p1);

            Vector delta = p2 - p1;

            Vector3D relativeTarget = rotateAround - this.CameraTarget;
            Vector3D relativePosition = rotateAround - this.CameraPosition;

            double d = -1;
            if (this.CameraMode != CameraMode.Inspect)
            {
                d = 0.2;
            }

            d *= this.RotationSensitivity;

            var q1 = new Quaternion(this.rotationAxisX, d * delta.X);
            var q2 = new Quaternion(this.rotationAxisY, d * delta.Y);
            Quaternion q = q1 * q2;

            var m = new Matrix3D();
            m.Rotate(q);

            Vector3D newLookDir = m.Transform(this.CameraLookDirection);
            Vector3D newUpDirection = m.Transform(this.CameraUpDirection);

            Vector3D newRelativeTarget = m.Transform(relativeTarget);
            Vector3D newRelativePosition = m.Transform(relativePosition);

            Vector3D newRightVector = Vector3D.CrossProduct(newLookDir, newUpDirection);
            newRightVector.Normalize();
            Vector3D modUpDir = Vector3D.CrossProduct(newRightVector, newLookDir);
            modUpDir.Normalize();
            if ((newUpDirection - modUpDir).Length > 1e-8)
            {
                newUpDirection = modUpDir;
            }

            Point3D newTarget = rotateAround - newRelativeTarget;
            Point3D newPosition = rotateAround - newRelativePosition;
            Vector3D newLookDirection = newTarget - newPosition;

            this.CameraLookDirection = newLookDirection;
            if (CameraMode == CameraMode.Inspect)
            {
                this.CameraPosition = newPosition;
            }
            this.CameraUpDirection = newUpDirection;
        }

        void WorkShop_MouseMove(object sender, MouseEventArgs e)
        {
            if (ActionType == ActionType.Rotate)
            {
                var currentMousePoint = e.GetPosition(this);
                var currentMouseVector = ProjectToTrackBall(currentMousePoint);

                Vector3D axis = Vector3D.CrossProduct(PreviousMouseVector, currentMouseVector);
                double angle = Vector3D.AngleBetween(PreviousMouseVector, currentMouseVector);

                if (axis.LengthSquared > 0)
                {
                    var rotation = RotateTransform.Rotation as QuaternionRotation3D;

                    if (rotation != null)
                    {
                        rotation.Quaternion = new Quaternion(axis, angle) * rotation.Quaternion;
                        PreviousMouseVector = currentMouseVector;
                    }
                }
            }
            else if (ActionType == ActionType.Translate)
            {
                var currentMousePoint = e.GetPosition(this);

                double multiplier = 2 * (Math.Tan(Math.PI / 8) / ScaleTransform.ScaleX) * 4;

                double deltaX = ((currentMousePoint.X - PreviousMousePoint.X) / Space.ActualWidth) * multiplier;
                double deltaY = -((currentMousePoint.Y - PreviousMousePoint.Y) / Space.ActualHeight) * multiplier;

                var deltaVector = new Vector3D(deltaX, deltaY, 0);
                var rotation = (QuaternionRotation3D)RotateTransform.Rotation;

                var matrix = new Matrix3D();
                matrix.Rotate(rotation.Quaternion);
                matrix.Invert();
                deltaVector = matrix.Transform(deltaVector);

                TranslateTransform.OffsetX += deltaVector.X;
                TranslateTransform.OffsetY += deltaVector.Y;
                TranslateTransform.OffsetZ += deltaVector.Z;

                PreviousMousePoint = currentMousePoint;
            }
        }

 private Matrix3D SetTransformMatrix(double offsetX, double offsetY, double offsetZ, int rotateAroundX)
 {
     Matrix3D resultMatrix = new Matrix3D();
     resultMatrix.Rotate(new Quaternion(new Vector3D(10, 0, 0), -rotateAroundX));
     resultMatrix.Translate(new Vector3D(offsetX, offsetY, -offsetZ));
     return resultMatrix;
 }

        /// <summary>
        /// rotate model by angle
        /// </summary>
        /// <param name="axis">axis for rotate</param>
        /// <param name="rotateAngle">rotation angle</param>
        public void RotateObjectByAngle(Vector3D axis, double rotateAngle)
        {
            try
            {
                if (_CurrentModel != null)
                {
                    Transform3DGroup group = _CurrentModel.Content.Transform as Transform3DGroup;

                    if (group.Children.Count > 0)
                    {
                        for (int i = 0; i < group.Children.Count; i++)
                        {
                            MatrixTransform3D rtrans = group.Children[i] as MatrixTransform3D;
                            if (rtrans != null)
                            {

                                Matrix3D matrix = rtrans.Matrix;
                                matrix.Rotate(new Quaternion(axis, rotateAngle));

                                //get current radius of X axis
                                double rotaterad = Math.Acos(matrix.M22);

                                if ((rotaterad * (180 / Math.PI)) > _MaxRotateXDegree)
                                {
                                    matrix.Rotate(new Quaternion(axis, -rotateAngle));
                                }

                                MatrixTransform3D mat = new MatrixTransform3D(matrix);

                                rtrans = mat;
                                group.Children[i] = rtrans;
                            }
                        }
                    }
                    else
                    {
                        Matrix3D matrix = new Matrix3D();
                        matrix.Rotate(new Quaternion(axis, rotateAngle));
                        MatrixTransform3D mat = new MatrixTransform3D(matrix);
                        group.Children.Add(mat);

                    }
                }
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Gets a transform matrix representing the transform of the robot model
        /// </summary>
        /// <returns></returns>
        public Matrix3D GetTransformMatrix()
        {
            Matrix3D mat = new Matrix3D();
            Vector3D offset = new Vector3D(-OriginX, -OriginY, -OriginZ);
            mat.Translate(offset);
            Vector3D xAxis = new Vector3D(1, 0, 0);
            Quaternion roll = new Quaternion(xAxis, OriginR * 180 / Math.PI);
            Vector3D yAxis = new Vector3D(0, 1, 0);
            Quaternion pitch = new Quaternion(yAxis, OriginP * 180 / Math.PI);
            Vector3D zAxis = new Vector3D(0, 0, 1);
            Quaternion yaw = new Quaternion(zAxis, OriginW * 180 / Math.PI);
            mat.Rotate(roll);
            mat.Rotate(pitch);
            mat.Rotate(yaw);

            RobotInfo.WriteToLogFile("Transform Matrix Created");
            return mat;
        }

        private void CaptureMouseMove(object sender, MouseEventArgs e)
        {
            Point currentMousePoint = e.GetPosition(this);

            if (this.pdbViewer.ActionType == PdbActionType.Rotate)
            {
                Vector3D currentMouseVector3D = this.ProjectToTrackball(currentMousePoint);

                Vector3D axis = Vector3D.CrossProduct(
                    this.previousMouseVector, currentMouseVector3D);
                double angle = 2 * Vector3D.AngleBetween(
                    this.previousMouseVector, currentMouseVector3D);

                if (axis.LengthSquared > 0)
                {
                    QuaternionRotation3D rotation =
                        this.rotateTransform.Rotation as QuaternionRotation3D;

                    if (rotation != null)
                    {
                        rotation.Quaternion = new Quaternion(axis, angle) *
                            rotation.Quaternion;
                    }
                }

                this.previousMouseVector = currentMouseVector3D;
            }
            else if (this.pdbViewer.ActionType == PdbActionType.Select ||
                this.pdbViewer.ActionType == PdbActionType.Deselect &&
                currentMousePoint != this.previousMousePoint)
            {
                double left = Math.Min(this.previousMousePoint.X, currentMousePoint.X);
                double top = Math.Min(this.previousMousePoint.Y, currentMousePoint.Y);
                double right = this.ActualWidth -
                    Math.Max(this.previousMousePoint.X, currentMousePoint.X);
                double bottom = this.ActualHeight -
                    Math.Max(this.previousMousePoint.Y, currentMousePoint.Y);

                left = Math.Max(0, left);
                top = Math.Max(this.CaptureElement.TranslatePoint(new Point(), this).Y, top);
                right = Math.Max(0, right);
                bottom = Math.Max(0, bottom);

                this.selectionRectangle.Margin = new Thickness(left, top, right, bottom);
                this.selectionRectangle.Visibility = Visibility.Visible;

                Rect selectionRect = new Rect(this.previousMousePoint, currentMousePoint);

                foreach (Atom atom in this.Molecule.Atoms)
                {
                    Point? atomPoint = this.GetViewportCoordinatePoint3Ds(atom);

                    bool contained = atomPoint != null && selectionRect.Contains(atomPoint.Value);

                    if (contained)
                    {
                        if (this.pdbViewer.ActionType == PdbActionType.Select)
                            atom.ShowAsSelected = true;
                        else if (this.pdbViewer.ActionType == PdbActionType.Deselect)
                            atom.ShowAsSelected = false;
                    }
                    else
                    {
                        atom.ShowAsSelected = atom.IsSelected;
                    }
                }
            }
            else if (this.pdbViewer.ActionType == PdbActionType.Translate)
            {
                double multiplier = 2 * Math.Tan(Math.PI / 8) * cameraOffset /
                    this.scaleTransform.ScaleX;

                double deltaX = (currentMousePoint.X - this.previousMousePoint.X) /
                    this.viewport.ActualWidth * multiplier;
                double deltaY = -(currentMousePoint.Y - this.previousMousePoint.Y) /
                    this.viewport.ActualHeight * multiplier;

                Vector3D deltaVector = new Vector3D(deltaX, deltaY, 0);

                QuaternionRotation3D rotation =
                    (QuaternionRotation3D)this.rotateTransform.Rotation;

                Matrix3D matrix = new Matrix3D();
                matrix.Rotate(rotation.Quaternion);
                matrix.Invert();

                deltaVector = matrix.Transform(deltaVector);

                this.translateTransform.OffsetX += deltaVector.X;
                this.translateTransform.OffsetY += deltaVector.Y;
                this.translateTransform.OffsetZ += deltaVector.Z;

                this.previousMousePoint = currentMousePoint;
            }
        }