Team DIANA
test_IPKF
Dependencies: mbed
Diff: source/matrix.c
- Revision:
- 0:fb6e494a7656
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/source/matrix.c Wed Apr 20 10:03:58 2016 +0000
@@ -0,0 +1,402 @@
+#include "matrix.h"
+
+int CreateMatrix(matrix *m)
+{
+ int i;
+ int k ;
+
+ m->element = (float**) malloc(m->row * (sizeof (float*)));
+ if(m->element == NULL)
+ {
+ return 1;
+ }
+ else
+ {
+ for(i = 0; i < m->row; i++)
+ {
+ m->element[i]= (float*) malloc(m->col * (sizeof (float)));
+ if(m->element[i] == NULL)
+ {
+ return 1;
+ }
+ }
+ }
+ for(i = 0; i < m->row; i++)
+ {
+ for(k = 0; k < m->col; k++)
+ {
+ m->element[i][k] = 0;
+ }
+ }
+ return 0;
+}
+
+void DeleteMatrix(matrix *m)
+{
+ int i;
+ for(i = 0; i< m->row; i ++)
+ {
+ if (m->element[i]!= NULL)
+ {
+ free(m->element[i]);
+ m->element[i] = NULL;
+ }
+ }
+ if (m->element[i]!= NULL)
+ {
+ free(m->element);
+ m->element = NULL;
+ }
+}
+
+int InitMatrix(matrix* m, int row, int col)
+{
+ m->row = row;
+ m->col = col;
+ return CreateMatrix(m);
+}
+
+int MxM (matrix* a, matrix* b, matrix* r)
+{
+ int i;
+ int z;
+ int j;
+ matrix temp;
+ InitMatrix(&temp, a->row, b->col);
+
+
+ if (a->col != b->row)
+ {
+ DeleteMatrix(&temp);
+ return 1;
+ }
+ else if(a->row!=r->row)
+ {
+ DeleteMatrix(&temp);
+ return 2;
+ }
+ else if(b->col!=r->col)
+ {
+ DeleteMatrix(&temp);
+ return 3;
+ }
+ else
+ {
+
+ for(i = 0; i < a->row; i++)
+ {
+ for(j = 0; j < b->col; j++)
+ {
+ for(z = 0; z < a->col; z++)
+ {
+ temp.element[i][j]+= a->element[i][z] * b->element[z][j];
+ }
+ }
+ }
+
+ for(i = 0; i < temp.row; i++)
+ {
+ for(j = 0; j < temp.col; j++)
+ {
+ r->element[i][j]=temp.element[i][j];
+ }
+ }
+ DeleteMatrix(&temp);
+ return 0;
+ }
+}
+
+int axM(matrix *m,float a, matrix* r)
+{
+ int i = 0;
+ int j = 0;
+ matrix temp;
+
+ InitMatrix(&temp,m->row,m->col);
+
+ if(m->row!=r->row)
+ {
+ DeleteMatrix(&temp);
+ return 1;
+ }
+ else if(m->col!=r->col)
+ {
+ DeleteMatrix(&temp);
+ return 2;
+ }
+ else
+ {
+
+
+ for(i = 0; i < m->row; i++)
+ {
+ for(j = 0; j < m->col; j++)
+ {
+ temp.element[i][j] = (m->element[i][j]) * a;
+ }
+ }
+ for(i = 0; i < temp.row; i++)
+ {
+ for(j = 0; j < temp.col; j++)
+ {
+ r->element[i][j]=temp.element[i][j];
+ }
+ }
+ DeleteMatrix(&temp);
+
+ return 0;
+ }
+}
+
+int Sum(matrix *a, matrix *b, matrix* r)
+{
+ int i = 0;
+ int j = 0;
+
+ if(a->col != b->col)
+ {
+ return 1;
+ }
+ else if(a->row != b->row)
+ {
+ return 2;
+ }
+ else if(a->row != r->row)
+ {
+ return 3;
+ }
+ else if(a->col != r->col)
+ {
+ return 4;
+ }
+ else
+ {
+ for(i = 0; i < r->row; i++)
+ {
+ for(j = 0; j < r->col; j++)
+ {
+ r->element[i][j] = (a->element[i][j]) + (b->element[i][j]);
+ }
+ }
+
+ return 0;
+ }
+}
+
+int Sub(matrix *a, matrix *b, matrix* r)
+{
+ int i = 0;
+ int j = 0;
+
+ if(a->col != b->col)
+ {
+ return 1;
+ }
+ else if(a->row != b->row)
+ {
+ return 2;
+ }
+ else if(a->row != r->row)
+ {
+ return 3;
+ }
+ else if(a->col != r->col)
+ {
+ return 4;
+ }
+ else
+ {
+ for(i = 0; i < r->row; i++)
+ {
+ for(j = 0; j < r->col; j++)
+ {
+ r->element[i][j] = (a->element[i][j]) - (b->element[i][j]);
+ }
+ }
+
+ return 0;
+ }
+}
+
+int Traspost(matrix *m, matrix *r)
+{
+ int i;
+ int j;
+
+ matrix temp;
+ InitMatrix(&temp,m->row,m->col);
+
+ if (m->row!=r->col)
+ {
+ DeleteMatrix(&temp);
+ return 1;
+ }
+ else if (m->col!=r->row)
+ {
+ DeleteMatrix(&temp);
+ return 2;
+ }
+ else
+ {
+ for(i = 0; i < m->row; i++)
+ {
+ for(j = 0; j < m->col; j++)
+ {
+ temp.element[j][i] = m->element[i][j];
+ }
+ }
+
+ for(i = 0; i < r->row; i++)
+ {
+ for(j = 0; j < r->col; j++)
+ {
+ r->element[i][j] = temp.element[i][j];
+ }
+ }
+
+ DeleteMatrix(&temp);
+ return 0;
+ }
+}
+
+int Inv(matrix *m, matrix *inv_m)
+{
+ int i = 0;
+ int j = 0;
+ int k = 0;
+ float regTemp = 0;
+
+ matrix temp;
+ temp.row = m->row;
+ temp.col = (m->col) * 2;
+ CreateMatrix(&temp);
+
+ if (m->col != m->row)
+ { DeleteMatrix(&temp);
+ return 1;
+ }
+ else if (m->row != inv_m->row)
+ { DeleteMatrix(&temp);
+ return 2;
+ }
+ else if (m->col != inv_m->col)
+ { DeleteMatrix(&temp);
+ return 3;
+ }
+ else
+ {
+
+ for(i = 0; i < m->row; i++)
+ {
+ for(j = 0 ; j < m->col; j++)
+ {
+ temp.element[i][j] = m->element[i][j];
+ if(j == 0)
+ {
+ temp.element[i][i + m->col] = 1;
+ }
+ }
+ }
+
+ /*start gauss algorithm*/
+ for(k = 0; k < temp.row; k++)
+ {
+ /*check if element [k][k] != 0*/
+ if(temp.element[k][k] == 0)
+ {
+ /*find an element [j][k] != 0*/
+ for(i = k; i < temp.row; i++)
+ {
+ if(temp.element[i][k] != 0)
+ {
+ break;
+ }
+ }
+ /*if not the matrix has no inverse*/
+ if(i == temp.row)
+ {
+ DeleteMatrix(&temp);
+ return 4;
+ }
+ /*else exchange row*/
+ for(j = 0; j < temp.col; j++)
+ {
+ regTemp = temp.element[k][j];
+ temp.element[k][j] = temp.element[i][j];
+ temp.element[i][j] = regTemp;
+ }
+ }
+ /*row k divided by element[k][k]*/
+ regTemp = temp.element[k][k];
+ for(j = 0; j<temp.col; j++)
+ {
+ temp.element[k][j] = temp.element[k][j]/regTemp;
+ }
+ /*compute the other value of matrix*/
+ for (i = 0; i < temp.row; i++)
+ {
+ if (i != k)
+ {
+ regTemp = temp.element[i][k];
+ for(j = 0; j < temp.col; j++)
+ {
+ temp.element[i][j] = temp.element[i][j] - temp.element[k][j] * regTemp;
+ }
+ }
+ }
+ }
+ for(i = 0; i < inv_m->row; i++)
+ {
+ for(j = 0; j < inv_m->col; j++)
+ {
+ inv_m->element[i][j] = temp.element[i][inv_m->col + j];
+ }
+ }
+ DeleteMatrix(&temp);
+ return 0;
+ }
+}
+
+void CrossProd(matrix* v, matrix* w, matrix* r)
+{
+ matrix s;
+
+ InitMatrix(&s, 3,3);
+
+ //Sv = [0,-v(3),v(2);v(3),0,-v(1);-v(2),v(1),0];
+ s.element[0][0] = 0;
+ s.element[0][1] = -(v->element[2][0]);
+ s.element[0][2] = v->element[1][0];
+ s.element[1][0] = v->element[2][0];
+ s.element[1][1] = 0;
+ s.element[1][2] = -v->element[0][0];
+ s.element[2][0] = -v->element[1][0];
+ s.element[2][1] = v->element[0][0];
+ s.element[2][2] = 0;
+
+ //k = Sv*w;
+ MxM(&s, w, r);
+
+ DeleteMatrix(&s);
+
+ return;
+}
+
+void Print_Matrix(matrix* m)
+{
+ int i;
+ int j;
+ for (i = 0; i<m->row;i++)
+ {
+ for( j = 0; j < m->col;j++)
+ {
+ printf("%f ",m->element[i][j]);
+ }
+ printf("\n");
+
+ }
+ printf("\n");
+ return;
+}
+
+