Team DIANA / Mbed 2 deprecated IPKF_Test_nucleo

test_IPKF

Dependencies:   mbed

source/matrix.c@0:fb6e494a7656, 2016-04-20 (annotated)

Committer:
LudovicoDani
Date:
Wed Apr 20 10:03:58 2016 +0000
Revision:
0:fb6e494a7656
IPKF_Test_nucleo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LudovicoDani 0:fb6e494a7656 1 #include "matrix.h"
LudovicoDani 0:fb6e494a7656 2
LudovicoDani 0:fb6e494a7656 3 int CreateMatrix(matrix *m)
LudovicoDani 0:fb6e494a7656 4 {
LudovicoDani 0:fb6e494a7656 5 int i;
LudovicoDani 0:fb6e494a7656 6 int k ;
LudovicoDani 0:fb6e494a7656 7
LudovicoDani 0:fb6e494a7656 8 m->element = (float**) malloc(m->row * (sizeof (float*)));
LudovicoDani 0:fb6e494a7656 9 if(m->element == NULL)
LudovicoDani 0:fb6e494a7656 10 {
LudovicoDani 0:fb6e494a7656 11 return 1;
LudovicoDani 0:fb6e494a7656 12 }
LudovicoDani 0:fb6e494a7656 13 else
LudovicoDani 0:fb6e494a7656 14 {
LudovicoDani 0:fb6e494a7656 15 for(i = 0; i < m->row; i++)
LudovicoDani 0:fb6e494a7656 16 {
LudovicoDani 0:fb6e494a7656 17 m->element[i]= (float*) malloc(m->col * (sizeof (float)));
LudovicoDani 0:fb6e494a7656 18 if(m->element[i] == NULL)
LudovicoDani 0:fb6e494a7656 19 {
LudovicoDani 0:fb6e494a7656 20 return 1;
LudovicoDani 0:fb6e494a7656 21 }
LudovicoDani 0:fb6e494a7656 22 }
LudovicoDani 0:fb6e494a7656 23 }
LudovicoDani 0:fb6e494a7656 24 for(i = 0; i < m->row; i++)
LudovicoDani 0:fb6e494a7656 25 {
LudovicoDani 0:fb6e494a7656 26 for(k = 0; k < m->col; k++)
LudovicoDani 0:fb6e494a7656 27 {
LudovicoDani 0:fb6e494a7656 28 m->element[i][k] = 0;
LudovicoDani 0:fb6e494a7656 29 }
LudovicoDani 0:fb6e494a7656 30 }
LudovicoDani 0:fb6e494a7656 31 return 0;
LudovicoDani 0:fb6e494a7656 32 }
LudovicoDani 0:fb6e494a7656 33
LudovicoDani 0:fb6e494a7656 34 void DeleteMatrix(matrix *m)
LudovicoDani 0:fb6e494a7656 35 {
LudovicoDani 0:fb6e494a7656 36 int i;
LudovicoDani 0:fb6e494a7656 37 for(i = 0; i< m->row; i ++)
LudovicoDani 0:fb6e494a7656 38 {
LudovicoDani 0:fb6e494a7656 39 if (m->element[i]!= NULL)
LudovicoDani 0:fb6e494a7656 40 {
LudovicoDani 0:fb6e494a7656 41 free(m->element[i]);
LudovicoDani 0:fb6e494a7656 42 m->element[i] = NULL;
LudovicoDani 0:fb6e494a7656 43 }
LudovicoDani 0:fb6e494a7656 44 }
LudovicoDani 0:fb6e494a7656 45 if (m->element[i]!= NULL)
LudovicoDani 0:fb6e494a7656 46 {
LudovicoDani 0:fb6e494a7656 47 free(m->element);
LudovicoDani 0:fb6e494a7656 48 m->element = NULL;
LudovicoDani 0:fb6e494a7656 49 }
LudovicoDani 0:fb6e494a7656 50 }
LudovicoDani 0:fb6e494a7656 51
LudovicoDani 0:fb6e494a7656 52 int InitMatrix(matrix* m, int row, int col)
LudovicoDani 0:fb6e494a7656 53 {
LudovicoDani 0:fb6e494a7656 54 m->row = row;
LudovicoDani 0:fb6e494a7656 55 m->col = col;
LudovicoDani 0:fb6e494a7656 56 return CreateMatrix(m);
LudovicoDani 0:fb6e494a7656 57 }
LudovicoDani 0:fb6e494a7656 58
LudovicoDani 0:fb6e494a7656 59 int MxM (matrix* a, matrix* b, matrix* r)
LudovicoDani 0:fb6e494a7656 60 {
LudovicoDani 0:fb6e494a7656 61 int i;
LudovicoDani 0:fb6e494a7656 62 int z;
LudovicoDani 0:fb6e494a7656 63 int j;
LudovicoDani 0:fb6e494a7656 64 matrix temp;
LudovicoDani 0:fb6e494a7656 65 InitMatrix(&temp, a->row, b->col);
LudovicoDani 0:fb6e494a7656 66
LudovicoDani 0:fb6e494a7656 67
LudovicoDani 0:fb6e494a7656 68 if (a->col != b->row)
LudovicoDani 0:fb6e494a7656 69 {
LudovicoDani 0:fb6e494a7656 70 DeleteMatrix(&temp);
LudovicoDani 0:fb6e494a7656 71 return 1;
LudovicoDani 0:fb6e494a7656 72 }
LudovicoDani 0:fb6e494a7656 73 else if(a->row!=r->row)
LudovicoDani 0:fb6e494a7656 74 {
LudovicoDani 0:fb6e494a7656 75 DeleteMatrix(&temp);
LudovicoDani 0:fb6e494a7656 76 return 2;
LudovicoDani 0:fb6e494a7656 77 }
LudovicoDani 0:fb6e494a7656 78 else if(b->col!=r->col)
LudovicoDani 0:fb6e494a7656 79 {
LudovicoDani 0:fb6e494a7656 80 DeleteMatrix(&temp);
LudovicoDani 0:fb6e494a7656 81 return 3;
LudovicoDani 0:fb6e494a7656 82 }
LudovicoDani 0:fb6e494a7656 83 else
LudovicoDani 0:fb6e494a7656 84 {
LudovicoDani 0:fb6e494a7656 85
LudovicoDani 0:fb6e494a7656 86 for(i = 0; i < a->row; i++)
LudovicoDani 0:fb6e494a7656 87 {
LudovicoDani 0:fb6e494a7656 88 for(j = 0; j < b->col; j++)
LudovicoDani 0:fb6e494a7656 89 {
LudovicoDani 0:fb6e494a7656 90 for(z = 0; z < a->col; z++)
LudovicoDani 0:fb6e494a7656 91 {
LudovicoDani 0:fb6e494a7656 92 temp.element[i][j]+= a->element[i][z] * b->element[z][j];
LudovicoDani 0:fb6e494a7656 93 }
LudovicoDani 0:fb6e494a7656 94 }
LudovicoDani 0:fb6e494a7656 95 }
LudovicoDani 0:fb6e494a7656 96
LudovicoDani 0:fb6e494a7656 97 for(i = 0; i < temp.row; i++)
LudovicoDani 0:fb6e494a7656 98 {
LudovicoDani 0:fb6e494a7656 99 for(j = 0; j < temp.col; j++)
LudovicoDani 0:fb6e494a7656 100 {
LudovicoDani 0:fb6e494a7656 101 r->element[i][j]=temp.element[i][j];
LudovicoDani 0:fb6e494a7656 102 }
LudovicoDani 0:fb6e494a7656 103 }
LudovicoDani 0:fb6e494a7656 104 DeleteMatrix(&temp);
LudovicoDani 0:fb6e494a7656 105 return 0;
LudovicoDani 0:fb6e494a7656 106 }
LudovicoDani 0:fb6e494a7656 107 }
LudovicoDani 0:fb6e494a7656 108
LudovicoDani 0:fb6e494a7656 109 int axM(matrix *m,float a, matrix* r)
LudovicoDani 0:fb6e494a7656 110 {
LudovicoDani 0:fb6e494a7656 111 int i = 0;
LudovicoDani 0:fb6e494a7656 112 int j = 0;
LudovicoDani 0:fb6e494a7656 113 matrix temp;
LudovicoDani 0:fb6e494a7656 114
LudovicoDani 0:fb6e494a7656 115 InitMatrix(&temp,m->row,m->col);
LudovicoDani 0:fb6e494a7656 116
LudovicoDani 0:fb6e494a7656 117 if(m->row!=r->row)
LudovicoDani 0:fb6e494a7656 118 {
LudovicoDani 0:fb6e494a7656 119 DeleteMatrix(&temp);
LudovicoDani 0:fb6e494a7656 120 return 1;
LudovicoDani 0:fb6e494a7656 121 }
LudovicoDani 0:fb6e494a7656 122 else if(m->col!=r->col)
LudovicoDani 0:fb6e494a7656 123 {
LudovicoDani 0:fb6e494a7656 124 DeleteMatrix(&temp);
LudovicoDani 0:fb6e494a7656 125 return 2;
LudovicoDani 0:fb6e494a7656 126 }
LudovicoDani 0:fb6e494a7656 127 else
LudovicoDani 0:fb6e494a7656 128 {
LudovicoDani 0:fb6e494a7656 129
LudovicoDani 0:fb6e494a7656 130
LudovicoDani 0:fb6e494a7656 131 for(i = 0; i < m->row; i++)
LudovicoDani 0:fb6e494a7656 132 {
LudovicoDani 0:fb6e494a7656 133 for(j = 0; j < m->col; j++)
LudovicoDani 0:fb6e494a7656 134 {
LudovicoDani 0:fb6e494a7656 135 temp.element[i][j] = (m->element[i][j]) * a;
LudovicoDani 0:fb6e494a7656 136 }
LudovicoDani 0:fb6e494a7656 137 }
LudovicoDani 0:fb6e494a7656 138 for(i = 0; i < temp.row; i++)
LudovicoDani 0:fb6e494a7656 139 {
LudovicoDani 0:fb6e494a7656 140 for(j = 0; j < temp.col; j++)
LudovicoDani 0:fb6e494a7656 141 {
LudovicoDani 0:fb6e494a7656 142 r->element[i][j]=temp.element[i][j];
LudovicoDani 0:fb6e494a7656 143 }
LudovicoDani 0:fb6e494a7656 144 }
LudovicoDani 0:fb6e494a7656 145 DeleteMatrix(&temp);
LudovicoDani 0:fb6e494a7656 146
LudovicoDani 0:fb6e494a7656 147 return 0;
LudovicoDani 0:fb6e494a7656 148 }
LudovicoDani 0:fb6e494a7656 149 }
LudovicoDani 0:fb6e494a7656 150
LudovicoDani 0:fb6e494a7656 151 int Sum(matrix *a, matrix *b, matrix* r)
LudovicoDani 0:fb6e494a7656 152 {
LudovicoDani 0:fb6e494a7656 153 int i = 0;
LudovicoDani 0:fb6e494a7656 154 int j = 0;
LudovicoDani 0:fb6e494a7656 155
LudovicoDani 0:fb6e494a7656 156 if(a->col != b->col)
LudovicoDani 0:fb6e494a7656 157 {
LudovicoDani 0:fb6e494a7656 158 return 1;
LudovicoDani 0:fb6e494a7656 159 }
LudovicoDani 0:fb6e494a7656 160 else if(a->row != b->row)
LudovicoDani 0:fb6e494a7656 161 {
LudovicoDani 0:fb6e494a7656 162 return 2;
LudovicoDani 0:fb6e494a7656 163 }
LudovicoDani 0:fb6e494a7656 164 else if(a->row != r->row)
LudovicoDani 0:fb6e494a7656 165 {
LudovicoDani 0:fb6e494a7656 166 return 3;
LudovicoDani 0:fb6e494a7656 167 }
LudovicoDani 0:fb6e494a7656 168 else if(a->col != r->col)
LudovicoDani 0:fb6e494a7656 169 {
LudovicoDani 0:fb6e494a7656 170 return 4;
LudovicoDani 0:fb6e494a7656 171 }
LudovicoDani 0:fb6e494a7656 172 else
LudovicoDani 0:fb6e494a7656 173 {
LudovicoDani 0:fb6e494a7656 174 for(i = 0; i < r->row; i++)
LudovicoDani 0:fb6e494a7656 175 {
LudovicoDani 0:fb6e494a7656 176 for(j = 0; j < r->col; j++)
LudovicoDani 0:fb6e494a7656 177 {
LudovicoDani 0:fb6e494a7656 178 r->element[i][j] = (a->element[i][j]) + (b->element[i][j]);
LudovicoDani 0:fb6e494a7656 179 }
LudovicoDani 0:fb6e494a7656 180 }
LudovicoDani 0:fb6e494a7656 181
LudovicoDani 0:fb6e494a7656 182 return 0;
LudovicoDani 0:fb6e494a7656 183 }
LudovicoDani 0:fb6e494a7656 184 }
LudovicoDani 0:fb6e494a7656 185
LudovicoDani 0:fb6e494a7656 186 int Sub(matrix *a, matrix *b, matrix* r)
LudovicoDani 0:fb6e494a7656 187 {
LudovicoDani 0:fb6e494a7656 188 int i = 0;
LudovicoDani 0:fb6e494a7656 189 int j = 0;
LudovicoDani 0:fb6e494a7656 190
LudovicoDani 0:fb6e494a7656 191 if(a->col != b->col)
LudovicoDani 0:fb6e494a7656 192 {
LudovicoDani 0:fb6e494a7656 193 return 1;
LudovicoDani 0:fb6e494a7656 194 }
LudovicoDani 0:fb6e494a7656 195 else if(a->row != b->row)
LudovicoDani 0:fb6e494a7656 196 {
LudovicoDani 0:fb6e494a7656 197 return 2;
LudovicoDani 0:fb6e494a7656 198 }
LudovicoDani 0:fb6e494a7656 199 else if(a->row != r->row)
LudovicoDani 0:fb6e494a7656 200 {
LudovicoDani 0:fb6e494a7656 201 return 3;
LudovicoDani 0:fb6e494a7656 202 }
LudovicoDani 0:fb6e494a7656 203 else if(a->col != r->col)
LudovicoDani 0:fb6e494a7656 204 {
LudovicoDani 0:fb6e494a7656 205 return 4;
LudovicoDani 0:fb6e494a7656 206 }
LudovicoDani 0:fb6e494a7656 207 else
LudovicoDani 0:fb6e494a7656 208 {
LudovicoDani 0:fb6e494a7656 209 for(i = 0; i < r->row; i++)
LudovicoDani 0:fb6e494a7656 210 {
LudovicoDani 0:fb6e494a7656 211 for(j = 0; j < r->col; j++)
LudovicoDani 0:fb6e494a7656 212 {
LudovicoDani 0:fb6e494a7656 213 r->element[i][j] = (a->element[i][j]) - (b->element[i][j]);
LudovicoDani 0:fb6e494a7656 214 }
LudovicoDani 0:fb6e494a7656 215 }
LudovicoDani 0:fb6e494a7656 216
LudovicoDani 0:fb6e494a7656 217 return 0;
LudovicoDani 0:fb6e494a7656 218 }
LudovicoDani 0:fb6e494a7656 219 }
LudovicoDani 0:fb6e494a7656 220
LudovicoDani 0:fb6e494a7656 221 int Traspost(matrix *m, matrix *r)
LudovicoDani 0:fb6e494a7656 222 {
LudovicoDani 0:fb6e494a7656 223 int i;
LudovicoDani 0:fb6e494a7656 224 int j;
LudovicoDani 0:fb6e494a7656 225
LudovicoDani 0:fb6e494a7656 226 matrix temp;
LudovicoDani 0:fb6e494a7656 227 InitMatrix(&temp,m->row,m->col);
LudovicoDani 0:fb6e494a7656 228
LudovicoDani 0:fb6e494a7656 229 if (m->row!=r->col)
LudovicoDani 0:fb6e494a7656 230 {
LudovicoDani 0:fb6e494a7656 231 DeleteMatrix(&temp);
LudovicoDani 0:fb6e494a7656 232 return 1;
LudovicoDani 0:fb6e494a7656 233 }
LudovicoDani 0:fb6e494a7656 234 else if (m->col!=r->row)
LudovicoDani 0:fb6e494a7656 235 {
LudovicoDani 0:fb6e494a7656 236 DeleteMatrix(&temp);
LudovicoDani 0:fb6e494a7656 237 return 2;
LudovicoDani 0:fb6e494a7656 238 }
LudovicoDani 0:fb6e494a7656 239 else
LudovicoDani 0:fb6e494a7656 240 {
LudovicoDani 0:fb6e494a7656 241 for(i = 0; i < m->row; i++)
LudovicoDani 0:fb6e494a7656 242 {
LudovicoDani 0:fb6e494a7656 243 for(j = 0; j < m->col; j++)
LudovicoDani 0:fb6e494a7656 244 {
LudovicoDani 0:fb6e494a7656 245 temp.element[j][i] = m->element[i][j];
LudovicoDani 0:fb6e494a7656 246 }
LudovicoDani 0:fb6e494a7656 247 }
LudovicoDani 0:fb6e494a7656 248
LudovicoDani 0:fb6e494a7656 249 for(i = 0; i < r->row; i++)
LudovicoDani 0:fb6e494a7656 250 {
LudovicoDani 0:fb6e494a7656 251 for(j = 0; j < r->col; j++)
LudovicoDani 0:fb6e494a7656 252 {
LudovicoDani 0:fb6e494a7656 253 r->element[i][j] = temp.element[i][j];
LudovicoDani 0:fb6e494a7656 254 }
LudovicoDani 0:fb6e494a7656 255 }
LudovicoDani 0:fb6e494a7656 256
LudovicoDani 0:fb6e494a7656 257 DeleteMatrix(&temp);
LudovicoDani 0:fb6e494a7656 258 return 0;
LudovicoDani 0:fb6e494a7656 259 }
LudovicoDani 0:fb6e494a7656 260 }
LudovicoDani 0:fb6e494a7656 261
LudovicoDani 0:fb6e494a7656 262 int Inv(matrix *m, matrix *inv_m)
LudovicoDani 0:fb6e494a7656 263 {
LudovicoDani 0:fb6e494a7656 264 int i = 0;
LudovicoDani 0:fb6e494a7656 265 int j = 0;
LudovicoDani 0:fb6e494a7656 266 int k = 0;
LudovicoDani 0:fb6e494a7656 267 float regTemp = 0;
LudovicoDani 0:fb6e494a7656 268
LudovicoDani 0:fb6e494a7656 269 matrix temp;
LudovicoDani 0:fb6e494a7656 270 temp.row = m->row;
LudovicoDani 0:fb6e494a7656 271 temp.col = (m->col) * 2;
LudovicoDani 0:fb6e494a7656 272 CreateMatrix(&temp);
LudovicoDani 0:fb6e494a7656 273
LudovicoDani 0:fb6e494a7656 274 if (m->col != m->row)
LudovicoDani 0:fb6e494a7656 275 { DeleteMatrix(&temp);
LudovicoDani 0:fb6e494a7656 276 return 1;
LudovicoDani 0:fb6e494a7656 277 }
LudovicoDani 0:fb6e494a7656 278 else if (m->row != inv_m->row)
LudovicoDani 0:fb6e494a7656 279 { DeleteMatrix(&temp);
LudovicoDani 0:fb6e494a7656 280 return 2;
LudovicoDani 0:fb6e494a7656 281 }
LudovicoDani 0:fb6e494a7656 282 else if (m->col != inv_m->col)
LudovicoDani 0:fb6e494a7656 283 { DeleteMatrix(&temp);
LudovicoDani 0:fb6e494a7656 284 return 3;
LudovicoDani 0:fb6e494a7656 285 }
LudovicoDani 0:fb6e494a7656 286 else
LudovicoDani 0:fb6e494a7656 287 {
LudovicoDani 0:fb6e494a7656 288
LudovicoDani 0:fb6e494a7656 289 for(i = 0; i < m->row; i++)
LudovicoDani 0:fb6e494a7656 290 {
LudovicoDani 0:fb6e494a7656 291 for(j = 0 ; j < m->col; j++)
LudovicoDani 0:fb6e494a7656 292 {
LudovicoDani 0:fb6e494a7656 293 temp.element[i][j] = m->element[i][j];
LudovicoDani 0:fb6e494a7656 294 if(j == 0)
LudovicoDani 0:fb6e494a7656 295 {
LudovicoDani 0:fb6e494a7656 296 temp.element[i][i + m->col] = 1;
LudovicoDani 0:fb6e494a7656 297 }
LudovicoDani 0:fb6e494a7656 298 }
LudovicoDani 0:fb6e494a7656 299 }
LudovicoDani 0:fb6e494a7656 300
LudovicoDani 0:fb6e494a7656 301 /*start gauss algorithm*/
LudovicoDani 0:fb6e494a7656 302 for(k = 0; k < temp.row; k++)
LudovicoDani 0:fb6e494a7656 303 {
LudovicoDani 0:fb6e494a7656 304 /*check if element [k][k] != 0*/
LudovicoDani 0:fb6e494a7656 305 if(temp.element[k][k] == 0)
LudovicoDani 0:fb6e494a7656 306 {
LudovicoDani 0:fb6e494a7656 307 /*find an element [j][k] != 0*/
LudovicoDani 0:fb6e494a7656 308 for(i = k; i < temp.row; i++)
LudovicoDani 0:fb6e494a7656 309 {
LudovicoDani 0:fb6e494a7656 310 if(temp.element[i][k] != 0)
LudovicoDani 0:fb6e494a7656 311 {
LudovicoDani 0:fb6e494a7656 312 break;
LudovicoDani 0:fb6e494a7656 313 }
LudovicoDani 0:fb6e494a7656 314 }
LudovicoDani 0:fb6e494a7656 315 /*if not the matrix has no inverse*/
LudovicoDani 0:fb6e494a7656 316 if(i == temp.row)
LudovicoDani 0:fb6e494a7656 317 {
LudovicoDani 0:fb6e494a7656 318 DeleteMatrix(&temp);
LudovicoDani 0:fb6e494a7656 319 return 4;
LudovicoDani 0:fb6e494a7656 320 }
LudovicoDani 0:fb6e494a7656 321 /*else exchange row*/
LudovicoDani 0:fb6e494a7656 322 for(j = 0; j < temp.col; j++)
LudovicoDani 0:fb6e494a7656 323 {
LudovicoDani 0:fb6e494a7656 324 regTemp = temp.element[k][j];
LudovicoDani 0:fb6e494a7656 325 temp.element[k][j] = temp.element[i][j];
LudovicoDani 0:fb6e494a7656 326 temp.element[i][j] = regTemp;
LudovicoDani 0:fb6e494a7656 327 }
LudovicoDani 0:fb6e494a7656 328 }
LudovicoDani 0:fb6e494a7656 329 /*row k divided by element[k][k]*/
LudovicoDani 0:fb6e494a7656 330 regTemp = temp.element[k][k];
LudovicoDani 0:fb6e494a7656 331 for(j = 0; j<temp.col; j++)
LudovicoDani 0:fb6e494a7656 332 {
LudovicoDani 0:fb6e494a7656 333 temp.element[k][j] = temp.element[k][j]/regTemp;
LudovicoDani 0:fb6e494a7656 334 }
LudovicoDani 0:fb6e494a7656 335 /*compute the other value of matrix*/
LudovicoDani 0:fb6e494a7656 336 for (i = 0; i < temp.row; i++)
LudovicoDani 0:fb6e494a7656 337 {
LudovicoDani 0:fb6e494a7656 338 if (i != k)
LudovicoDani 0:fb6e494a7656 339 {
LudovicoDani 0:fb6e494a7656 340 regTemp = temp.element[i][k];
LudovicoDani 0:fb6e494a7656 341 for(j = 0; j < temp.col; j++)
LudovicoDani 0:fb6e494a7656 342 {
LudovicoDani 0:fb6e494a7656 343 temp.element[i][j] = temp.element[i][j] - temp.element[k][j] * regTemp;
LudovicoDani 0:fb6e494a7656 344 }
LudovicoDani 0:fb6e494a7656 345 }
LudovicoDani 0:fb6e494a7656 346 }
LudovicoDani 0:fb6e494a7656 347 }
LudovicoDani 0:fb6e494a7656 348 for(i = 0; i < inv_m->row; i++)
LudovicoDani 0:fb6e494a7656 349 {
LudovicoDani 0:fb6e494a7656 350 for(j = 0; j < inv_m->col; j++)
LudovicoDani 0:fb6e494a7656 351 {
LudovicoDani 0:fb6e494a7656 352 inv_m->element[i][j] = temp.element[i][inv_m->col + j];
LudovicoDani 0:fb6e494a7656 353 }
LudovicoDani 0:fb6e494a7656 354 }
LudovicoDani 0:fb6e494a7656 355 DeleteMatrix(&temp);
LudovicoDani 0:fb6e494a7656 356 return 0;
LudovicoDani 0:fb6e494a7656 357 }
LudovicoDani 0:fb6e494a7656 358 }
LudovicoDani 0:fb6e494a7656 359
LudovicoDani 0:fb6e494a7656 360 void CrossProd(matrix* v, matrix* w, matrix* r)
LudovicoDani 0:fb6e494a7656 361 {
LudovicoDani 0:fb6e494a7656 362 matrix s;
LudovicoDani 0:fb6e494a7656 363
LudovicoDani 0:fb6e494a7656 364 InitMatrix(&s, 3,3);
LudovicoDani 0:fb6e494a7656 365
LudovicoDani 0:fb6e494a7656 366 //Sv = [0,-v(3),v(2);v(3),0,-v(1);-v(2),v(1),0];
LudovicoDani 0:fb6e494a7656 367 s.element[0][0] = 0;
LudovicoDani 0:fb6e494a7656 368 s.element[0][1] = -(v->element[2][0]);
LudovicoDani 0:fb6e494a7656 369 s.element[0][2] = v->element[1][0];
LudovicoDani 0:fb6e494a7656 370 s.element[1][0] = v->element[2][0];
LudovicoDani 0:fb6e494a7656 371 s.element[1][1] = 0;
LudovicoDani 0:fb6e494a7656 372 s.element[1][2] = -v->element[0][0];
LudovicoDani 0:fb6e494a7656 373 s.element[2][0] = -v->element[1][0];
LudovicoDani 0:fb6e494a7656 374 s.element[2][1] = v->element[0][0];
LudovicoDani 0:fb6e494a7656 375 s.element[2][2] = 0;
LudovicoDani 0:fb6e494a7656 376
LudovicoDani 0:fb6e494a7656 377 //k = Sv*w;
LudovicoDani 0:fb6e494a7656 378 MxM(&s, w, r);
LudovicoDani 0:fb6e494a7656 379
LudovicoDani 0:fb6e494a7656 380 DeleteMatrix(&s);
LudovicoDani 0:fb6e494a7656 381
LudovicoDani 0:fb6e494a7656 382 return;
LudovicoDani 0:fb6e494a7656 383 }
LudovicoDani 0:fb6e494a7656 384
LudovicoDani 0:fb6e494a7656 385 void Print_Matrix(matrix* m)
LudovicoDani 0:fb6e494a7656 386 {
LudovicoDani 0:fb6e494a7656 387 int i;
LudovicoDani 0:fb6e494a7656 388 int j;
LudovicoDani 0:fb6e494a7656 389 for (i = 0; i<m->row;i++)
LudovicoDani 0:fb6e494a7656 390 {
LudovicoDani 0:fb6e494a7656 391 for( j = 0; j < m->col;j++)
LudovicoDani 0:fb6e494a7656 392 {
LudovicoDani 0:fb6e494a7656 393 printf("%f ",m->element[i][j]);
LudovicoDani 0:fb6e494a7656 394 }
LudovicoDani 0:fb6e494a7656 395 printf("\n");
LudovicoDani 0:fb6e494a7656 396
LudovicoDani 0:fb6e494a7656 397 }
LudovicoDani 0:fb6e494a7656 398 printf("\n");
LudovicoDani 0:fb6e494a7656 399 return;
LudovicoDani 0:fb6e494a7656 400 }
LudovicoDani 0:fb6e494a7656 401
LudovicoDani 0:fb6e494a7656 402