MatrixMath.cpp

00001 /**
00002  * @brief  version 0.9
00003  * @file   MatrixMath.cpp
00004  * @author Ernesto Palacios
00005  *
00006  * Created on 15 de septiembre de 2011, 09:44 AM.
00007  *
00008  * Develop Under  GPL v3.0 License
00009  * http://www.gnu.org/licenses/gpl-3.0.html
00010  */
00011 
00012 #include "mbed.h"
00013 #include "MatrixMath.h"
00014 
00015 ///Transpose matrix
00016 Matrix MatrixMath::Transpose(const Matrix &Mat)
00017 {
00018     Matrix result(Mat._nCols, Mat._nRows); //Transpose Matrix
00019 
00020     for (int i = 0; i < result._nRows; i++)
00021         for (int j = 0; j < result._nCols; j++)
00022             result._matrix[i][j] = Mat._matrix[j][i];
00023 
00024     return result;
00025 }
00026 
00027 ///Transpose matrix
00028 Matrix MatrixMath::Matrixcross(const float px, const float py, const float pz)
00029 {
00030     Matrix result(3,3); //Transpose Matrix
00031     result(1,2) = -pz;
00032     result(1,3) =  py;
00033     result(2,1) =  pz;
00034     result(2,3) = -px;
00035     result(3,1) = -py;
00036     result(3,2) =  px;
00037     return result;
00038 }
00039 Matrix MatrixMath::Vector2mat(const Vector3 vec)
00040 {
00041     Matrix result(3,1); //Transpose Matrix
00042     result(1,1) = vec.x;
00043     result(2,1) = vec.y;
00044     result(3,1) = vec.z;
00045     return result;
00046 }
00047 Matrix MatrixMath::Inv(const Matrix &Mat)
00048 {
00049     if (Mat._nRows == Mat._nCols)
00050     {
00051         if (Mat._nRows == 2) // 2x2 Matrices
00052         {
00053             float det = MatrixMath::det(Mat);
00054             if (det != 0)
00055             {
00056                 Matrix Inv(2, 2);
00057                 Inv._matrix[0][0] = Mat._matrix[1][1];
00058                 Inv._matrix[1][0] = -Mat._matrix[1][0];
00059                 Inv._matrix[0][1] = -Mat._matrix[0][1];
00060                 Inv._matrix[1][1] = Mat._matrix[0][0];
00061 
00062                 Inv *= 1 / det;
00063 
00064                 return Inv;
00065             }
00066             else
00067             {
00068                 printf("\n\nWANRING: same matrix returned");
00069                 printf("\nSingular Matrix, cannot perform Invert @matrix\n ");
00070                 Mat.print();
00071                 printf("\n  _____________\n");
00072 
00073                 return Mat;
00074             }
00075         }
00076         else
00077         { // nxn Matrices
00078 
00079             float det = MatrixMath::det(Mat);
00080             if (det != 0)
00081             {
00082                 Matrix Inv(Mat); //
00083                 Matrix SubMat;
00084 
00085                 // Matrix of Co-factors
00086                 for (int i = 0; i < Mat._nRows; i++)
00087                     for (int j = 0; j < Mat._nCols; j++)
00088                     {
00089                         SubMat = Mat;
00090 
00091                         Matrix::DeleteRow(SubMat, i + 1);
00092                         Matrix::DeleteCol(SubMat, j + 1);
00093 
00094                         if ((i + j) % 2 == 0)
00095                             Inv._matrix[i][j] = MatrixMath::det(SubMat);
00096                         else
00097                             Inv._matrix[i][j] = -MatrixMath::det(SubMat);
00098                     }
00099 
00100                 // Adjugate Matrix
00101                 Inv = MatrixMath::Transpose(Inv);
00102 
00103                 // Inverse Matrix
00104                 Inv = 1 / det * Inv;
00105 
00106                 return Inv;
00107             }
00108             else
00109             {
00110                 printf("\n\nWANRING: same matrix returned");
00111                 printf("\nSingular Matrix, cannot perform Invert @matrix\n");
00112                 Mat.print();
00113                 printf("\n  _____________\n");
00114 
00115                 return Mat;
00116             }
00117         }
00118     }
00119     else
00120     {
00121         printf("\n\nERROR:\nMust be square Matrix @ MatrixMath::Determinant\n");
00122     }
00123 }
00124 
00125 Matrix MatrixMath::Eye(int Rows)
00126 {
00127     Matrix Identity(Rows, Rows); //Square Matrix
00128 
00129     for (int i = 0; i < Rows; i++)
00130         Identity._matrix[i][i] = 1;
00131 
00132     return Identity;
00133 }
00134 
00135 // Very Versitle Function. Accepts two Vector Matrices of any type:
00136 // Vector types may be: [n,1] dot [n,1]
00137 //                      [n,1] dot [1,n]  always same
00138 //                      [1,n] dot [n,1]    'depth'
00139 //                      [1,n] dot [1,n]
00140 float MatrixMath::dot(const Matrix &leftM, const Matrix &rightM)
00141 {
00142     if (leftM.isVector() && rightM.isVector())
00143     {
00144         if (leftM._nRows == 1)
00145         {
00146             if (rightM._nRows == 1)
00147             {
00148                 if (leftM._nCols == rightM._nCols)
00149                 {
00150                     // Calculate ( 1,n )( 1,n )
00151                     float dotP;
00152                     Matrix Cross;
00153 
00154                     Cross = leftM * MatrixMath::Transpose(rightM);
00155                     dotP = Cross.sum();
00156 
00157                     return dotP;
00158                 }
00159                 else
00160                 {
00161                     printf("\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n");
00162                 }
00163             }
00164             else
00165             {
00166                 if (leftM._nCols == rightM._nRows)
00167                 {
00168                     // Calculate (1, n)( n, 1 )
00169                     float dotP;
00170                     Matrix Cross;
00171 
00172                     Cross = leftM * rightM;
00173                     dotP = Cross.sum();
00174 
00175                     return dotP;
00176                 }
00177                 else
00178                 {
00179                     printf("\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n");
00180                 }
00181             }
00182         }
00183         else
00184         {
00185             if (rightM._nRows == 1)
00186             {
00187                 if (leftM._nRows == rightM._nCols)
00188                 {
00189                     // Calculate ( n,1 )( 1,n )
00190                     float dotP;
00191                     Matrix Cross;
00192 
00193                     Cross = MatrixMath::Transpose(leftM) * MatrixMath::Transpose(rightM);
00194                     dotP = Cross.sum();
00195 
00196                     return dotP;
00197                 }
00198                 else
00199                 {
00200                     printf("\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n");
00201                 }
00202             }
00203             else
00204             {
00205                 if (leftM._nRows == rightM._nRows)
00206                 {
00207                     // Calculate (n, 1)( n, 1 )
00208                     float dotP;
00209                     Matrix Cross;
00210 
00211                     Cross = MatrixMath::Transpose(leftM) * rightM;
00212                     dotP = Cross.sum();
00213 
00214                     return dotP;
00215                 }
00216                 else
00217                 {
00218                     printf("\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n");
00219                 }
00220             }
00221         }
00222     }
00223     else
00224     {
00225         printf("\n\nERROR:\n Matrix is not a Vector @ MatrixMath::dot()\n");
00226     }
00227 }
00228 
00229 float MatrixMath::det(const Matrix &Mat)
00230 {
00231     if (Mat._nRows == Mat._nCols)
00232     {
00233 
00234         if (Mat._nRows == 2) // 2x2 Matrix
00235         {
00236             float det;
00237             det = Mat._matrix[0][0] * Mat._matrix[1][1] -
00238                   Mat._matrix[1][0] * Mat._matrix[0][1];
00239             return det;
00240         }
00241         else if (Mat._nRows == 3) // 3x3 Matrix
00242         {
00243             float det;
00244             MatrixMath dummy; //For Private Method.
00245 
00246             det = dummy.Det3x3(Mat);
00247             return det;
00248         }
00249         else
00250         {
00251 
00252             float part1 = 0;
00253             float part2 = 0;
00254 
00255             //Find +/- on First Row
00256             for (int i = 0; i < Mat._nCols; i++)
00257             {
00258                 Matrix reduced(Mat);           // Copy Original Matrix
00259                 Matrix::DeleteRow(reduced, 1); // Delete First Row
00260 
00261                 if (i % 2 == 0) //Even Rows
00262                 {
00263 
00264                     Matrix::DeleteCol(reduced, i + 1);
00265                     part1 += Mat._matrix[0][i] * MatrixMath::det(reduced);
00266                 }
00267                 else // Odd Rows
00268                 {
00269                     Matrix::DeleteCol(reduced, i + 1);
00270                     part2 += Mat._matrix[0][i] * MatrixMath::det(reduced);
00271                 }
00272             }
00273             return part1 - part2;
00274         }
00275     }
00276     else
00277     {
00278         printf("\n\nERROR:\nMatrix must be square Matrix @ MatrixMath::det");
00279     }
00280 }
00281 
00282 Matrix MatrixMath::kron(const Matrix &Mat_A, const Matrix &Mat_B)
00283 {
00284     Matrix result(Mat_A._nRows * Mat_B._nRows, Mat_A._nCols * Mat_B._nCols);
00285     for (unsigned int row_a = 0; row_a < Mat_A._nRows; row_a++)
00286     {
00287         for (unsigned int col_a = 0; col_a < Mat_A._nCols; col_a++)
00288         {
00289             Matrix block = Mat_A._matrix[row_a][col_a]*Mat_B;
00290 
00291             for (unsigned int row_b = 0; row_b < Mat_B._nRows; row_b++)
00292             {
00293                 for (unsigned int col_b = 0; col_b < Mat_B._nCols; col_b++)
00294                 {
00295                     result._matrix[row_b + (row_a * Mat_B._nRows)][col_b + (col_a * Mat_B._nCols)] = block._matrix[row_b][col_b];
00296                 }
00297             }
00298         }
00299     }
00300     return result;
00301 }
00302 
00303 /************************************/
00304 
00305 //Private Functions
00306 
00307 /**@brief
00308  * Expands the Matrix adding first and second column to the Matrix then
00309  * performs the Algorithm.
00310  * @param Mat
00311  * @return Determinant
00312  */
00313 float MatrixMath::Det3x3(const Matrix &Mat)
00314 {
00315     Matrix D(Mat); //Copy Initial matrix
00316 
00317     Matrix::AddCol(D, Matrix::ExportCol(Mat, 1), 4); //Repeat First Column
00318     Matrix::AddCol(D, Matrix::ExportCol(Mat, 2), 5); //Repeat Second Column
00319 
00320     float det = 0;
00321     for (int i = 0; i < 3; i++)
00322         det += D._matrix[0][i] * D._matrix[1][1 + i] * D._matrix[2][2 + i] - D._matrix[0][2 + i] * D._matrix[1][1 + i] * D._matrix[2][i];
00323 
00324     return det;
00325 }