C# Matrix3D.RotateAt方法代码示例

		public static Matrix3D CalcRotationMatrix(double x, double y, double z, Point3D center, Vector3D up, Vector3D look, Transform3D transform, RotationType type)
		{
			//Transform3DGroup trm = new Transform3DGroup();
			//trm.Children.Add(transform);
			Vector3D realup = transform.Transform(up);
			if (type != RotationType.LockAxisY)
			{
				up = realup;
			}
			if (type != RotationType.LockAxisZ)
			{
				look = transform.Transform(look);
			}
			center = transform.Transform(center);
			Vector3D axisX = Vector3D.CrossProduct(up, look);
			Matrix3D matrix = new Matrix3D();
			//Quaternion q = new Quaternion();
			//q.
			double ang = AngleBetween(realup, YAxis) + x;
			if (ang >= 90)
			{
				x = 90 - ang;
			}
			matrix.RotateAt(new Quaternion(axisX, x), center);
			matrix.RotateAt(new Quaternion(up, y), center);
			matrix.RotateAt(new Quaternion(look, z), center);
			return matrix;
		}

		public static Matrix3D CalculateRotationMatrix(double x, double y, double z, Point3D center, Vector3D up, Vector3D look, Transform3D transform)
		{
			//Transform3DGroup trm = new Transform3DGroup();
			//trm.Children.Add(transform);
			up = transform.Transform(up);
			look = transform.Transform(look);
			center = transform.Transform(center);
			Vector3D axisZ = Vector3D.CrossProduct(up, look);
			Matrix3D matrix = new Matrix3D();
			matrix.RotateAt(new Quaternion(axisZ, x), center);
			matrix.RotateAt(new Quaternion(up, y), center);
			matrix.RotateAt(new Quaternion(look, z), center);
			return matrix;
		}

		public static MatrixTransform3D Rotate3D(Transform3D transform, Vector3D look, Vector3D dir, Point3D center)
		{
			Matrix3D m = new Matrix3D();
			Vector3D realook = transform.Transform(look);
			Vector3D axis = Vector3D.CrossProduct(realook, dir);
			double angle = Math.Acos(Vector3D.DotProduct(realook, dir));
			Quaternion q = new Quaternion(axis, angle);
			m.RotateAt(q, center);
			MatrixTransform3D rlt = transform as MatrixTransform3D;
			return new MatrixTransform3D(Matrix3D.Multiply(rlt.Matrix, m));
		}

        public void TestOriginFinder(int sampleSize)
        {
            double error = 0;
            double totalDur = 0;
            Random randomMaker = new Random(DateTime.Now.Millisecond);
            double maxErr = 0;

            for (int j = 0; j < sampleSize; j++)
            {
                Point3D[] standardPoints = new Point3D[]
                {
                    new Point3D(0, 0.5, 1.5), //top
                    new Point3D(0.5, 0.5, 1.5), //right
                    new Point3D(0, 0.5, 2), //front
                    new Point3D(-0.5, 0.25, 1.5) //back
                };

                Point3D[] capturePoints = new Point3D[standardPoints.Length];

                Matrix3D matrix = new Matrix3D();

                //matrix.Translate(new Vector3D(0, 10, 0));

                double size = 1;
                matrix.RotateAt(new Quaternion(new Vector3D((randomMaker.Next(1) - 1) * randomMaker.NextDouble() * size,
                                                                                                        (randomMaker.Next(1) - 1) * randomMaker.NextDouble() * size,
                                                                                                        randomMaker.NextDouble() * size),
                                                                                                        (randomMaker.Next(1) - 1) * randomMaker.NextDouble() * 360),
                                                                             new Point3D(0, 0, 0));

                matrix.Translate(new Vector3D((randomMaker.Next(1) - 1) * randomMaker.NextDouble() * size,
                                                                            (randomMaker.Next(1) - 1) * randomMaker.NextDouble() * size,
                                                                            (randomMaker.Next(1) - 1) * randomMaker.NextDouble() * size));

                for (int i = 0; i < standardPoints.Length; i++)
                {
                    capturePoints[i] = matrix.Transform(standardPoints[i]);
                }

                testOrigin = matrix.Transform(new Point3D());
                DateTime start = DateTime.Now.ToUniversalTime();
                Point3D newOrigin = this.BruteForceEstimateOrigin(standardPoints, capturePoints, 5);
                TimeSpan end = DateTime.Now.ToUniversalTime() - start;
                int duration = (int)end.TotalSeconds;
                totalDur += duration;
                Vector3D og = new Vector3D(testOrigin.X, testOrigin.Y, testOrigin.Z);
                Vector3D offset = new Vector3D(testOrigin.X - newOrigin.X, testOrigin.Y - newOrigin.Y, testOrigin.Z - newOrigin.Z);
                error += offset.Length;
                if (maxErr < offset.Length)
                {
                    maxErr = offset.Length;
                    Console.WriteLine("Max Error: {1}", j, Math.Round(maxErr, 6));
                }
            }
            double avgErr = error / sampleSize;
            double avgDur = totalDur / sampleSize;
            Console.WriteLine("Average Error: {0}\t Max Error: {1}", avgErr, maxErr);
        }

        /// <summary>
        /// Rotates the point cloud by a given angle
        /// </summary>
        /// <param name="axis">The axis of rotation</param>
        /// <param name="angle">The angle to which te point cloud is to be rotated</param>
        public void rotate(double[] axis, double angle)
        {
            if (!(axis.Length != 3)) {
                //centre of rotation
                Point3D centre = new Point3D(axis[0], axis[1], axis[2]);

                //pull out the entire tree
                PARSE.ICP.PointRGB[] pts = this.getAllPoints();

                //create a new kd tree
                KdTree.KDTree newPoints = new KdTree.KDTree(3);

                //iterate over every point and translate + jam in new tree
                foreach (PARSE.ICP.PointRGB point in pts)
                {
                    //create rot matrix
                    Matrix3D mtx = new Matrix3D();
                    Quaternion q = new Quaternion(new Vector3D(0, 1, 0), angle);
                    mtx.RotateAt(q, centre);

                    //complete rotation
                    Point3D newPoint = mtx.Transform(point.point);

                    //check if the x, y and z max and min coords need updating
                    //check min values
                    if (newPoint.X < minx) { minx = newPoint.X; }
                    if (newPoint.Y < miny) { miny = newPoint.Y; }
                    if (newPoint.Z < minz) { minz = newPoint.Z; }

                    //check max values
                    if (newPoint.X > maxx) { maxx = newPoint.X; }
                    if (newPoint.Y > maxy) { maxy = newPoint.Y; }
                    if (newPoint.Z > maxz) { maxz = newPoint.Z; }

                    //jam into the tree hole
                    double[] key = new double[3] { newPoint.X, newPoint.Y, newPoint.Z };
                    newPoints.insert(key, new PARSE.ICP.PointRGB(newPoint, point.r, point.g, point.b));
                }

                //replace the old kd tree with the new one
                this.points = newPoints;
            }
            else{
                //throw an exception and annoy Bernie in the process ;)
            }
        }

        private Vector3D CalculateHeadOrientation(WagSkeleton skeleton)
        {
            Vector3D headOrientation = new Vector3D(0, 0, -1);

              FaceTrackFrame face = skeleton.FaceFrame;
              var FacePoints = face.Get3DShape();

              Vector3DF eyeLeft = FacePoints[Constants.LEFT_EYE];
              Vector3DF eyeRight = FacePoints[Constants.RIGHT_EYE];
              Vector3DF faceTop = FacePoints[Constants.FACE_TOP];
              Vector3DF faceBottom = FacePoints[Constants.FACE_BOTTOM];

              Vector3D faceVectorHorizontal = new Vector3D(eyeLeft.X - eyeRight.X, eyeLeft.Y - eyeRight.Y, eyeLeft.Z - eyeRight.Z);
              Vector3D faceVectorVertical = new Vector3D(faceTop.X - faceBottom.X, faceTop.Y - faceBottom.Y, faceTop.Z - faceBottom.Z);

              headOrientation = Vector3D.CrossProduct(faceVectorHorizontal, faceVectorVertical);
              headOrientation = Calibration.CalibrateTransform.Transform(headOrientation);
              headOrientation.Normalize();
              Matrix3D headPointsPointUpMatrix = new Matrix3D();
              headPointsPointUpMatrix.RotateAt(new System.Windows.Media.Media3D.Quaternion(new Vector3D(int.MaxValue, 0, 0), -20), skeleton.TransformedJoints[JointType.Head].Position.ToPoint3D());
              Vector3D lowered = headPointsPointUpMatrix.Transform(headOrientation);

              return lowered;
        }

        private Vector3D CalculateHeadOrientation(WagSkeleton skeleton)
        {
            Vector3D headOrientation = new Vector3D(0, 0, -1);

            FaceTrackFrame face = skeleton.FaceFrame;
            var FacePoints = face.Get3DShape();

            Vector3DF eyeLeft = FacePoints[LEFT_EYE];
            Vector3DF eyeRight = FacePoints[RIGHT_EYE];
            Vector3DF faceTop = FacePoints[FACE_TOP];
            Vector3DF faceBottom = FacePoints[FACE_BOTTOM];

            Vector3D faceVectorHorizontal = new Vector3D(eyeLeft.X - eyeRight.X, eyeLeft.Y - eyeRight.Y, eyeLeft.Z - eyeRight.Z);
            Vector3D faceVectorVertical = new Vector3D(faceTop.X - faceBottom.X, faceTop.Y - faceBottom.Y, faceTop.Z - faceBottom.Z);

            headOrientation = Vector3D.CrossProduct(faceVectorHorizontal, faceVectorVertical);
            headOrientation = originTransform.Transform(headOrientation);
            headOrientation.Normalize();

            Matrix3D headPointsPointUpMatrix = new Matrix3D();
            headPointsPointUpMatrix.RotateAt(new Quaternion(new Vector3D(1, 0, 0), -20), skeleton.TransformedJoints[JointType.Head].Position.ToPoint3D());
            Vector3D lowered = headPointsPointUpMatrix.Transform(headOrientation);

            if (headOrientation.Z > 0)
                throw new Exception("Right hand rule violation");

            return lowered;
        }

        private void MainForm_Click(object sender, EventArgs e)
        {
            Console.Clear();

            #region Create source data.

            for (int i = 0; i < pointsCountForFill; ++i) {
                this.sourcePoints[i] = new Point3D(
                    this.rand.Next(Math.Max(this.ClientSize.Width, this.ClientSize.Height)),
                    this.rand.Next(Math.Max(this.ClientSize.Width, this.ClientSize.Height)),
                    this.rand.Next(Math.Max(this.ClientSize.Width, this.ClientSize.Height)));
            }
            for (int i = pointsCountForFill; i < pointsCount; ++i) {
                this.sourcePoints[i] = new Point3D(
                    this.rand.Next(Math.Max(this.ClientSize.Width, this.ClientSize.Height)),
                    this.rand.Next(Math.Max(this.ClientSize.Width, this.ClientSize.Height)),
                    this.rand.Next(Math.Max(this.ClientSize.Width, this.ClientSize.Height)));
            }

            #endregion Create source data.

            #region Create target data.

            Matrix3D matrix3D = new Matrix3D();
            matrix3D.Translate(new Vector3D(10, 50, 20));
            matrix3D.RotateAt(new Quaternion(new Vector3D(2, 9, 1), 20), new Point3D(30, 30, 70));
            matrix3D.Translate(new Vector3D(10, 50, 20));
            matrix3D.RotateAt(new Quaternion(new Vector3D(1, 3, 5), -5), new Point3D(10, 0, 10));

            Console.WriteLine("{0,30}{1,30}{2,30}{3,30}", matrix3D.M11, matrix3D.M21, matrix3D.M31, matrix3D.OffsetX);
            Console.WriteLine("{0,30}{1,30}{2,30}{3,30}", matrix3D.M12, matrix3D.M22, matrix3D.M32, matrix3D.OffsetY);
            Console.WriteLine("{0,30}{1,30}{2,30}{3,30}", matrix3D.M13, matrix3D.M23, matrix3D.M33, matrix3D.OffsetZ);

            for (int i = 0; i < pointsCount; ++i) {
                this.targetPoints[i] = matrix3D.Transform(this.sourcePoints[i]);
            }

            Point3D[] sourcePointsToFill = new Point3D[pointsCountForFill];
            Point3D[] targetPointsToFill = new Point3D[pointsCountForFill];
            for (int i = 0; i < pointsCountForFill; ++i) {
                sourcePointsToFill[i] = this.sourcePoints[i];
                targetPointsToFill[i] = this.targetPoints[i];
            }
            AffineCoorTransformator ct = new AffineCoorTransformator();
            ct.Fill(sourcePointsToFill, targetPointsToFill);

            double[] matrix = ct.MatrixCopy;
            int index = 0;
            for (int i = 0; i < AffineCoorTransformator.dim; ++i) {
                for (int j = 0; j < AffineCoorTransformator.dimExt; ++j) {
                    Console.Write("{0,30}", matrix[index]);
                    ++index;
                }
                Console.WriteLine();
            }
            Console.WriteLine();

            #endregion Create target data.

            double[] error = new double[pointsCount];
            double totalError = 0;
            for (int i = 0; i < pointsCount; ++i) {
                this.targetPointsRes[i] = ct.Transform(this.sourcePoints[i]);

                error[i] = (this.targetPointsRes[i] - this.targetPoints[i]).Length;
                Console.WriteLine("Error {0}: {1}.", i, error[i]);
                totalError += error[i];
            }
            double totalErrorMax = pointsCount * eps * Math.Sqrt(AffineCoorTransformator.dim);
            Console.WriteLine();
            Console.WriteLine("Max: {0}. Current: {1}. Middle: {2}", totalErrorMax, totalError, totalError / pointsCount);
            Console.WriteLine();

            this.Invalidate();
        }

        //taken from http://stackoverflow.com/questions/2042214/wpf-3d-rotate-a-model-around-its-own-axes
        //adjusted for offset by Dawid Rusin
        private Matrix3D CalculateRotationMatrix(double x, double y, double z)
        {
            Matrix3D matrix = new Matrix3D();
            matrix.RotateAt(new Quaternion(new Vector3D(1, 0, 0), x), new Point3D(offsetX, offsetY, offsetZ));
            matrix.RotateAt(new Quaternion(new Vector3D(0, 1, 0) * matrix, y), new Point3D(offsetX, offsetY, offsetZ));
            matrix.RotateAt(new Quaternion(new Vector3D(0, 0, 1) * matrix, z), new Point3D(offsetX, offsetY, offsetZ));

            return matrix;
        }