C# Math.Matrix4x4类代码示例

        public void ToArrayTest( )
        {
            Matrix4x4 matrix = new Matrix4x4( );

            matrix.V00 = 1;
            matrix.V01 = 2;
            matrix.V02 = 3;
            matrix.V03 = 4;

            matrix.V10 = 5;
            matrix.V11 = 6;
            matrix.V12 = 7;
            matrix.V13 = 8;

            matrix.V20 = 9;
            matrix.V21 = 10;
            matrix.V22 = 11;
            matrix.V23 = 12;

            matrix.V30 = 13;
            matrix.V31 = 14;
            matrix.V32 = 15;
            matrix.V33 = 16;

            float[] array = matrix.ToArray( );

            for ( int i = 0; i < 16; i++ )
            {
                Assert.AreEqual<float>( array[i], (float) ( i + 1 ) );
            }
        }

 public void updateAngle(float y,float p,float r)
 {
     worldMatrix =
             Matrix4x4.CreateTranslation(new Vector3(0, 0, 0)) *
             Matrix4x4.CreateFromYawPitchRoll(y, p, r);
         Recalculate();
 }

        public void AddMatricesTest( )
        {
            Matrix4x4 expectedResult = new Matrix4x4( );

            expectedResult.V00 = 3;
            expectedResult.V01 = 3;
            expectedResult.V02 = 7;
            expectedResult.V03 = 7;

            expectedResult.V10 = 8;
            expectedResult.V11 = 5;
            expectedResult.V12 = 5;
            expectedResult.V13 = 2;

            expectedResult.V20 = 6;
            expectedResult.V21 = 5;
            expectedResult.V22 = 5;
            expectedResult.V23 = 4;

            expectedResult.V30 = 3;
            expectedResult.V31 = 7;
            expectedResult.V32 = 3;
            expectedResult.V33 = 7;

            Matrix4x4 result = a1 + a2;

            Assert.AreEqual<bool>( true, ApproximateEquals( result, expectedResult ) );
        }

        public void setTransformatinMatrix(Matrix4x4 m)
        {
            estimate00.Text = m.V00.ToString();
            estimate10.Text = m.V01.ToString();
            estimate20.Text = m.V01.ToString();
            estimate30.Text = m.V03.ToString();

            estimate01.Text = m.V10.ToString();
            estimate11.Text = m.V11.ToString();
            estimate21.Text = m.V12.ToString();
            estimate31.Text = m.V13.ToString();

            estimate02.Text = m.V20.ToString();
            estimate12.Text = m.V21.ToString();
            estimate22.Text = m.V22.ToString();
            estimate32.Text = m.V23.ToString();

            estimate03.Text = m.V30.ToString();
            estimate13.Text = m.V31.ToString();
            estimate23.Text = m.V32.ToString();
            estimate33.Text = m.V33.ToString();



        }

        public void SetMatrix( Matrix4x4 matrix )
        {
            float[] array = matrix.ToArray( );

            for ( int i = 0, k = 0; i < 4; i++ )
            {
                for ( int j = 0; j < 4; j++, k++ )
                {
                    string textBoxName = string.Format( "i{0}_j{1}_Box", i, j );

                    if ( textBoxes.ContainsKey( textBoxName ) )
                    {
                        textBoxes[textBoxName].Text = FormatFloat( array[k] );
                    }
                }
            }
        }

        /// <summary>
        ///   Executes the transform calculations (T = Z*X).
        /// </summary>
        /// 
        /// <returns>Transform matrix T.</returns>
        /// 
        /// <remarks>Calling this method also updates the Transform property.</remarks>
        /// 
        public void Compute(DenavitHartenbergParameters parameters)
        {
            // Calculate Z with Z = TranslationZ(d).RotationZ(theta)
            Z = Matrix4x4.Multiply
            (
                Matrix4x4.CreateTranslation(new Vector3(0f, 0f, (float)parameters.Offset)),
                Matrix4x4.CreateRotationZ((float)parameters.Theta)
            );

            // Calculate X with X = TranslationX(radius).RotationZ(alpha)
            X = Matrix4x4.Multiply
            (
                Matrix4x4.CreateTranslation(new Vector3((float)parameters.Radius, 0f, 0f)),
                Matrix4x4.CreateRotationX((float)parameters.Alpha)
            );

            // Calculate the transform with T=Z.X
            Transform = Matrix4x4.Multiply(Z, X);
        }

        public void setTransformatinMatrix(Matrix4x4 m)
        {
            est00.Text = m.V00.ToString();
            est01.Text = m.V01.ToString();
            est02.Text = m.V01.ToString();
            est03.Text = m.V03.ToString();

            est10.Text = m.V10.ToString();
            est11.Text = m.V11.ToString();
            est12.Text = m.V12.ToString();
            est13.Text = m.V13.ToString();

            est20.Text = m.V20.ToString();
            est21.Text = m.V21.ToString();
            est22.Text = m.V22.ToString();
            est23.Text = m.V23.ToString();

            est30.Text = m.V30.ToString();
            est31.Text = m.V31.ToString();
            est32.Text = m.V32.ToString();
            est33.Text = m.V33.ToString();
        }

        public ViewPosit(int w,int h)
        {
            sizeRect = 50;
            wImg = w;
            hImg = h;
            center = new Point(w / 2, h / 2);
            pktList = new System.Collections.Generic.List<Point>();
            pktList.Add(new Point(center.X + sizeRect, center.Y + sizeRect));
            pktList.Add(new Point(center.X - sizeRect, center.Y + sizeRect));
            pktList.Add(new Point(center.X - sizeRect, center.Y - sizeRect));
            pktList.Add(new Point(center.X + sizeRect, center.Y - sizeRect));

            colorList = new System.Collections.Generic.List<Bgr>();
            colorList.Add(new Bgr(255,0,0));
            colorList.Add(new Bgr(0,255,0));
            colorList.Add(new Bgr(0,0,255));
            colorList.Add(new Bgr(255,0,255));

            worldMatrix = Matrix4x4.Identity;
            viewMatrix = Matrix4x4.CreateLookAt(new Vector3(0, 0, 5), new Vector3(0, 0, 0));
            perspectiveMatrix = Matrix4x4.CreatePerspective(1, 1, 1, 1000);
        }

        public void setMtx(Matrix4x4 mtx)
        {
            m00.Text = mtx.V00.ToString();
            m01.Text = mtx.V01.ToString();
            m02.Text = mtx.V02.ToString();
            m03.Text = mtx.V03.ToString();

            m10.Text = mtx.V10.ToString();
            m11.Text = mtx.V11.ToString();
            m12.Text = mtx.V12.ToString();
            m13.Text = mtx.V13.ToString();

            m20.Text = mtx.V20.ToString();
            m21.Text = mtx.V21.ToString();
            m22.Text = mtx.V22.ToString();
            m23.Text = mtx.V23.ToString();

            m30.Text = mtx.V30.ToString();
            m31.Text = mtx.V31.ToString();
            m32.Text = mtx.V32.ToString();
            m33.Text = mtx.V33.ToString();
        }

        public WorldRendererControl( )
        {
            InitializeComponent( );

            objectPoints = new Vector3[]
            {
                new Vector3( 0, 0, 0 ),
            };

            colors = new Color[]
            {
                Color.White,
            };

            lines = new int[0, 2];

            // create default matrices
            worldMatrix = Matrix4x4.Identity;
            viewMatrix  = Matrix4x4.CreateLookAt( new Vector3( 0, 0, 5 ), new Vector3( 0, 0, 0 ) );
            perspectiveMatrix = Matrix4x4.CreatePerspective( 1, 1, 1, 1000 );

            Recalculate( );
        }

 /// <summary>
 /// Adds corresponding components of two matrices.
 /// </summary>
 /// 
 /// <param name="matrix1">The matrix to add to.</param>
 /// <param name="matrix2">The matrix to add to the first matrix.</param>
 /// 
 /// <returns>Returns a matrix which components are equal to sum of corresponding
 /// components of the two specified matrices.</returns>
 ///
 public static Matrix4x4 Add(Matrix4x4 matrix1, Matrix4x4 matrix2)
 {
     return matrix1 + matrix2;
 }

        /// <summary>
        /// Adds corresponding components of two matrices.
        /// </summary>
        /// 
        /// <param name="matrix1">The matrix to add to.</param>
        /// <param name="matrix2">The matrix to add to the first matrix.</param>
        /// 
        /// <returns>Returns a matrix which components are equal to sum of corresponding
        /// components of the two specified matrices.</returns>
        ///
        public static Matrix4x4 operator +(Matrix4x4 matrix1, Matrix4x4 matrix2)
        {
            Matrix4x4 m = new Matrix4x4();

            m.V00 = matrix1.V00 + matrix2.V00;
            m.V01 = matrix1.V01 + matrix2.V01;
            m.V02 = matrix1.V02 + matrix2.V02;
            m.V03 = matrix1.V03 + matrix2.V03;

            m.V10 = matrix1.V10 + matrix2.V10;
            m.V11 = matrix1.V11 + matrix2.V11;
            m.V12 = matrix1.V12 + matrix2.V12;
            m.V13 = matrix1.V13 + matrix2.V13;

            m.V20 = matrix1.V20 + matrix2.V20;
            m.V21 = matrix1.V21 + matrix2.V21;
            m.V22 = matrix1.V22 + matrix2.V22;
            m.V23 = matrix1.V23 + matrix2.V23;

            m.V30 = matrix1.V30 + matrix2.V30;
            m.V31 = matrix1.V31 + matrix2.V31;
            m.V32 = matrix1.V32 + matrix2.V32;
            m.V33 = matrix1.V33 + matrix2.V33;

            return m;
        }

 /// <summary>
 /// Multiplies two specified matrices.
 /// </summary>
 /// 
 /// <param name="matrix1">Matrix to multiply.</param>
 /// <param name="matrix2">Matrix to multiply by.</param>
 /// 
 /// <returns>Return new matrix, which the result of multiplication of the two specified matrices.</returns>
 /// 
 public static Matrix4x4 Multiply(Matrix4x4 matrix1, Matrix4x4 matrix2)
 {
     return matrix1 * matrix2;
 }

        public void MultiplyMatricesTest( )
        {
            Matrix4x4 expectedResult = new Matrix4x4( );

            expectedResult.V00 = 23;
            expectedResult.V01 = 29;
            expectedResult.V02 = 21;
            expectedResult.V03 = 27;

            expectedResult.V10 = 27;
            expectedResult.V11 = 21;
            expectedResult.V12 = 29;
            expectedResult.V13 = 23;

            expectedResult.V20 = 24;
            expectedResult.V21 = 27;
            expectedResult.V22 = 23;
            expectedResult.V23 = 26;

            expectedResult.V30 = 26;
            expectedResult.V31 = 23;
            expectedResult.V32 = 27;
            expectedResult.V33 = 24;

            Matrix4x4 result = a1 * a2;

            Assert.AreEqual<bool>( true, ApproximateEquals( result, expectedResult ) );
        }

        private AForge.Point[] PerformProjection( Vector3[] model, Matrix4x4 transformationMatrix, int viewSize )
        {
            AForge.Point[] projectedPoints = new AForge.Point[model.Length];

            for ( int i = 0; i < model.Length; i++ )
            {
                Vector3 scenePoint = ( transformationMatrix * model[i].ToVector4( ) ).ToVector3( );

                projectedPoints[i] = new AForge.Point(
                    (int) ( scenePoint.X / scenePoint.Z * viewSize ),
                    (int) ( scenePoint.Y / scenePoint.Z * viewSize ) );
            }

            return projectedPoints;
        }