C# Matrix3D.Translate方法代码示例

    static void Main(string[] args)
    {
      navigationMatrix = new Matrix3D();
      navigationMatrix.Translate(new Vector3D(0, 100, 110));
      navigationMatrix.Scale(new Vector3D((double)1 / 5, (double)1 / 5, (double)1 / 5));

      displayProfile = new Bin[Bin.RANGEL, Bin.RANGEA, Bin.RANGEB];
      for (int l = 0; l < Bin.RANGEL; l++)
        for (int a = 0; a < Bin.RANGEA; a++)
          for (int b = 0; b < Bin.RANGEB; b++)
            displayProfile[l, a, b] = new Bin(l, a, b);

      PopulateProfile(displayProfile, navigationMatrix);

      String path = Environment.CurrentDirectory + PATH_TO_VIDEO;
      if (!System.IO.File.Exists(path))
        return;


      //Opens the movie file
      capture = new Capture(path);
      double fps = capture.GetCaptureProperty(Emgu.CV.CvEnum.CAP_PROP.CV_CAP_PROP_FPS);

      //Reads frame by frame
      Timer timer = new Timer(1000 / fps);
      timer.Elapsed += new ElapsedEventHandler(timer_Elapsed);
      timer.Start();

      Console.Read();
    }

        /// <summary>
        /// LookAtRH
        /// </summary>
        /// http://msdn.microsoft.com/en-us/library/bb281711(v=vs.85).aspx
        /// <returns></returns>
        public static Matrix3D SetViewMatrix(Point3D cameraPosition, Vector3D lookDirection, Vector3D upDirection)
        {
            // Normalize vectors:
            lookDirection.Normalize();
            upDirection.Normalize();
            double dotProduct = Vector3D.DotProduct(lookDirection, upDirection);
            // Define vectors, XScale, YScale, and ZScale:
            double denom = Math.Sqrt(1 - Math.Pow(dotProduct, 2));
            Vector3D XScale = Vector3D.CrossProduct(lookDirection, upDirection) / denom;
            Vector3D YScale = (upDirection - dotProduct * lookDirection) / denom;
            Vector3D ZScale = lookDirection;

            // Construct M matrix:
            Matrix3D M = new Matrix3D()
            {
                M11 = XScale.X, M12 = YScale.X, M13 = ZScale.X,
                M21 = XScale.Y, M22 = YScale.Y, M23 = ZScale.Y,
                M31 = XScale.Z, M32 = YScale.Z, M33 = ZScale.Z
            };

            // Translate the camera position to the origin:
            Matrix3D translateMatrix = new Matrix3D();
            translateMatrix.Translate(new Vector3D(-cameraPosition.X, -cameraPosition.Y, -cameraPosition.Z));
            // Define reflect matrix about the Z axis:
            Matrix3D reflectMatrix = new Matrix3D();
            reflectMatrix.M33 = -1;

            // Construct the View matrix:
            Matrix3D viewMatrix = translateMatrix * M * reflectMatrix;
            return viewMatrix;
        }

 static void SetupMatrices()
 {
     mx1 = new Matrix3D();
     mx1.Translate(new Vector3D(0, -2.4, 0));
     mx2 = new Matrix3D();
     mx3 = new Matrix3D();
     mx3.Translate(new Vector3D(0, 2.4, 0));
     mx4 = new Matrix3D();
     mx4.Translate(new Vector3D(0, 0, -1));
 }

 public static Matrix3D GetLocalToParent(IDictionary dictionary)
 {
     var matrix3D = new Matrix3D();
     if (dictionary != null)
     {
         var rotations = Helpers.IDictionaryHelper.GetRotationsXYZ(dictionary);
         matrix3D = Helpers.Matrix3DHelper.RotateXYZ(new Matrix3D(), Helpers.IDictionaryHelper.GetRotationsXYZ(dictionary));
         matrix3D.Translate(Helpers.IDictionaryHelper.GetTranslation(dictionary));
     }
     return matrix3D;
 }

        private static Transform3D GetTransform3D(IDictionary source, IFactory factory)
        {
            // transform should NOT include Scale
            ITranslation itranslation = null;
            IRotations irotations = null;
            if (factory != null)
            {
                itranslation = factory.Create<ITranslation>(source,null);
                irotations = factory.Create<IRotations>(source,null);
                var matrix3D = new Matrix3D();
                if (irotations != null) matrix3D = IMatrix3DHelper.RotateXYZ(new Matrix3D(), irotations.RotationsXYZ);
                if (itranslation != null) matrix3D.Translate(itranslation.Translation);

                if (!matrix3D.IsIdentity)
                {
                    return new MatrixTransform3D { Matrix = matrix3D };
                }
            }
            return null;
        }

        public static IMatrix3D FromIDictionary(IDictionary source, IFactory factory_in)
        {
            IFactory factory = factory_in;
            if (factory == null) factory = Node.Net.Factories.Deprecated.Factory.Default;
            var matrix3D = new Matrix3D();
            var irotations = factory.Create<IRotations>(source,null);
            if (irotations == null) irotations = Node.Net.Factories.Deprecated.Factory.Default.Create<IRotations>(source,null);
            var iscale = factory.Create<IScale>(source,null);
            //if (iscale == null) iscale = Node.Net.Factories.Factory.Default.Create<IScale>(source);
            var itranslation = factory.Create<ITranslation>(source,null);
            if (itranslation == null) itranslation = factory.Create<ITranslation>(source,null);
            if (irotations != null)
            {
                matrix3D = RotateXYZ(new Matrix3D(), irotations.RotationsXYZ);
            }
            if (iscale != null) matrix3D.Scale(iscale.Scale);
            if (itranslation != null) matrix3D.Translate(itranslation.Translation);

            if (!matrix3D.IsIdentity)
            {
                return new ConcreteMatrix3D { Matrix3D = matrix3D };
            }
            return new ConcreteMatrix3D();
        }

        private Matrix3D calculateTranslationMatrix(double x, double y, double z)
        {
            Matrix3D matrix = new Matrix3D();
            matrix.Translate(new Vector3D(x, y, z));

            return matrix;
        }

 internal override void Append(ref Matrix3D matrix)
 {
     matrix.Translate(new Vector3D(_cachedOffsetXValue, _cachedOffsetYValue, _cachedOffsetZValue));
 }

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

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

 private void ComposeMatrix()
 {
     if (_decomposeDirty)
     {
         _matrix = Matrix3D.Identity;
         _matrix.Translate(_translation);
         _matrix.Prepend(Math3D.CreateYawPitchRollMatrix(_rotation));
         _decomposeDirty = false;
     }
 }

        public void UpdateOrCreateJoint(Joint joint)
        {
            int segments = 5;
            double jointRadius = 0.07;
            double boneRadius = 0.03;

            int jointId = (int)joint.ID;
            int connectedToJoint = SkeletonMetadata.BoneConnectionMapping[jointId];
            _jointPositions[jointId] = new Vector3D(joint.Position.X, joint.Position.Y, joint.Position.Z);

            // Create new 3d cube for the joint if not yet existing
            GeometryModel3D model;
            if (_joints[jointId] != null)
            {
                model = _joints[jointId];
            }
            else
            {
                model = Model3DFactory.CreateNormalizedSphere(defaultMaterial, segments);
                _environment.Children.Add(model);
                _joints[jointId] = model;

                if (connectedToJoint >= 0)
                {
                    GeometryModel3D cylinder = Model3DFactory.CreateNormalizedCylinder(defaultMaterial, segments);
                    _environment.Children.Add(cylinder);
                    _bones[jointId] = cylinder;
                }
            }

            // Performance improvement: not using a transformation group, but multiply
            // matrices first and use a single MatrixTransform3D
            var matrix = new Matrix3D();
            matrix.Scale(new Vector3D(jointRadius, jointRadius, jointRadius));
            matrix.Translate(new Vector3D(joint.Position.X, joint.Position.Y, joint.Position.Z));

            // Update position and the material/color (based on current tracking state), color right shoulder blue
            if (joint.ID == JointID.ShoulderRight)
                model.Material = rightShoulderIndicatorMaterial;
            else
                model.Material = _trackingStateMaterials[joint.TrackingState];
            model.Transform = new MatrixTransform3D(matrix);

            if (connectedToJoint >= 0)
            {
                GeometryModel3D bone = _bones[jointId];
                Vector3D boneStart = _jointPositions[jointId];
                Vector3D boneEnd = _jointPositions[connectedToJoint];
                Vector3D boneCenter = (boneStart + boneEnd) / 2;
                Vector3D boneVector = boneEnd - boneStart;

                // Again, compute a single transformation matrix and apply it to each bone
                var boneMatrix = new Matrix3D();
                boneMatrix.Scale(new Vector3D(boneRadius, boneRadius, boneVector.Length));
                boneMatrix.Rotate(GetQuaternionFromVectors(new Vector3D(0, 0, 1), boneVector));
                boneMatrix.Translate(boneCenter);

                bone.Material = _trackingStateMaterials[joint.TrackingState];
                bone.Transform = new MatrixTransform3D(boneMatrix); ;
            }
        }

        private void UpdateCameraMatrix()
        {
            // right-handed coordinates

            #if false
            // front view
            var eye = new Vector3D(0.0, 0.0, -CameraDistance);
            var at = new Vector3D(0.0, 0.0, 0.0);
            var up = new Vector3D(0.0, 1.0, 0.0);
            var lookAt = WWMatrixUtil.CalculateLookAt(eye, at, up);
            #else
            // virtual trackball
            var cameraRot = mVirtualTrackball.RotationMatrix();
            var cameraTranslate = new Matrix3D();
            cameraTranslate.Translate(new Vector3D(0.0, 0.0, -mCameraDistanceCurrent));
            var cameraMat = cameraTranslate * cameraRot;
            var lookAt = cameraMat;
            lookAt.Invert();
            #endif

            var viewProjectionMatrix = WWMatrixUtil.CreatePerspectiveProjectionMatrix(mCameraNear, mCameraDistanceCurrent * 2.0, CameraFovHDegree, 1.0);

            mWorldProjectionMatrix = lookAt * viewProjectionMatrix;
        }

        /// <summary>
        /// パーティクルの描画設定をまとめて行います。
        /// </summary>
        public override void RenderParticles(IEnumerable<Particle> particles)
        {
            base.RenderParticles(particles);

            foreach (var particle in particles)
            {
                if (particle.Brush != null)
                {
                    var opacity = (double)((particle.Color >> 24) & 0xFF) / 255.0;

                    particle.Brush.Opacity = EffectObject.InheritedOpacity * opacity;
                }

                if (particle.Material != null)
                {
                    var em = particle.Material as EmissiveMaterial;
                    if (em != null)
                    {
                        em.Color = ToColor(particle.Color);
                    }
                    else
                    {
                        var dm = particle.Material as DiffuseMaterial;
                        if (dm != null)
                        {
                            dm.Color = ToColor(particle.Color);
                        }
                    }
                }

                // 行列変換
                var m = new Matrix3D();

                if (particle.Scale != 1.0)
                {
                    m.Scale(new Vector3D(particle.Scale, particle.Scale, 1.0));
                }

                if (particle.Rotation != 0.0)
                {
                    double rot = MathEx.ToDeg(particle.Rotation);
                    m.Rotate(new Quaternion(new Vector3D(0, 0, 1), rot));
                }

                m.Translate(new Vector3D(particle.X, particle.Y, -15.0));

                particle.Model.Transform = new MatrixTransform3D(m);
            }
        }