Robot

Dependencies:   mbed QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Matrix.cpp Source File

Matrix.cpp

00001 //
00002 // Created by m007 on 26/06/2020.
00003 //
00004 
00005 #include "Matrix.h"
00006 
00007 template<class T>
00008 Matrix<T>::Matrix(){
00009         this->rows = 3;
00010         this->columns = 3;
00011         n = new T *[this->rows];
00012         for (int i = 0; i <this->rows ; i++){
00013             n[i] = new T[this->columns];
00014             for (int j = 0; j < this->columns; j++){
00015                 n[i][j] = 0;
00016             }
00017         }
00018 }
00019 
00020 template<class T>
00021 Matrix<T>::Matrix(int rows,int columns){
00022         objectCount++;
00023         this->rows = rows;
00024         this->columns = columns;
00025         n = new T *[this->rows];
00026         for (int i = 0; i <this->rows ; i++){
00027             n[i] = new T[this->columns];
00028             for (int j = 0; j < this->columns; j++){
00029                 n[i][j] = 0;
00030             }
00031         }
00032 }
00033 
00034 template<class T>
00035 Matrix<T>::~Matrix() {
00036     for(int i = 0; i < this->rows; ++i) {
00037         delete[] n[i];
00038     }
00039     //Free the array of pointers
00040     delete[] n;
00041 }
00042 
00043 template<class T>
00044 T Matrix<T>::Determinant(){
00045     T det = 0;
00046     if(this->rows == this->columns) {
00047         switch (this->rows) {
00048             case 2: {
00049                 det = n[0][0] * n[1][1] - n[0][1] * n[1][0];
00050                 break;
00051             }
00052             case 3: {
00053                 T a = n[0][0];
00054                 T b = n[0][1];
00055                 T c = n[0][2];
00056                 T d = n[1][0];
00057                 T e = n[1][1];
00058                 T f = n[1][2];
00059                 T g = n[2][0];
00060                 T h = n[2][1];
00061                 T i = n[2][2];
00062                 det = (a * e * i + b * f * g + c * d * h);
00063                 det = det - a * f * h;
00064                 det = det - b * d * i;
00065                 det = det - c * e * g;
00066                 break;
00067             }
00068             case 4: {
00069                 Matrix<T> *temp[4];
00070                 for (int i = 0; i < 4; i++)
00071                     temp[i] = new Matrix<T>(3, 3);
00072                 for (int k = 0; k < 4; k++) {
00073                     for (int i = 1; i < 4; i++) {
00074                         int j1 = 0;
00075                         for (int j = 0; j < 4; j++) {
00076                             if (k == j)
00077                                 continue;
00078                             temp[k]->n[i - 1][j1++] = this->n[i][j];
00079                         }
00080                     }
00081                 }
00082                 det = this->n[0][0] * temp[0]->Determinant() - this->n[0][1] * temp[1]->Determinant() +
00083                       this->n[0][2] * temp[2]->Determinant()
00084                       - this->n[0][3] * temp[3]->Determinant();
00085                 break;
00086             }
00087             case 5: {
00088                 Matrix<T> *temp[5];
00089                 for (int i = 0; i < 5; i++)
00090                     temp[i] = new Matrix<T>(4, 4);
00091                 for (int k = 0; k < 5; k++) {
00092                     for (int i = 1; i < 5; i++) {
00093                         int j1 = 0;
00094                         for (int j = 0; j < 5; j++) {
00095                             if (k == j)
00096                                 continue;
00097                             temp[k]->n[i - 1][j1++] = this->n[i][j];
00098                         }
00099                     }
00100                 }
00101                 det = this->n[0][0] * temp[0]->Determinant() - this->n[0][1] * temp[1]->Determinant()
00102                       + this->n[0][2] * temp[2]->Determinant() - this->n[0][3] * temp[3]->Determinant()
00103                       + this->n[0][4] * temp[4]->Determinant();
00104                 break;
00105             }
00106             case 6:
00107             case 7:
00108             case 8:
00109             case 9:
00110             case 10:
00111             case 11:
00112             case 12:
00113             default: {
00114                 Matrix **temp = new Matrix *[rows];
00115                 for (int i = 0; i < rows; i++)
00116                     temp[i] = new Matrix(rows - 1, rows - 1);
00117                 for (int k = 0; k < rows; k++) {
00118                     for (int i = 1; i < rows; i++) {
00119                         int j1 = 0;
00120                         for (int j = 0; j < rows; j++) {
00121                             if (k == j)
00122                                 continue;
00123                             temp[k]->n[i - 1][j1++] = this->n[i][j];
00124                         }
00125                     }
00126                 }
00127                 for (int k = 0; k < rows; k++) {
00128                     if ((k % 2) == 0) {
00129                         det = det + (this->n[0][k] * temp[k]->Determinant());
00130                     } else {
00131                         det = det - (this->n[0][k] * temp[k]->Determinant());
00132                     }
00133                 }
00134                 for (int i = 0; i < rows; i++)
00135                     delete temp[i];
00136                 delete[] temp;
00137                break;
00138             }
00139         }
00140     }
00141     return det;
00142 }
00143 
00144 template<class T>
00145 Matrix<T> Matrix<T>::Cofactor(){
00146     Matrix<T> cofactor(this->rows, this->columns);
00147 
00148     if (this->rows != this->columns)
00149         return cofactor;
00150     if (this->rows < 2)
00151         return cofactor;
00152     else if (this->rows == 2){
00153         for (int i = 0; i < this->rows; i++) {
00154             for (int j = 0; j < this->columns; j++){
00155                 cofactor(i, j) =this->n[((this->rows-1)-i)][((this->columns-1)-j)];
00156                 if((i+j)%2 != 0) {
00157                     cofactor(i, j) = -1*cofactor(i, j);
00158                 }
00159             }
00160         }
00161         return cofactor;
00162     }
00163     else if (this->rows >= 3){
00164         int DIM = this->rows;
00165         Matrix<T> ***temp = new Matrix<T>**[DIM];
00166         for (int i = 0; i < DIM; i++)
00167             temp[i] = new Matrix<T>*[DIM];
00168         for (int i = 0; i < DIM; i++)
00169             for (int j = 0; j < DIM; j++)
00170                 temp[i][j] = new Matrix<T>(DIM - 1, DIM - 1);
00171         for (int k1 = 0; k1 < DIM; k1++){
00172             for (int k2 = 0; k2 < DIM; k2++){
00173                 int i1 = 0;
00174                 for (int i = 0; i < DIM; i++){
00175                     int j1 = 0;
00176                     for (int j = 0; j < DIM; j++){
00177                         if (k1 == i || k2 == j)
00178                             continue;
00179                         temp[k1][k2]->n[i1][j1++]
00180                                 = this->n[i][j];
00181                     }
00182                     if (k1 != i)
00183                         i1++;
00184                 }
00185             }
00186         }
00187         bool flagPositive = true;
00188         for (int k1 = 0; k1 < DIM; k1++){
00189             flagPositive = ((k1 % 2) == 0);
00190             for (int k2 = 0; k2 < DIM; k2++){
00191                 if (flagPositive == true){
00192                     cofactor.n[k1][k2]
00193                             = temp[k1][k2]->Determinant();
00194                     flagPositive = false;
00195                 }
00196                 else{
00197                     cofactor.n[k1][k2]
00198                             = -temp[k1][k2]->Determinant();
00199                     flagPositive = true;
00200                 }
00201             }
00202         }
00203         for (int i = 0; i < DIM; i++)
00204             for (int j = 0; j < DIM; j++)
00205                 delete temp[i][j];
00206         for (int i = 0; i < DIM; i++)
00207             delete[] temp[i];
00208         delete[] temp;
00209     }
00210     return cofactor;
00211 }
00212 template<class T>
00213 Matrix<T> Matrix<T>::Inverse(){
00214     Matrix<T> cofactor(this->rows, this->columns);
00215     Matrix<T> inv(this->rows, this->columns);
00216     if (this->rows != this->columns)
00217         return inv;
00218 
00219     T det = this->Determinant();
00220     cofactor = this->Cofactor();
00221     if(det != 0) {
00222         for (int i = 0; i < this->rows; i++) {
00223             for (int j = 0; j < this->columns; j++) {
00224                 inv.n[j][i] = cofactor.n[i][j] / det;
00225             }
00226         }
00227     }
00228     return inv;
00229 }
00230 
00231 template<class T>
00232 Matrix<T>  Matrix<T>::PseudoInverse(){
00233     Matrix<T> trans(this->columns, this->rows);
00234     Matrix<T> A(this->rows, this->columns);
00235     A = *this;
00236     trans = Transpose();
00237     Matrix<T> result = Matrix<T>(trans.rows, trans.columns);
00238     if((trans*A).Determinant()!=0){
00239         result = ((trans * A).Inverse())*trans;
00240     }else if((A*trans).Determinant()!=0){
00241         result = trans *((A*trans).Inverse());
00242     }else{
00243         printf("Error Matrix PseudoInverse\n");
00244     }
00245     return result;
00246 
00247 }
00248 
00249 template<class T>
00250 Matrix<T> Matrix<T>::Transpose(){
00251     Matrix<T> trans(this->columns, this->rows);
00252         for (int i = 0; i < this->columns; i++) {
00253             for (int j = 0; j < this->rows; j++) {
00254                 trans.n[i][j] = this->n[j][i];
00255             }
00256         }
00257 
00258     return trans;
00259 }
00260 template<class T>
00261 void Matrix<T>::PrintResult(){
00262     for (int x = 0; x<this->rows; x++) {
00263         for (int y = 0; y < this->columns; y++) {
00264                 printf(" %f ", n[x][y]);
00265         }
00266         printf("\n");
00267     }
00268     printf("\n");
00269 }
00270 template<class T>
00271 void  Matrix<T>::Reconfigurate(int rows,int columns){
00272     Matrix<T>::~Matrix();
00273     this->rows = rows;
00274     this->columns = columns;
00275     n = new T *[this->rows];
00276     for (int i = 0; i <this->rows ; i++){
00277         n[i] = new T[this->columns];
00278         for (int j = 0; j < this->columns; j++){
00279             n[i][j] = 0;
00280         }
00281     }
00282 }
00283 template<class T>
00284 Matrix<T>&  Matrix<T>::operator=(const Matrix &other ){
00285         if(this->columns<=other.columns && this->rows<=other.rows){
00286             for(int i=0;i<this->rows;i++){
00287                 for(int j=0;j<this->columns;j++){
00288                     this->n[i][j]=other.n[i][j];
00289                     }
00290                 }
00291         }else{
00292             printf("ERROR: Matrix A = Matrix B. B has more dimensions than A \n");
00293         }
00294          return (*this);
00295 }
00296 
00297 template<class T>
00298 Matrix<T>& Matrix<T>::operator=(const T param[][2]){
00299     for(int i=0;i<this->rows;i++){
00300         for(int j=0;j<this->columns;j++){
00301             this->n[i][j]=param[i][j];
00302         }
00303     }
00304     return (*this);
00305 }
00306 
00307 template<class T>
00308 Matrix<T>& Matrix<T>::operator=(const T param[][3]){
00309     for(int i=0;i<this->rows;i++){
00310         for(int j=0;j<this->columns;j++){
00311             this->n[i][j]=param[i][j];
00312         }
00313     }
00314     return (*this);
00315 }
00316 
00317 template<class T>
00318 Matrix<T>& Matrix<T>::operator=(const T param[][4]){
00319     for(int i=0;i<this->rows;i++){
00320         for(int j=0;j<this->columns;j++){
00321             this->n[i][j]=param[i][j];
00322         }
00323     }
00324     return (*this);
00325 }
00326 
00327 
00328 template<class T>
00329 Matrix<T>  Matrix<T>::operator+(const Matrix &other){
00330     Matrix<T> result = Matrix<T>(rows, other.columns);
00331     if (this->rows == other.columns && this->columns == other.rows) {
00332         for (int i = 0; i < this->rows; i++) {
00333             for (int j = 0; j < this->columns; j++) {
00334                 result.n[i][j] = other.n[i][j] + this->n[i][j];
00335             }
00336         }
00337     }
00338         return result;
00339 }
00340 
00341 template<class T>
00342 Matrix<T>  Matrix<T>::operator*(const Matrix& other) {
00343         Matrix<T> result = Matrix<T>(this->rows, other.columns);
00344         if (this->columns == other.rows) {
00345             for (int i = 0; i < this->rows; i++) {
00346                 for (int j = 0; j < this->columns; j++) {
00347                     for (int k = 0; k < this->columns; k++) {
00348                         result.n[i][j] += this->n[i][k]*other.n[k][j];
00349                     }
00350                 }
00351             }
00352         }
00353         return result;
00354 }
00355 template<class T>
00356 Vector<T> Matrix<T>::operator*(Vector<T>& other){
00357     Vector<T> result = Vector<T>(other.getDimension());
00358     for (int x=0;x<other.getDimension();x++){
00359         result(x)= (this->n[x][0]*other(0)+this->n[x][1]*other(1)+this->n[x][2]*other(2));
00360         if( (result (x)>-0.01)&&(result (x))<0.01){
00361             result (x) = 0;
00362         }
00363     }
00364     return result;
00365 
00366 }
00367 template<class T>
00368 T& Matrix<T>::operator()(int row, int col) {
00369         return n[row][col];
00370 }
00371 
00372 template<class T>
00373 int Matrix<T>::objectCount = 0;
00374 
00375 template class Matrix<float>;
00376 template class Matrix<int>;
00377 template class Vector<float>;
00378 template class Vector<int>;