Paparazzi UAS  v4.2.2_stable-4-gcc32f65 Paparazzi is a free software Unmanned Aircraft System.
pprz_simple_matrix.h
Go to the documentation of this file.
1 #ifndef PPRZ_SIMPLE_MATRIX_H
2 #define PPRZ_SIMPLE_MATRIX_H
3
4 #include <float.h> /* for FLT_EPSILON */
5 #include <math.h>
6 #ifdef HAVE_STDIO
7 #include <stdio.h> /* for printf'ing warnings */
8 #define warn_message printf
9 #else
10 #define warn_message(...) do { } while(0)
11 #endif
12
13 //
14 // C = A*B A:(i,k) B:(k,j) C:(i,j)
15 //
16 #define MAT_MUL(_i, _k, _j, C, A, B) { \
17  int l,c,m; \
18  for (l=0; l<_i; l++) \
19  for (c=0; c<_j; c++) { \
20  C[l][c] = 0.; \
21  for (m=0; m<_k; m++) \
22  C[l][c] += A[l][m]*B[m][c]; \
23  } \
24  }
25
26 //
27 // C = A*B' A:(i,k) B:(j,k) C:(i,j)
28 //
29 #define MAT_MUL_T(_i, _k, _j, C, A, B) { \
30  int l,c,m; \
31  for (l=0; l<_i; l++) \
32  for (c=0; c<_j; c++) { \
33  C[l][c] = 0.; \
34  for (m=0; m<_k; m++) \
35  C[l][c] += A[l][m]*B[c][m]; \
36  } \
37  }
38
39
40 //
41 // C = A-B
42 //
43 #define MAT_SUB(_i, _j, C, A, B) { \
44  int l,c; \
45  for (l=0; l<_i; l++) \
46  for (c=0; c<_j; c++) \
47  C[l][c] = A[l][c] - B[l][c]; \
48  }
49
50
51
52
53 //
54 // invS = 1/det(S) com(S)'
55 //
56 #define MAT_INV33(_invS, _S) { \
57  const float m00 = _S[1][1]*_S[2][2] - _S[1][2]*_S[2][1]; \
58  const float m10 = _S[0][1]*_S[2][2] - _S[0][2]*_S[2][1]; \
59  const float m20 = _S[0][1]*_S[1][2] - _S[0][2]*_S[1][1]; \
60  const float m01 = _S[1][0]*_S[2][2] - _S[1][2]*_S[2][0]; \
61  const float m11 = _S[0][0]*_S[2][2] - _S[0][2]*_S[2][0]; \
62  const float m21 = _S[0][0]*_S[1][2] - _S[0][2]*_S[1][0]; \
63  const float m02 = _S[1][0]*_S[2][1] - _S[1][1]*_S[2][0]; \
64  const float m12 = _S[0][0]*_S[2][1] - _S[0][1]*_S[2][0]; \
65  const float m22 = _S[0][0]*_S[1][1] - _S[0][1]*_S[1][0]; \
66  float det = _S[0][0]*m00 - _S[1][0]*m10 + _S[2][0]*m20; \
67  if (fabs(det) < FLT_EPSILON) { \
68  /* If the determinant is too small then set it to epsilon preserving sign. */ \
69  warn_message("warning: %s:%d MAT_INV33 trying to invert non-invertable matrix '%s' and put result in '%s'.\n", __FILE__, __LINE__, #_S, #_invS); \
70  det = copysignf(FLT_EPSILON, det); \
71  } \
72  _invS[0][0] = m00 / det; \
73  _invS[1][0] = -m01 / det; \
74  _invS[2][0] = m02 / det; \
75  _invS[0][1] = -m10 / det; \
76  _invS[1][1] = m11 / det; \
77  _invS[2][1] = -m12 / det; \
78  _invS[0][2] = m20 / det; \
79  _invS[1][2] = -m21 / det; \
80  _invS[2][2] = m22 / det; \
81  }
82
83
84 #endif /* PPRZ_SIMPLE_MATRIX_H */
85