// Creating Unity Transformation matrix using 3x3 Rotation matrix and translation vector acquired from RigidTransform UnityEngine.Matrix4x4 AccordToUnityMatrix(UnityEngine.Matrix4x4 UnityM, Accord.Math.Matrix3x3 RotationM, Accord.Math.Vector3 Trans) { UnityM.m00 = RotationM.V00; UnityM.m10 = RotationM.V10; UnityM.m20 = RotationM.V20; UnityM.m01 = RotationM.V01; UnityM.m11 = RotationM.V11; UnityM.m21 = RotationM.V21; UnityM.m02 = RotationM.V02; UnityM.m12 = RotationM.V12; UnityM.m22 = RotationM.V22; UnityM.m03 = Trans.X; UnityM.m13 = Trans.Y; UnityM.m23 = Trans.Z; UnityM.m30 = 0; UnityM.m31 = 0; UnityM.m32 = 0; UnityM.m33 = 1; return(UnityM); }
/// <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; }
/// <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; }
/// <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 operator *( Matrix3x3 matrix, float factor ) { Matrix3x3 m = new Matrix3x3( ); m.V00 = matrix.V00 * factor; m.V01 = matrix.V01 * factor; m.V02 = matrix.V02 * factor; m.V10 = matrix.V10 * factor; m.V11 = matrix.V11 * factor; m.V12 = matrix.V12 * factor; m.V20 = matrix.V20 * factor; m.V21 = matrix.V21 * factor; m.V22 = matrix.V22 * factor; return m; }
/// <summary> /// Creates rotation matrix around Z axis. /// </summary> /// /// <param name="radians">Rotation angle around Z axis in radians.</param> /// /// <returns>Returns rotation matrix to rotate an object around Z axis.</returns> /// public static Matrix3x3 CreateRotationZ( float radians ) { Matrix3x3 m = new Matrix3x3( ); float cos = (float) System.Math.Cos( radians ); float sin = (float) System.Math.Sin( radians ); m.V00 = m.V11 = cos; m.V01 = -sin; m.V10 = sin; m.V22 = 1; return m; }
/// <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> /// 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 Matrix3x3 operator +( Matrix3x3 matrix1, Matrix3x3 matrix2 ) { Matrix3x3 m = new Matrix3x3( ); m.V00 = matrix1.V00 + matrix2.V00; m.V01 = matrix1.V01 + matrix2.V01; m.V02 = matrix1.V02 + matrix2.V02; m.V10 = matrix1.V10 + matrix2.V10; m.V11 = matrix1.V11 + matrix2.V11; m.V12 = matrix1.V12 + matrix2.V12; m.V20 = matrix1.V20 + matrix2.V20; m.V21 = matrix1.V21 + matrix2.V21; m.V22 = matrix1.V22 + matrix2.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; }
/// <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 Matrix3x3 operator *( Matrix3x3 matrix1, Matrix3x3 matrix2 ) { Matrix3x3 m = new Matrix3x3( ); m.V00 = matrix1.V00 * matrix2.V00 + matrix1.V01 * matrix2.V10 + matrix1.V02 * matrix2.V20; m.V01 = matrix1.V00 * matrix2.V01 + matrix1.V01 * matrix2.V11 + matrix1.V02 * matrix2.V21; m.V02 = matrix1.V00 * matrix2.V02 + matrix1.V01 * matrix2.V12 + matrix1.V02 * matrix2.V22; m.V10 = matrix1.V10 * matrix2.V00 + matrix1.V11 * matrix2.V10 + matrix1.V12 * matrix2.V20; m.V11 = matrix1.V10 * matrix2.V01 + matrix1.V11 * matrix2.V11 + matrix1.V12 * matrix2.V21; m.V12 = matrix1.V10 * matrix2.V02 + matrix1.V11 * matrix2.V12 + matrix1.V12 * matrix2.V22; m.V20 = matrix1.V20 * matrix2.V00 + matrix1.V21 * matrix2.V10 + matrix1.V22 * matrix2.V20; m.V21 = matrix1.V20 * matrix2.V01 + matrix1.V21 * matrix2.V11 + matrix1.V22 * matrix2.V21; m.V22 = matrix1.V20 * matrix2.V02 + matrix1.V21 * matrix2.V12 + matrix1.V22 * matrix2.V22; 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 Matrix3x3 Multiply( Matrix3x3 matrix1, Matrix3x3 matrix2 ) { return matrix1 * matrix2; }
/// <summary> /// Creates a diagonal matrix using the specified vector as diagonal elements. /// </summary> /// /// <param name="vector">Vector to use for diagonal elements of the matrix.</param> /// /// <returns>Returns a diagonal matrix.</returns> /// public static Matrix3x3 CreateDiagonal( Vector3 vector ) { Matrix3x3 m = new Matrix3x3( ); m.V00 = vector.X; m.V11 = vector.Y; m.V22 = vector.Z; return m; }
/// <summary> /// Creates a matrix from 3 columns specified as vectors. /// </summary> /// /// <param name="column0">First column of the matrix to create.</param> /// <param name="column1">Second column of the matrix to create.</param> /// <param name="column2">Third column of the matrix to create.</param> /// /// <returns>Returns a matrix from specified columns.</returns> /// public static Matrix3x3 CreateFromColumns( Vector3 column0, Vector3 column1, Vector3 column2 ) { Matrix3x3 m = new Matrix3x3( ); m.V00 = column0.X; m.V10 = column0.Y; m.V20 = column0.Z; m.V01 = column1.X; m.V11 = column1.Y; m.V21 = column1.Z; m.V02 = column2.X; m.V12 = column2.Y; m.V22 = column2.Z; return m; }
/// <summary> /// Creates a matrix from 3 rows specified as vectors. /// </summary> /// /// <param name="row0">First row of the matrix to create.</param> /// <param name="row1">Second row of the matrix to create.</param> /// <param name="row2">Third row of the matrix to create.</param> /// /// <returns>Returns a matrix from specified rows.</returns> /// public static Matrix3x3 CreateFromRows( Vector3 row0, Vector3 row1, Vector3 row2 ) { Matrix3x3 m = new Matrix3x3( ); m.V00 = row0.X; m.V01 = row0.Y; m.V02 = row0.Z; m.V10 = row1.X; m.V11 = row1.Y; m.V12 = row1.Z; m.V20 = row2.X; m.V21 = row2.Y; m.V22 = row2.Z; return m; }
/// <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 Add( Matrix3x3 matrix, float value ) { return matrix + value; }
/// <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 Matrix3x3 Add( Matrix3x3 matrix1, Matrix3x3 matrix2 ) { return matrix1 + matrix2; }
/// <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 ); }
/// <summary> /// Subtracts corresponding components of two matrices. /// </summary> /// /// <param name="matrix1">The matrix to subtract from.</param> /// <param name="matrix2">The matrix to subtract from the first matrix.</param> /// /// <returns>Returns a matrix which components are equal to difference of corresponding /// components of the two specified matrices.</returns> /// public static Matrix3x3 operator -( Matrix3x3 matrix1, Matrix3x3 matrix2 ) { Matrix3x3 m = new Matrix3x3( ); m.V00 = matrix1.V00 - matrix2.V00; m.V01 = matrix1.V01 - matrix2.V01; m.V02 = matrix1.V02 - matrix2.V02; m.V10 = matrix1.V10 - matrix2.V10; m.V11 = matrix1.V11 - matrix2.V11; m.V12 = matrix1.V12 - matrix2.V12; m.V20 = matrix1.V20 - matrix2.V20; m.V21 = matrix1.V21 - matrix2.V21; m.V22 = matrix1.V22 - matrix2.V22; 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> /// Subtracts corresponding components of two matrices. /// </summary> /// /// <param name="matrix1">The matrix to subtract from.</param> /// <param name="matrix2">The matrix to subtract from the first matrix.</param> /// /// <returns>Returns a matrix which components are equal to difference of corresponding /// components of the two specified matrices.</returns> /// public static Matrix3x3 Subtract( Matrix3x3 matrix1, Matrix3x3 matrix2 ) { return matrix1 - matrix2; }
/// <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> /// Multiplies specified matrix by the specified vector. /// </summary> /// /// <param name="matrix">Matrix to multiply by vector.</param> /// <param name="vector">Vector to multiply matrix by.</param> /// /// <returns>Returns new vector which is the result of multiplication of the specified matrix /// by the specified vector.</returns> /// public static Vector3 Multiply( Matrix3x3 matrix, Vector3 vector ) { return matrix * vector; }
/// <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; }
// Select alternate pose estimation private void alternatePoseButton_Click(object sender, EventArgs e) { rotationMatrix = alternateRotationMatrix; translationVector = alternateTranslationVector; UpdateEstimationInformation(); pictureBox.Invalidate(); }