public static Skeleton ApplyTransformToFrame(Skeleton frame, Matrix3x3 trans)
 {
     Skeleton res = new Skeleton();
     foreach (Joint j in frame.Joints)
     {
         Joint j2 = j;
         j2.Position = MultiplyPositionMatrix(trans, j2.Position);
         res.Joints[j2.JointType] = j2;
     }
     return res;
 }

        public static Matrix3x3 GetRotationMatrix(Skeleton mFrame, Skeleton rFrame)
        {
            Skeleton mFrameShift = ShiftFrame(mFrame, mFrame.Joints[JointType.HipCenter].Position);
            Skeleton rFrameShift = ShiftFrame(rFrame, rFrame.Joints[JointType.HipCenter].Position);

            Matrix3x3 rotMatrix = new Matrix3x3();
            double bestDist = -1;
            float bestR = 0;
            for (float r = (float)-Math.PI / 2; r < Math.PI / 2; r = r + (float)(Math.PI / 100))
            {
                rotMatrix = Matrix3x3.CreateRotationX(r);
                double dist = GetDistBetweenFrames(mFrameShift, ApplyTransformToFrame(rFrameShift, rotMatrix));
                if (dist < bestDist || bestDist < 0)
                {
                    bestR = r;
                    bestDist = dist;
                }
            }
            float alpha = bestR;
            rFrameShift = ApplyTransformToFrame(rFrameShift, Matrix3x3.CreateRotationX(bestR));

            bestDist = -1;
            bestR = 0;
            for (float r = (float)-Math.PI / 2; r < Math.PI / 2; r = r + (float)(Math.PI / 100))
            {
                rotMatrix = Matrix3x3.CreateRotationY(r);
                double dist = GetDistBetweenFrames(mFrameShift, ApplyTransformToFrame(rFrameShift, rotMatrix));
                if (dist < bestDist || bestDist < 0)
                {
                    bestR = r;
                    bestDist = dist;
                }
            }
            float beta = bestR;
            rFrameShift = ApplyTransformToFrame(rFrameShift, Matrix3x3.CreateRotationY(bestR));

            bestDist = -1;
            bestR = 0;
            for (float r = (float)-Math.PI / 2; r < Math.PI / 2; r = r + (float)(Math.PI / 100))
            {
                rotMatrix = Matrix3x3.CreateRotationZ(r);
                double dist = GetDistBetweenFrames(mFrameShift, ApplyTransformToFrame(rFrameShift, rotMatrix));
                if (dist < bestDist || bestDist < 0)
                {
                    bestR = r;
                    bestDist = dist;
                }
            }
            float gamma = bestR;

            return Matrix3x3.CreateFromYawPitchRoll(beta, alpha, gamma);
        }

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

            expectedResult.V00 = 3;
            expectedResult.V01 = 3;
            expectedResult.V02 = 6;

            expectedResult.V10 = 4;
            expectedResult.V11 = 5;
            expectedResult.V12 = 3;

            expectedResult.V20 = 4;
            expectedResult.V21 = 5;
            expectedResult.V22 = 3;

            Matrix3x3 result = a1 + a2;

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

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

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

            matrix.V10 = 4;
            matrix.V11 = 5;
            matrix.V12 = 6;

            matrix.V20 = 7;
            matrix.V21 = 8;
            matrix.V22 = 9;

            float[] array = matrix.ToArray( );

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

        /// <summary>
        /// Creates 4x4 tranformation matrix from 3x3 rotation matrix.
        /// </summary>
        /// 
        /// <param name="rotationMatrix">Source 3x3 rotation matrix.</param>
        /// 
        /// <returns>Returns 4x4 rotation matrix.</returns>
        /// 
        /// <remarks><para>The source 3x3 rotation matrix is copied into the top left corner of the result 4x4 matrix,
        /// i.e. it represents 0th, 1st and 2nd row/column. The <see cref="V33"/> element is set to 1 and the rest
        /// elements of 3rd row and 3rd column are set to zeros.</para></remarks>
        /// 
        public static Matrix4x4 CreateFromRotation(Matrix3x3 rotationMatrix)
        {
            Matrix4x4 m = Matrix4x4.Identity;

            m.V00 = rotationMatrix.V00;
            m.V01 = rotationMatrix.V01;
            m.V02 = rotationMatrix.V02;

            m.V10 = rotationMatrix.V10;
            m.V11 = rotationMatrix.V11;
            m.V12 = rotationMatrix.V12;

            m.V20 = rotationMatrix.V20;
            m.V21 = rotationMatrix.V21;
            m.V22 = rotationMatrix.V22;

            return m;
        }

        // Calculate average error between real angles of the specified quadrilateral and angles of the
        // quadrilateral which is the projection of currently estimated pose
        private float GetError( Point[] imagePoints, Matrix3x3 rotation, Vector3 translation )
        {
            Vector3 v1 = rotation * modelPoints[0] + translation;
            v1.X = v1.X * focalLength / v1.Z;
            v1.Y = v1.Y * focalLength / v1.Z;

            Vector3 v2 = rotation * modelPoints[1] + translation;
            v2.X = v2.X * focalLength / v2.Z;
            v2.Y = v2.Y * focalLength / v2.Z;

            Vector3 v3 = rotation * modelPoints[2] + translation;
            v3.X = v3.X * focalLength / v3.Z;
            v3.Y = v3.Y * focalLength / v3.Z;

            Vector3 v4 = rotation * modelPoints[3] + translation;
            v4.X = v4.X * focalLength / v4.Z;
            v4.Y = v4.Y * focalLength / v4.Z;

            Point[] modeledPoints = new Point[4]
            {
                new Point( v1.X, v1.Y ),
                new Point( v2.X, v2.Y ),
                new Point( v3.X, v3.Y ),
                new Point( v4.X, v4.Y ),
            };

            float ia1 = GeometryTools.GetAngleBetweenVectors( imagePoints[0], imagePoints[1], imagePoints[3] );
            float ia2 = GeometryTools.GetAngleBetweenVectors( imagePoints[1], imagePoints[2], imagePoints[0] );
            float ia3 = GeometryTools.GetAngleBetweenVectors( imagePoints[2], imagePoints[3], imagePoints[1] );
            float ia4 = GeometryTools.GetAngleBetweenVectors( imagePoints[3], imagePoints[0], imagePoints[2] );

            float ma1 = GeometryTools.GetAngleBetweenVectors( modeledPoints[0], modeledPoints[1], modeledPoints[3] );
            float ma2 = GeometryTools.GetAngleBetweenVectors( modeledPoints[1], modeledPoints[2], modeledPoints[0] );
            float ma3 = GeometryTools.GetAngleBetweenVectors( modeledPoints[2], modeledPoints[3], modeledPoints[1] );
            float ma4 = GeometryTools.GetAngleBetweenVectors( modeledPoints[3], modeledPoints[0], modeledPoints[2] );

            return (
                System.Math.Abs( ia1 - ma1 ) +
                System.Math.Abs( ia2 - ma2 ) +
                System.Math.Abs( ia3 - ma3 ) +
                System.Math.Abs( ia4 - ma4 )
                ) / 4;
        }

 /// <summary>
 /// Multiplies matrix by the specified factor.
 /// </summary>
 /// 
 /// <param name="matrix">Matrix to multiply.</param>
 /// <param name="factor">Factor to multiple the specified matrix by.</param>
 /// 
 /// <returns>Returns new matrix with all components equal to corresponding components of the
 /// specified matrix multiples by the specified factor.</returns>
 /// 
 public static Matrix3x3 Multiply( Matrix3x3 matrix, float factor )
 {
     return matrix * factor;
 }

        private void CompareMatrixWithArray( Matrix3x3 matrix, float[] array )
        {
            float[] matrixArray = matrix.ToArray( );

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

        public void SubtractMatricesTest( )
        {
            Matrix3x3 expectedResult = new Matrix3x3( );

            expectedResult.V00 = -1;
            expectedResult.V01 = 1;
            expectedResult.V02 = 0;

            expectedResult.V10 = 2;
            expectedResult.V11 = -1;
            expectedResult.V12 = -1;

            expectedResult.V20 = -2;
            expectedResult.V21 = 1;
            expectedResult.V22 = 1;

            Matrix3x3 result = a1 - a2;

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

 /// <summary>
 /// Tests whether the matrix equals to the specified one.
 /// </summary>
 /// 
 /// <param name="matrix">The matrix to test equality with.</param>
 /// 
 /// <returns>Returns <see langword="true"/> if the two matrices are equal or <see langword="false"/> otherwise.</returns>
 /// 
 public bool Equals( Matrix3x3 matrix )
 {
     return ( this == matrix );
 }

 public static Skeleton TransRotateTrans(Skeleton frame, Matrix3x3 rot)
 {
     Skeleton frameShift = ShiftFrame(frame, frame.Joints[JointType.HipCenter].Position);
     Skeleton frameRot = ApplyTransformToFrame(frameShift, rot);
     return ShiftFrame(frameRot, NegativePosition(frame.Joints[JointType.HipCenter].Position));
 }

        /// <summary>
        /// Estimate pose of a model from it's projected 2D coordinates.
        /// </summary>
        /// 
        /// <param name="points">4 2D points of the <see cref="Model">model's</see> projection.</param>
        /// <param name="rotation">Gets object's rotation.</param>
        /// <param name="translation">Gets object's translation.</param>
        /// 
        /// <exception cref="ArgumentException">4 points must be be given for pose estimation.</exception>
        /// 
        public void EstimatePose( Point[] points, out Matrix3x3 rotation, out Vector3 translation )
        {
            if ( points.Length != 4 )
            {
                throw new ArgumentException( "4 points must be be given for pose estimation." );
            }

            float Z0 = 0, scale = 1;

            Vector3 X0 = new Vector3( points[0].X );
            Vector3 Y0 = new Vector3( points[0].Y );

            Vector3 XI = new Vector3( points[1].X, points[2].X, points[3].X );
            Vector3 YI = new Vector3( points[1].Y, points[2].Y, points[3].Y );

            int count = 0;

            Vector3 iVector = new Vector3( );
            Vector3 jVector = new Vector3( );
            Vector3 kVector = new Vector3( );
            Vector3 imageXs = new Vector3( );
            Vector3 imageYs = new Vector3( );

            Vector3 eps = new Vector3( 1 );

            for ( ; count < 100; count++ )
            {
                // calculate new scale orthographic projection (SOP)
                imageXs = XI * eps - X0;
                imageYs = YI * eps - Y0;

                // calculate I and J vectors
                iVector = modelPseudoInverse * imageXs;
                jVector = modelPseudoInverse * imageYs;
                // convert them to unit vectors i and j
                float iNorm = iVector.Normalize( );
                float jNorm = jVector.Normalize( );
                // scale of projection
                scale = ( iNorm + jNorm ) / 2;
                // calculate n vector k
                kVector = Vector3.Cross( iVector, jVector );
                // z-coordinate Z0 of the translation vector
                Z0 = focalLength / scale;

                // calculate new epsilon values
                Vector3 oldEps = eps;
                eps = ( modelVectors * kVector ) / Z0 + 1;

                // check if it is time to stop
                if ( ( eps - oldEps ).Abs( ).Max < stop_epsilon )
                    break;
            }

            // create rotation matrix
            rotation = Matrix3x3.CreateFromRows( iVector, jVector, kVector );

            // create translation vector
            Vector3 temp = rotation * modelPoints[0];
            translation = new Vector3(
                points[0].X / scale - temp.X,
                points[0].Y / scale - temp.Y,
                focalLength / scale );
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="Posit"/> class.
        /// </summary>
        /// 
        /// <param name="model">Array of vectors containing coordinates of four real model's point (points
        /// must not be on the same plane).</param>
        /// <param name="focalLength">Effective focal length of the camera used to capture the model.</param>
        /// 
        /// <exception cref="ArgumentException">The model must have 4 points.</exception>
        /// 
        public Posit( Vector3[] model, float focalLength )
        {
            if ( model.Length != 4 )
            {
                throw new ArgumentException( "The model must have 4 points." );
            }

            this.focalLength = focalLength;
            modelPoints = (Vector3[]) model.Clone( );

            // compute model vectors
            modelVectors = Matrix3x3.CreateFromRows(
                model[1] - model[0],
                model[2] - model[0],
                model[3] - model[0] );

            // compute pseudo inverse matrix
            modelPseudoInverse = modelVectors.PseudoInverse( );
        }

        /// <summary>
        /// Calculate Singular Value Decomposition (SVD) of the matrix, such as A=U*E*V<sup>T</sup>.
        /// </summary>
        /// 
        /// <param name="u">Output parameter which gets 3x3 U matrix.</param>
        /// <param name="e">Output parameter which gets diagonal elements of the E matrix.</param>
        /// <param name="v">Output parameter which gets 3x3 V matrix.</param>
        /// 
        /// <remarks><para>Having components U, E and V the source matrix can be reproduced using below code:
        /// <code>
        /// Matrix3x3 source = u * Matrix3x3.Diagonal( e ) * v.Transpose( );
        /// </code>
        /// </para></remarks>
        /// 
        public void SVD( out Matrix3x3 u, out Vector3 e, out Matrix3x3 v )
        {
            double[,] uArray = new double[3, 3]
            {
                { V00, V01, V02 },
                { V10, V11, V12 },
                { V20, V21, V22 }
            };
            double[,] vArray;
            double[] eArray;

            svd.svdcmp( uArray, out eArray, out vArray );

            // build U matrix
            u = new Matrix3x3( );
            u.V00 = (float) uArray[0, 0];
            u.V01 = (float) uArray[0, 1];
            u.V02 = (float) uArray[0, 2];
            u.V10 = (float) uArray[1, 0];
            u.V11 = (float) uArray[1, 1];
            u.V12 = (float) uArray[1, 2];
            u.V20 = (float) uArray[2, 0];
            u.V21 = (float) uArray[2, 1];
            u.V22 = (float) uArray[2, 2];

            // build V matrix
            v = new Matrix3x3( );
            v.V00 = (float) vArray[0, 0];
            v.V01 = (float) vArray[0, 1];
            v.V02 = (float) vArray[0, 2];
            v.V10 = (float) vArray[1, 0];
            v.V11 = (float) vArray[1, 1];
            v.V12 = (float) vArray[1, 2];
            v.V20 = (float) vArray[2, 0];
            v.V21 = (float) vArray[2, 1];
            v.V22 = (float) vArray[2, 2];

            // build E Vector3
            e = new Vector3( );
            e.X = (float) eArray[0];
            e.Y = (float) eArray[1];
            e.Z = (float) eArray[2];
        }

        /// <summary>
        /// Calculate adjugate of the matrix, adj(A).
        /// </summary>
        /// 
        /// <returns>Returns adjugate of the matrix.</returns>
        /// 
        public Matrix3x3 Adjugate( )
        {
            Matrix3x3 m = new Matrix3x3( );

            m.V00 = V11 * V22 - V12 * V21;
            m.V01 = -( V01 * V22 - V02 * V21 );
            m.V02 = V01 * V12 - V02 * V11;

            m.V10 = -( V10 * V22 - V12 * V20 );
            m.V11 = V00 * V22 - V02 * V20;
            m.V12 = -( V00 * V12 - V02 * V10 );

            m.V20 = V10 * V21 - V11 * V20;
            m.V21 = -( V00 * V21 - V01 * V20 );
            m.V22 = V00 * V11 - V01 * V10;

            return m;
        }

        /// <summary>
        /// Multiply transposition of this matrix by itself, A<sup>T</sup>*A.
        /// </summary>
        /// 
        /// <returns>Returns a matrix which is the result of multiplying this matrix's transposition by itself.</returns>
        ///
        public Matrix3x3 MultiplyTransposeBySelf( )
        {
            Matrix3x3 m = new Matrix3x3( );

            m.V00 = V00 * V00 + V10 * V10 + V20 * V20;
            m.V10 = m.V01 = V00 * V01 + V10 * V11 + V20 * V21;
            m.V20 = m.V02 = V00 * V02 + V10 * V12 + V20 * V22;

            m.V11 = V01 * V01 + V11 * V11 + V21 * V21;
            m.V21 = m.V12 = V01 * V02 + V11 * V12 + V21 * V22;

            m.V22 = V02 * V02 + V12 * V12 + V22 * V22;

            return m;
        }

        /// <summary>
        /// Transpose the matrix, A<sup>T</sup>.
        /// </summary>
        /// 
        /// <returns>Return a matrix which equals to transposition of this matrix.</returns>
        /// 
        public Matrix3x3 Transpose( )
        {
            Matrix3x3 m = new Matrix3x3( );

            m.V00 = V00;
            m.V01 = V10;
            m.V02 = V20;

            m.V10 = V01;
            m.V11 = V11;
            m.V12 = V21;

            m.V20 = V02;
            m.V21 = V12;
            m.V22 = V22;

            return m;
        }

 public static SkeletonPoint MultiplyPositionMatrix(Matrix3x3 mat, SkeletonPoint p)
 {
     SkeletonPoint pos = new SkeletonPoint();
     pos.X = p.X * mat.V00 + p.Y * mat.V01 + p.Z * mat.V02;
     pos.Y = p.X * mat.V10 + p.Y * mat.V11 + p.Z * mat.V12;
     pos.Z = p.X * mat.V20 + p.Y * mat.V21 + p.Z * mat.V22;
     return pos;
 }

        /// <summary>
        /// Adds specified value to all components of the specified matrix.
        /// </summary>
        /// 
        /// <param name="matrix">Matrix to add value to.</param>
        /// <param name="value">Value to add to all components of the specified matrix.</param>
        /// 
        /// <returns>Returns new matrix with all components equal to corresponding components of the
        /// specified matrix increased by the specified value.</returns>
        /// 
        public static Matrix3x3 operator +( Matrix3x3 matrix, float value )
        {
            Matrix3x3 m = new Matrix3x3( );

            m.V00 = matrix.V00 + value;
            m.V01 = matrix.V01 + value;
            m.V02 = matrix.V02 + value;

            m.V10 = matrix.V10 + value;
            m.V11 = matrix.V11 + value;
            m.V12 = matrix.V12 + value;

            m.V20 = matrix.V20 + value;
            m.V21 = matrix.V21 + value;
            m.V22 = matrix.V22 + value;

            return m;
        }

        public void InverseTest( float v00, float v01, float v02, float v10, float v11, float v12, float v20, float v21, float v22 )
        {
            Matrix3x3 matrix = new Matrix3x3( );

            matrix.V00 = v00;
            matrix.V01 = v01;
            matrix.V02 = v02;

            matrix.V10 = v10;
            matrix.V11 = v11;
            matrix.V12 = v12;

            matrix.V20 = v20;
            matrix.V21 = v21;
            matrix.V22 = v22;

            Matrix3x3 inverse = matrix.Inverse( );
            Matrix3x3 identity = matrix * inverse;

            Assert.AreEqual<bool>( true, ApproximateEquals( identity, Matrix3x3.Identity ) );
        }