C# Matrix3D.Transform方法代码示例

        /// <summary>
        /// Projects the stored 3D points in to 2D.
        /// </summary>
        /// <param name="objectToViewportTransform">The transformation matrix to use</param>
        public void Project(Matrix3D objectToViewportTransform)
        {
            Point3D projPoint1 = objectToViewportTransform.Transform(_p1);
            Point3D projPoint2 = objectToViewportTransform.Transform(_p2);

            _p1Transformed = new Point(projPoint1.X, projPoint1.Y);
            _p2Transformed = new Point(projPoint2.X, projPoint2.Y);
        }

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

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

        //// http://en.wikipedia.org/wiki/Polygon_triangulation
        //// http://en.wikipedia.org/wiki/Monotone_polygon
        //// http://www.codeproject.com/KB/recipes/hgrd.aspx LGPL
        //// http://www.springerlink.com/content/g805787811vr1v9v/
        
        /// <summary>
        /// Flattens this polygon.
        /// </summary>
        /// <returns>
        /// The 2D polygon.
        /// </returns>
        public Polygon Flatten()
        {
            // http://forums.xna.com/forums/p/16529/86802.aspx
            // http://stackoverflow.com/questions/1023948/rotate-normal-vector-onto-axis-plane
            var up = this.GetNormal();
            up.Normalize();
            var right = Vector3D.CrossProduct(
                up, Math.Abs(up.X) > Math.Abs(up.Z) ? new Vector3D(0, 0, 1) : new Vector3D(1, 0, 0));
            var backward = Vector3D.CrossProduct(right, up);
            var m = new Matrix3D(
                backward.X, right.X, up.X, 0, backward.Y, right.Y, up.Y, 0, backward.Z, right.Z, up.Z, 0, 0, 0, 0, 1);

            // make first point origin
            var offs = m.Transform(this.Points[0]);
            m.OffsetX = -offs.X;
            m.OffsetY = -offs.Y;

            var polygon = new Polygon { Points = new PointCollection(this.Points.Count) };
            foreach (var p in this.Points)
            {
                var pp = m.Transform(p);
                polygon.Points.Add(new Point(pp.X, pp.Y));
            }

            return polygon;
        }

        public static Point3D[] Transform(Matrix3D matrix, Point3D[] points)
        {
            var transformed_points = new List<Point3D>();
            foreach (var point in points)
            {
                transformed_points.Add(matrix.Transform(point));
            }

            return transformed_points.ToArray();
        }

        public static IList<Point3D> GetGeometryPoints(MeshGeometry3D mesh, Matrix3D transformMatrix)
        {
            if (mesh != null)
            {
                bool applyMatrix = (transformMatrix != Matrix3D.Identity);

                List<Point3D> points;
                if ((mesh.TriangleIndices != null) && (mesh.TriangleIndices.Count > 0))
                {
                    points = new List<Point3D>(mesh.TriangleIndices.Count);
                    for (int i = 0, count = mesh.TriangleIndices.Count; i < count; i++)
                    {
                        int positionIndex = mesh.TriangleIndices[i];
                        if ((positionIndex < 0) || (positionIndex >= mesh.Positions.Count))
                            break;

                        if (applyMatrix)
                            points.Add(transformMatrix.Transform(mesh.Positions[positionIndex]));
                        else
                            points.Add(mesh.Positions[positionIndex]);
                    }
                }
                else
                {
                    points = new List<Point3D>(mesh.Positions.Count);
                    if (applyMatrix)
                    {
                        foreach (Point3D p in mesh.Positions)
                            points.Add(transformMatrix.Transform(p));
                    }
                    else
                        points.AddRange(mesh.Positions);
                }

                if (points.Count > 0)
                    return points;
                else
                    return null;
            }
            else
                return null;
        }

    private static void PopulateProfile(Bin[, ,] dProfile, Matrix3D navigationMatrix)
    {
      DataTable dataFile = new DataTable();
      try
      {
        // add the csv bin file
        using (GenericParserAdapter parser = new GenericParserAdapter(PATH_TO_BIN_PROFILE))
        {
          System.Data.DataSet dsResult = parser.GetDataSet();
          dataFile = dsResult.Tables[0];
        }
      }
      catch
      { }

      for (int i = 1; i < dataFile.Rows.Count; i++)
      {
        //lab vale as got form profile index
        Point3D labBin = new Point3D();
        labBin.X = Convert.ToDouble(dataFile.Rows[i][0].ToString());
        labBin.Y = Convert.ToDouble(dataFile.Rows[i][1].ToString());
        labBin.Z = Convert.ToDouble(dataFile.Rows[i][2].ToString());

        //trasfered points
        Point3D labCoordinate = navigationMatrix.Transform(labBin);

        //gets the bin to fill up
        Bin actualBin = GetProfileBin(dProfile, labCoordinate);

        //bin RGB Value
        actualBin.binRGB.X = Convert.ToByte(dataFile.Rows[i][9].ToString());
        actualBin.binRGB.Y = Convert.ToByte(dataFile.Rows[i][10].ToString());
        actualBin.binRGB.Z = Convert.ToByte(dataFile.Rows[i][11].ToString());

        //Measure Lab Values
        actualBin.measuredLAB.X = Convert.ToDouble(dataFile.Rows[i][3].ToString());
        actualBin.measuredLAB.Y = Convert.ToDouble(dataFile.Rows[i][4].ToString());
        actualBin.measuredLAB.Z = Convert.ToDouble(dataFile.Rows[i][5].ToString());

        //measured XYZ Values
        actualBin.measuredXYZ.X = Convert.ToDouble(dataFile.Rows[i][6].ToString());
        actualBin.measuredXYZ.Y = Convert.ToDouble(dataFile.Rows[i][7].ToString());
        actualBin.measuredXYZ.Z = Convert.ToDouble(dataFile.Rows[i][8].ToString());

        //is empty check
        actualBin.isEmpty = false;
      }
    }

        /// <summary>
        /// Translate the point cloud by a given value
        /// </summary>
        /// <param name="tx">Up to three co-ords</param>
        public void translate(double[] tx)
        {
            if (tx.Length == 3) {
                //turn the transformation vector into and object
                Console.WriteLine("Translating");
                TranslateTransform3D translation = new TranslateTransform3D(tx[0], tx[1], tx[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) {

                    //perform the new translation which does appear to work.
                    Matrix3D mtx = new Matrix3D();
                    mtx.Translate(new Vector3D(tx[0], tx[1], tx[2]));

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

                    //perform the old translation method which doesn't appear to work.
                    //point.point.Offset(tx[0], tx[1], tx[2]);
                    //double[] key = new double[3]{point.point.X, point.point.Y, point.point.Z};
                    //newPoints.insert(key, point);
                }

                //replace the old kd tree with the new one
                this.points = newPoints;
            }
            else {
                //probably want to throw an exception here
            }
        }

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

        /// <summary>
        /// Transforms the given mesh
        /// </summary>
        /// <param name="mesh">This mesh</param>
        /// <param name="transform">Transform</param>
        public static void Transform(this MeshGeometry3D mesh, Matrix3D transform)
        {
            Matrix3D rotationOnly = transform;
            rotationOnly.OffsetX = rotationOnly.OffsetY = rotationOnly.OffsetZ = 0;

            // Transform positions
            for(int i = 0; i < mesh.Positions.Count; i++)
                mesh.Positions[i] = transform.Transform(mesh.Positions[i]);
            // Transform normals
            for (int i = 0; i < mesh.Normals.Count; i++)
                mesh.Normals[i] = rotationOnly.Transform(mesh.Normals[i]);
        }

 /// <summary>
 /// Vector3D * Matrix3D multiplication
 /// </summary>
 /// <param name="vector">Vector being tranformed.</param>
 /// <param name="matrix">Transformation matrix applied to the vector.</param>
 /// <returns>Result of multiplication.</returns>
 public static Vector3D Multiply(Vector3D vector, Matrix3D matrix)
 {
     return matrix.Transform(vector);
 }

        /// <summary>
        /// Determines whether the back of the plane is showing, given a rotation.
        /// Credit to Joel Pryde.
        /// </summary>
        /// <param name="x">The x rotation.</param>
        /// <param name="y">The y rotation.</param>
        /// <param name="z">The z rotation.</param>
        /// <returns>
        /// <c>true</c> if the back of the plane is showing, given a rotation; otherwise, <c>false</c>.
        /// </returns>
        private static bool IsBackShowingRotation(double x, double y, double z)
        {
            Matrix3D rotMatrix = new Matrix3D();
            rotMatrix.Rotate(new Quaternion(new Vector3D(1, 0, 0), x));
            rotMatrix.Rotate(new Quaternion(new Vector3D(0, 1, 0) * rotMatrix, y));
            rotMatrix.Rotate(new Quaternion(new Vector3D(0, 0, 1) * rotMatrix, z));

            Vector3D transformZ = rotMatrix.Transform(new Vector3D(0, 0, 1));
            return Vector3D.DotProduct(new Vector3D(0, 0, 1), transformZ) < 0;
        }

        public void RotateTwoAxes(double dx, double dy)
        {
            Vector3D up = ModelUpDirection; // new Vector3D(0, 0, 1);
            Vector3D dir = LookDirection;
            dir.Normalize();

            Vector3D right = Vector3D.CrossProduct(dir, UpDirection);
            right.Normalize();

            double d = -0.5;
            if (CameraMode == CameraMode.WalkAround)
                d = 0.1;
            d *= RotationSensitivity;

            var q1 = new Quaternion(up, d*dx);
            var q2 = new Quaternion(right, d*dy);
            Quaternion q = q1*q2;

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

            Vector3D newLookDir = m.Transform(LookDirection);
            Vector3D newUpDir = m.Transform(UpDirection);

            right = Vector3D.CrossProduct(newLookDir, newUpDir);
            right.Normalize();
            Vector3D modUpDir = Vector3D.CrossProduct(right, newLookDir);
            modUpDir.Normalize();
            if ((newUpDir - modUpDir).Length > 1e-8)
                newUpDir = modUpDir;

            LookDirection = newLookDir;
            UpDirection = newUpDir;
        }

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