public static void CalcTMatrix(Vector3D value)
 {
     Tmatrix = new Matrix3D(1, 0, 0, -value.X,
                            0, 1, 0, -value.Y,
                            0, 0, 1, -value.Z,
                            0, 0, 0, 1);    
 }     

 internal static void PrependInverseTransform(Transform3D transform, ref Matrix3D viewMatrix)
 {
     if (transform != null && transform != Transform3D.Identity) 
     {
         PrependInverseTransform(transform.Value, ref viewMatrix); 
     } 
 }

        private static void WriteTransformMatrix(Matrix3D m, XmlWriter bw)
        {
            bw.WriteStartElement("T");

            if (m.IsIdentity)
                bw.WriteAttributeString("value", "Identity");
            else
            {
                bw.WriteAttributeString("M11", m.M11.ToString());
                bw.WriteAttributeString("M12", m.M12.ToString());
                bw.WriteAttributeString("M13", m.M13.ToString());
                bw.WriteAttributeString("M14", m.M14.ToString());

                bw.WriteAttributeString("M21", m.M21.ToString());
                bw.WriteAttributeString("M22", m.M22.ToString());
                bw.WriteAttributeString("M23", m.M23.ToString());
                bw.WriteAttributeString("M24", m.M24.ToString());

                bw.WriteAttributeString("M31", m.M31.ToString());
                bw.WriteAttributeString("M32", m.M32.ToString());
                bw.WriteAttributeString("M33", m.M33.ToString());
                bw.WriteAttributeString("M34", m.M34.ToString());

                bw.WriteAttributeString("M41", m.OffsetX.ToString());
                bw.WriteAttributeString("M42", m.OffsetY.ToString());
                bw.WriteAttributeString("M43", m.OffsetZ.ToString());
                bw.WriteAttributeString("M44", m.M44.ToString());
            }
            bw.WriteEndElement();
        }

        /// <summary>
        /// 当窗口大小发生改变时，更新视口投影矩阵
        /// </summary>
        public void UpdateProjViewMat(Double height, double width)
        {
            double aspectRatio = width / height;
            double FoV = MathUtils.DegreesToRadians(45);
            double zn = 0.125;
            double xScale = 1 / Math.Tan(FoV / 2);
            double yScale = aspectRatio * xScale;
            //double yScale = 1 / Math.Tan(FoV / 2);
            //double xScale = yScale / aspectRatio;
            double m33 = -1;
            double m43 = zn * m33;
            projViewMat = new Matrix3D(
                xScale, 0, 0, 0,
                0, yScale, 0, 0,
                0, 0, m33, -1,
                0, 0, m43, 0);

            double scaleX = width / 2;
            double scaleY = height / 2;

            projViewMat.Append(new Matrix3D(
                scaleX, 0, 0, 0,
                0, -scaleY, 0, 0,
                0, 0, 1, 0,
                scaleX, scaleY, 0, 1));

            UpdateTo2DMat();
        }

        public PhotoAlbum()
        {
            InitializeComponent();
            dt.Interval = TimeSpan.FromSeconds(2);
            dt.Tick += new EventHandler(dt_Tick);
            for (int i = 0; i < 16; i++)
            {
                ImageBrush ib = new ImageBrush(new BitmapImage(new Uri("pack://application:,,,/images/albums/im" + i + ".jpg")));
                ib.Stretch = Stretch.Uniform;

                ModelVisual3D mv = new ModelVisual3D();
                Material mat = new DiffuseMaterial(ib);
                GeometryModel3D plane = new GeometryModel3D(planeFactory.Mesh, mat);
                mv.Content = plane;
                mvs[i] = mv;
                myViewPort3D.Children.Add(mv);
                Matrix3D trix = new Matrix3D();
                double x = ran.NextDouble() * 50 - 50;
                double y = ran.NextDouble() * 2 - 2;
                double z = -i * 10;
                p3s[i] = new Point3D(x, y, z);
                trix.Append(new TranslateTransform3D(x, y, z).Value);
                mv.Transform = new MatrixTransform3D(trix);
            }

            pa = new Point3DAnimation(p3s[0], TimeSpan.FromMilliseconds(300));
            pa.AccelerationRatio = 0.3;
            pa.DecelerationRatio = 0.3;
            pa.Completed += new EventHandler(pa_Completed);
            cam.BeginAnimation(PerspectiveCamera.PositionProperty, pa);
        }

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

        private int AddModelFaces(Matrix3D parentMatrix, Model3D model)
        {
            if (model.Transform != null)
                parentMatrix = model.Transform.Value * parentMatrix;

            int result = 0;
            Model3DGroup models = (model as Model3DGroup);
            if (models != null)
            {
                // This is a group.  Recurse through the children
                foreach (Model3D m in models.Children)
                {
                    result += AddModelFaces(parentMatrix, m);
                }
            }
            else
            {
                if (!(model is GeometryModel3D))
                    throw new InvalidOperationException("Current only GeometryModel3D models supported for TerrianCollisionMask3D.");

                Geometry3D geometry = ((GeometryModel3D)model).Geometry;

                IList<Point3D> meshPoints = GeometryHelper.GetGeometryPoints((MeshGeometry3D)geometry, parentMatrix);
                if (meshPoints != null)
                {
                    AddFaces(meshPoints, 3, false);
                    result = meshPoints.Count;
                }
            }

            return result;
        }

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

        protected override CollisionMask OnInitialise(Matrix3D initialMatrix)
        {
            if (Visual.Content != null)
            {
                TerrianCollisionMask3D terrianCollisionMask3D = null;
                CollisionMask collisionMask = _GetCollisionMask(this.Visual);
                if (collisionMask != null)
                {
                    if (!(collisionMask is TerrianCollisionMask3D))
                        throw new InvalidOperationException("Currently another type of CollisionMaskMask is attached to this visual.");
                    else
                        terrianCollisionMask3D = (TerrianCollisionMask3D)collisionMask;
                }
                else
                {
                    terrianCollisionMask3D = new TerrianCollisionMask3D();
                    _SetCollisionMask(this.Visual, terrianCollisionMask3D);
                }

                terrianCollisionMask3D.Initialise(this.World, initialMatrix);
                return terrianCollisionMask3D;
            }
            else
                return null;
        }

 /// <summary>
 /// Creates a new detection result
 /// </summary>
 /// <param name="marker">A reference to the found marker.</param>
 /// <param name="confidence">The confidence / quality  of the result.</param>
 /// <param name="transformation">The transformation matrix for the marker.</param>
 /// <param name="square">The pixel coordinates where the square marker was found. </param>
 public DetectionResult(Marker marker, double confidence, Matrix3D transformation, Square square)
 {
    this.Marker          = marker;
    this.Confidence      = confidence;
    this.Transformation  = transformation;
    this.Square          = square;
 }

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

		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 CTire VehicleAddTire(Matrix3D pLocalMatrix,
            Vector3D pPin,
            float pMass,
            float pWidth,
            float pRadius,
            float pSuspesionShock,
            float pSuspesionSpring,
            float pSuspesionLength,
            object pUserData,
            int pCollisionID)
        {
            IntPtr aTireHandle = Newton.NewtonVehicleAddTire(m_Handle,
                        new NewtonMatrix(pLocalMatrix).NWMatrix,
                        new NewtonVector3(pPin).NWVector3,
                        pMass,
                        pWidth,
                        pRadius,
                        pSuspesionShock,
                        pSuspesionSpring,
                        pSuspesionLength,
                        (IntPtr)0, //pUserData.GetHashCode(),
                        pCollisionID);

            CTire aTire = new CTire(this, aTireHandle);
            aTire.UserData = pUserData;

            return aTire;
        }

		/// <summary>
		/// Constructor
		/// </summary>
		/// <param name="matrix">Combined matrix</param>
		/// <param name="normalize">Normalize?</param>
		public Frustum( Matrix3D matrix, bool normalize )
		{
			// Left clipping plane
			planes[ 0 ].Normal = new Vector3D( matrix.M14 + matrix.M11, matrix.M24 + matrix.M21, matrix.M34 + matrix.M31 );
			planes[ 0 ].D = matrix.M44 + matrix.OffsetX;

			// Right clipping plane
			planes[ 1 ].Normal = new Vector3D( matrix.M14 - matrix.M11, matrix.M24 - matrix.M21, matrix.M34 - matrix.M31 );
			planes[ 1 ].D = matrix.M44 - matrix.OffsetX;

			// Top clipping plane
			planes[ 2 ].Normal = new Vector3D( matrix.M14 - matrix.M12, matrix.M24 - matrix.M22, matrix.M34 - matrix.M32 );
			planes[ 2 ].D = matrix.M44 - matrix.OffsetY;

			// Bottom clipping plane
			planes[ 3 ].Normal = new Vector3D( matrix.M14 + matrix.M12, matrix.M24 + matrix.M22, matrix.M34 + matrix.M32 );
			planes[ 3 ].D = matrix.M44 + matrix.OffsetY;

			// Near clipping plane
			planes[ 4 ].Normal = new Vector3D( matrix.M13, matrix.M23, matrix.M33 );
			planes[ 4 ].D = matrix.OffsetZ;

			// Far clipping plane
			planes[ 5 ].Normal = new Vector3D( matrix.M14 - matrix.M13, matrix.M24 - matrix.M23, matrix.M34 - matrix.M33 );
			planes[ 5 ].D = matrix.M44 - matrix.OffsetZ;

			// Normalize the plane equations, if requested
			if ( normalize )
			{
				for ( int index = 0; index < planes.Length; ++index ) planes[ index ].Normalize();
			}
		}

        private static Matrix3D GetViewMatrix(ProjectionCamera camera)
        {
            if (camera == null) throw new ArgumentNullException("camera");

            // This math is identical to what you find documented for
            // D3DXMatrixLookAtRH with the exception that WPF uses a
            // LookDirection vector rather than a LookAt point.

            Vector3D zAxis = -camera.LookDirection;
            zAxis.Normalize();

            Vector3D xAxis = Vector3D.CrossProduct(camera.UpDirection, zAxis);
            xAxis.Normalize();

            Vector3D yAxis = Vector3D.CrossProduct(zAxis, xAxis);

            Vector3D position = (Vector3D)camera.Position;
            double offsetX = -Vector3D.DotProduct(xAxis, position);
            double offsetY = -Vector3D.DotProduct(yAxis, position);
            double offsetZ = -Vector3D.DotProduct(zAxis, position);

            Matrix3D m = new Matrix3D(
                xAxis.X, yAxis.X, zAxis.X, 0,
                xAxis.Y, yAxis.Y, zAxis.Y, 0,
                xAxis.Z, yAxis.Z, zAxis.Z, 0,
                offsetX, offsetY, offsetZ, 1);

            return m;
        }

        //// 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 Matrix3D ToMatrix3D(this Matrix matrix)
        {
            Matrix3D matrix3D = new Matrix3D()
            {
                M11 = matrix.M11,
                M12 = matrix.M12,
                M13 = matrix.M13,
                M14 = matrix.M14,

                M21 = matrix.M21,
                M22 = matrix.M22,
                M23 = matrix.M23,
                M24 = matrix.M24,

                M31 = matrix.M31,
                M32 = matrix.M32,
                M33 = matrix.M33,
                M34 = matrix.M34,

                OffsetX = matrix.M41,
                OffsetY = matrix.M42,
                OffsetZ = matrix.M43,
                M44 = matrix.M44
            };

            return matrix3D;
        }

        public static Matrix3D ToMatrix3D( this Matrix4x4F source )
        {
            Matrix3D destination = new Matrix3D( );
            destination.M11 = (float)source.M11;
            destination.M12 = (float)source.M12;
            destination.M13 = (float)source.M13;
            destination.M14 = (float)source.M14;

            destination.M21 = (float)source.M21;
            destination.M22 = (float)source.M22;
            destination.M23 = (float)source.M23;
            destination.M24 = (float)source.M24;

            destination.M31 = (float)source.M31;
            destination.M32 = (float)source.M32;
            destination.M33 = (float)source.M33;
            destination.M34 = (float)source.M34;

            destination.OffsetX = (float)source.M41;
            destination.OffsetY = (float)source.M42;
            destination.OffsetZ = (float)source.M43;
            destination.M44 = (float)source.M44;

            return destination;
        }

        /// <summary>
        /// Create a look at matrix using U, V, and N vectors
        /// </summary>
        /// <param name="eye">Location of the eye</param>
        /// <param name="lookDirection">Direction to look</param>
        /// <param name="up">The up vector</param>
        /// <returns>Transform matrix from world space to camera space.</returns>
        public static Matrix3D LookAt(Vector3D eye, Vector3D lookDirection, Vector3D up)
        {
            var n = lookDirection;
            n.Normalize();

            var v = up - (Vector3D.DotProduct(up, n) * n);
            v.Normalize();

            // The "-" below makes this be a right-handed uvn space so we aren't
            // negating the x component in SensorToScreenCoordinatesTransform.
            // It also makes SensorToScreenPositionTransform give an immediately
            // usable transform without having to flip the X.
            var u = -Vector3D.CrossProduct(n, v);

            var lookAt = new Matrix3D(
                u.X,
                v.X,
                n.X,
                0,
                u.Y,
                v.Y,
                n.Y,
                0,
                u.Z,
                v.Z,
                n.Z,
                0,
                -Vector3D.DotProduct(u, eye),
                -Vector3D.DotProduct(v, eye),
                -Vector3D.DotProduct(n, eye),
                1);

            return lookAt;
        }

		public static Vector3D MatrixToEulerAngle(Matrix3D pMatrix)
		{
			NewtonVector3 aNewtonVector3 = new NewtonVector3( new Vector3D() );
			Newton.NewtonGetEulerAngle(new NewtonMatrix(pMatrix).NWMatrix,
				aNewtonVector3.NWVector3);
			return aNewtonVector3.ToDirectX();
		}