Yo Fleyt / Mbed 2 deprecated roboticLab_withclass_3_July

test morning

Dependencies:   ISR_Mini-explorer mbed

Fork of roboticLab_withclass_3_July by Georgios Tsamis

myMatrix.cpp@14:696187e74411, 2017-07-12 (annotated)

Committer:
Ludwigfr
Date:
Wed Jul 12 18:08:07 2017 +0000
Revision:
14:696187e74411
Parent:
11:b91fe0ed4fed 
lol

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ludwigfr 11:b91fe0ed4fed 1 //#include "../include/myMatrix.hpp"
Ludwigfr 8:072a76960e27 2 #include "myMatrix.hpp"
Ludwigfr 8:072a76960e27 3
Ludwigfr 11:b91fe0ed4fed 4 //using namespace std;
Ludwigfr 8:072a76960e27 5
Ludwigfr 8:072a76960e27 6 myMatrix::myMatrix(int inNbRow,int inNbColumn){
Ludwigfr 9:1cc27f33d3e1 7 int i;
Ludwigfr 8:072a76960e27 8 this->nbRow=inNbRow;
Ludwigfr 8:072a76960e27 9 this->nbColumn=inNbColumn;
Ludwigfr 8:072a76960e27 10 //initialise data
Ludwigfr 8:072a76960e27 11
Ludwigfr 8:072a76960e27 12 //memory
Ludwigfr 10:d0109d7cbe7c 13 this->data= new float*[this->nbRow];
Ludwigfr 10:d0109d7cbe7c 14 for(i = 0; i < this->nbRow; ++i)
Ludwigfr 10:d0109d7cbe7c 15 this->data[i] = new float[this->nbColumn];
Ludwigfr 8:072a76960e27 16
Ludwigfr 8:072a76960e27 17 //fill with 0
Ludwigfr 9:1cc27f33d3e1 18 this->fillWithZeroes();
Ludwigfr 9:1cc27f33d3e1 19 }
Ludwigfr 8:072a76960e27 20
Ludwigfr 14:696187e74411 21 /*
Ludwigfr 14:696187e74411 22 myMatrix::~myMatrix()
Ludwigfr 14:696187e74411 23 {
Ludwigfr 14:696187e74411 24 //for (int i = 0; i < this->nbRow; i++) {
Ludwigfr 14:696187e74411 25 // delete [] (this->data[i]);
Ludwigfr 14:696187e74411 26 //}
Ludwigfr 14:696187e74411 27 //delete [] (this->data);
Ludwigfr 14:696187e74411 28
Ludwigfr 14:696187e74411 29 }
Ludwigfr 14:696187e74411 30 */
Ludwigfr 9:1cc27f33d3e1 31 void myMatrix::fillWithZeroes(){
Ludwigfr 14:696187e74411 32 for(int i = 0; i < mat1.nbRow; ++i) {
Ludwigfr 14:696187e74411 33 for(int j = 0; j < mat1.nbColumn; ++j) {
Ludwigfr 8:072a76960e27 34 this->data[i][j] = 0;
Ludwigfr 8:072a76960e27 35 }
Ludwigfr 8:072a76960e27 36 }
Ludwigfr 8:072a76960e27 37 }
Ludwigfr 8:072a76960e27 38
Ludwigfr 10:d0109d7cbe7c 39 void myMatrix::set(int i,int j, float value){
Ludwigfr 10:d0109d7cbe7c 40 this->data[i][j]=value;
Ludwigfr 10:d0109d7cbe7c 41 }
Ludwigfr 10:d0109d7cbe7c 42
Ludwigfr 10:d0109d7cbe7c 43 float myMatrix::get(int i,int j){
Ludwigfr 10:d0109d7cbe7c 44 return this->data[i][j];
Ludwigfr 10:d0109d7cbe7c 45 }
Ludwigfr 8:072a76960e27 46
Ludwigfr 9:1cc27f33d3e1 47 void myMatrix::fillByCopy(myMatrix mat1){
Ludwigfr 9:1cc27f33d3e1 48 for(int i = 0; i < mat1.nbRow; ++i) {
Ludwigfr 9:1cc27f33d3e1 49 for(int j = 0; j < mat1.nbColumn; ++j) {
Ludwigfr 9:1cc27f33d3e1 50 this->data[i][j] = mat1.data[i][j];
Ludwigfr 9:1cc27f33d3e1 51 }
Ludwigfr 9:1cc27f33d3e1 52 }
Ludwigfr 9:1cc27f33d3e1 53 }
Ludwigfr 9:1cc27f33d3e1 54
Ludwigfr 8:072a76960e27 55 //assume the dimensions are correct
Ludwigfr 8:072a76960e27 56 void myMatrix::fillWithTranspose(myMatrix mat1){
Ludwigfr 8:072a76960e27 57 for(int i = 0; i < mat1.nbRow; ++i) {
Ludwigfr 8:072a76960e27 58 for(int j = 0; j < mat1.nbColumn; ++j) {
Ludwigfr 8:072a76960e27 59 this->data[j][i] = mat1.data[i][j];
Ludwigfr 8:072a76960e27 60 }
Ludwigfr 8:072a76960e27 61 }
Ludwigfr 8:072a76960e27 62 }
Ludwigfr 8:072a76960e27 63
Ludwigfr 8:072a76960e27 64 //assume the dimensions are correct, and that this is fill with 0
Ludwigfr 8:072a76960e27 65 void myMatrix::fillByMultiplication(myMatrix mat1,myMatrix mat2){
Ludwigfr 8:072a76960e27 66 // Multiplying matrix firstMatrix and secondMatrix and storing in array mult.
Ludwigfr 8:072a76960e27 67 float sum=0;
Ludwigfr 8:072a76960e27 68 for(int i = 0; i < mat1.nbRow; ++i) {
Ludwigfr 8:072a76960e27 69 for(int j = 0; j < mat2.nbColumn; ++j) {
Ludwigfr 8:072a76960e27 70 for(int k=0; k < mat1.nbColumn; ++k) {
Ludwigfr 8:072a76960e27 71 sum += mat1.data[i][k] * mat2.data[k][j];
Ludwigfr 8:072a76960e27 72 }
Ludwigfr 8:072a76960e27 73 this->data[i][j] = sum;
Ludwigfr 8:072a76960e27 74 sum = 0;
Ludwigfr 8:072a76960e27 75 }
Ludwigfr 8:072a76960e27 76 }
Ludwigfr 8:072a76960e27 77 }
Ludwigfr 8:072a76960e27 78
Ludwigfr 8:072a76960e27 79 void myMatrix::addition(myMatrix mat1){
Ludwigfr 8:072a76960e27 80 for(int i = 0; i < mat1.nbRow; ++i) {
Ludwigfr 8:072a76960e27 81 for(int j = 0; j < mat1.nbColumn; ++j) {
Ludwigfr 8:072a76960e27 82 this->data[i][j] += mat1.data[i][j];
Ludwigfr 8:072a76960e27 83 }
Ludwigfr 8:072a76960e27 84 }
Ludwigfr 8:072a76960e27 85 }
Ludwigfr 8:072a76960e27 86
Ludwigfr 8:072a76960e27 87 void myMatrix::subtraction(myMatrix mat1){
Ludwigfr 8:072a76960e27 88 for(int i = 0; i < mat1.nbRow; ++i) {
Ludwigfr 8:072a76960e27 89 for(int j = 0; j < mat1.nbColumn; ++j) {
Ludwigfr 8:072a76960e27 90 this->data[i][j] -= mat1.data[i][j];
Ludwigfr 8:072a76960e27 91 }
Ludwigfr 8:072a76960e27 92 }
Ludwigfr 8:072a76960e27 93 }
Ludwigfr 8:072a76960e27 94
Ludwigfr 8:072a76960e27 95 /*
Ludwigfr 8:072a76960e27 96 void myMatrix::print(){
Ludwigfr 8:072a76960e27 97 for(int i = 0; i < this->nbRow; ++i) {
Ludwigfr 8:072a76960e27 98 for(int j = 0; j < this->nbColumn; ++j) {
Ludwigfr 8:072a76960e27 99 cout << this->data[i][j] << " ";
Ludwigfr 8:072a76960e27 100 }
Ludwigfr 8:072a76960e27 101 cout << endl;
Ludwigfr 8:072a76960e27 102 }
Ludwigfr 8:072a76960e27 103 }
Ludwigfr 8:072a76960e27 104 */
Ludwigfr 8:072a76960e27 105
Ludwigfr 8:072a76960e27 106 //The rest is for inversion
Ludwigfr 8:072a76960e27 107
Ludwigfr 8:072a76960e27 108 // calculate the cofactor of element (row,col)
Ludwigfr 8:072a76960e27 109 int GetMinor(float **src, float **dest, int row, int col, int order)
Ludwigfr 8:072a76960e27 110 {
Ludwigfr 8:072a76960e27 111 // indicate which col and row is being copied to dest
Ludwigfr 8:072a76960e27 112 int colCount=0,rowCount=0;
Ludwigfr 8:072a76960e27 113
Ludwigfr 8:072a76960e27 114 for(int i = 0; i < order; i++ )
Ludwigfr 8:072a76960e27 115 {
Ludwigfr 8:072a76960e27 116 if( i != row )
Ludwigfr 8:072a76960e27 117 {
Ludwigfr 8:072a76960e27 118 colCount = 0;
Ludwigfr 8:072a76960e27 119 for(int j = 0; j < order; j++ )
Ludwigfr 8:072a76960e27 120 {
Ludwigfr 8:072a76960e27 121 // when j is not the element
Ludwigfr 8:072a76960e27 122 if( j != col )
Ludwigfr 8:072a76960e27 123 {
Ludwigfr 8:072a76960e27 124 dest[rowCount][colCount] = src[i][j];
Ludwigfr 8:072a76960e27 125 colCount++;
Ludwigfr 8:072a76960e27 126 }
Ludwigfr 8:072a76960e27 127 }
Ludwigfr 8:072a76960e27 128 rowCount++;
Ludwigfr 8:072a76960e27 129 }
Ludwigfr 8:072a76960e27 130 }
Ludwigfr 8:072a76960e27 131
Ludwigfr 8:072a76960e27 132 return 1;
Ludwigfr 8:072a76960e27 133 }
Ludwigfr 8:072a76960e27 134
Ludwigfr 8:072a76960e27 135 // Calculate the determinant recursively.
Ludwigfr 8:072a76960e27 136 double CalcDeterminant( float **mat, int order)
Ludwigfr 8:072a76960e27 137 {
Ludwigfr 8:072a76960e27 138 // order must be >= 0
Ludwigfr 8:072a76960e27 139 // stop the recursion when matrix is a single element
Ludwigfr 8:072a76960e27 140 if( order == 1 )
Ludwigfr 8:072a76960e27 141 return mat[0][0];
Ludwigfr 8:072a76960e27 142
Ludwigfr 8:072a76960e27 143 // the determinant value
Ludwigfr 8:072a76960e27 144 float det = 0;
Ludwigfr 8:072a76960e27 145
Ludwigfr 8:072a76960e27 146 // allocate the cofactor matrix
Ludwigfr 8:072a76960e27 147 float **minor;
Ludwigfr 8:072a76960e27 148 minor = new float*[order-1];
Ludwigfr 8:072a76960e27 149 for(int i=0;i<order-1;i++)
Ludwigfr 8:072a76960e27 150 minor[i] = new float[order-1];
Ludwigfr 8:072a76960e27 151
Ludwigfr 8:072a76960e27 152 for(int i = 0; i < order; i++ )
Ludwigfr 8:072a76960e27 153 {
Ludwigfr 8:072a76960e27 154 // get minor of element (0,i)
Ludwigfr 8:072a76960e27 155 GetMinor( mat, minor, 0, i , order);
Ludwigfr 8:072a76960e27 156 // the recusion is here!
Ludwigfr 8:072a76960e27 157
Ludwigfr 8:072a76960e27 158 det += (i%2==1?-1.0:1.0) * mat[0][i] * CalcDeterminant(minor,order-1);
Ludwigfr 8:072a76960e27 159 //det += pow( -1.0, i ) * mat[0][i] * CalcDeterminant( minor,order-1 );
Ludwigfr 8:072a76960e27 160 }
Ludwigfr 8:072a76960e27 161
Ludwigfr 8:072a76960e27 162 // release memory
Ludwigfr 8:072a76960e27 163 for(int i=0;i<order-1;i++)
Ludwigfr 8:072a76960e27 164 delete [] minor[i];
Ludwigfr 8:072a76960e27 165 delete [] minor;
Ludwigfr 8:072a76960e27 166
Ludwigfr 8:072a76960e27 167 return det;
Ludwigfr 8:072a76960e27 168 }
Ludwigfr 8:072a76960e27 169
Ludwigfr 8:072a76960e27 170 // matrix inversion
Ludwigfr 8:072a76960e27 171 // the result is put in Y
Ludwigfr 8:072a76960e27 172 void MatrixInversion(float **A, int order, float **Y)
Ludwigfr 8:072a76960e27 173 {
Ludwigfr 8:072a76960e27 174 // get the determinant of a
Ludwigfr 8:072a76960e27 175 double det = 1.0/CalcDeterminant(A,order);
Ludwigfr 8:072a76960e27 176
Ludwigfr 8:072a76960e27 177 // memory allocation
Ludwigfr 8:072a76960e27 178 float *temp = new float[(order-1)*(order-1)];
Ludwigfr 8:072a76960e27 179 float **minor = new float*[order-1];
Ludwigfr 8:072a76960e27 180 for(int i=0;i<order-1;i++)
Ludwigfr 8:072a76960e27 181 minor[i] = temp+(i*(order-1));
Ludwigfr 8:072a76960e27 182
Ludwigfr 8:072a76960e27 183 for(int j=0;j<order;j++)
Ludwigfr 8:072a76960e27 184 {
Ludwigfr 8:072a76960e27 185 for(int i=0;i<order;i++)
Ludwigfr 8:072a76960e27 186 {
Ludwigfr 8:072a76960e27 187 // get the co-factor (matrix) of A(j,i)
Ludwigfr 8:072a76960e27 188 GetMinor(A,minor,j,i,order);
Ludwigfr 8:072a76960e27 189 Y[i][j] = det*CalcDeterminant(minor,order-1);
Ludwigfr 8:072a76960e27 190 if( (i+j)%2 == 1)
Ludwigfr 8:072a76960e27 191 Y[i][j] = -Y[i][j];
Ludwigfr 8:072a76960e27 192 }
Ludwigfr 8:072a76960e27 193 }
Ludwigfr 8:072a76960e27 194
Ludwigfr 8:072a76960e27 195 // release memory
Ludwigfr 8:072a76960e27 196 //delete [] minor[0];
Ludwigfr 8:072a76960e27 197 delete [] temp;
Ludwigfr 8:072a76960e27 198 delete [] minor;
Ludwigfr 8:072a76960e27 199 }
Ludwigfr 8:072a76960e27 200
Ludwigfr 8:072a76960e27 201
Ludwigfr 8:072a76960e27 202 //assume the matrix is invertible and square
Ludwigfr 8:072a76960e27 203 void myMatrix::fillWithInverse(myMatrix mat1){
Ludwigfr 8:072a76960e27 204 MatrixInversion(mat1.data,mat1.nbColumn,this->data);
Ludwigfr 8:072a76960e27 205 }
Ludwigfr 8:072a76960e27 206
Ludwigfr 8:072a76960e27 207 /*
Ludwigfr 8:072a76960e27 208 //assume the dimensions are correct
Ludwigfr 8:072a76960e27 209 void myMatrix::fillWithArray(float** array){
Ludwigfr 8:072a76960e27 210 for(int i = 0; i < this->nbRow; ++i) {
Ludwigfr 8:072a76960e27 211 for(int j = 0; j < this->nbColumn; ++j) {
Ludwigfr 8:072a76960e27 212 this->data[j][i] = array[i][j];
Ludwigfr 8:072a76960e27 213 }
Ludwigfr 8:072a76960e27 214 }
Ludwigfr 8:072a76960e27 215 }
Ludwigfr 8:072a76960e27 216 */