Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
pprz_simple_matrix.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009-2014 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
27 #ifndef PPRZ_SIMPLE_MATRIX_H
28 #define PPRZ_SIMPLE_MATRIX_H
29 
30 #ifdef __cplusplus
31 extern "C" {
32 #endif
33 
34 #include <float.h> /* for FLT_EPSILON */
35 #include <math.h>
36 #ifdef HAVE_STDIO
37 #include <stdio.h> /* for printf'ing warnings */
38 #define warn_message printf
39 #else
40 #define warn_message(...) do { } while(0)
41 #endif
42 
43 //
44 // C = A*B A:(i,k) B:(k,j) C:(i,j)
45 //
46 #define MAT_MUL(_i, _k, _j, C, A, B) { \
47  int l,c,m; \
48  for (l=0; l<_i; l++) \
49  for (c=0; c<_j; c++) { \
50  C[l][c] = 0.; \
51  for (m=0; m<_k; m++) \
52  C[l][c] += A[l][m]*B[m][c]; \
53  } \
54  }
55 
56 //
57 // C = A*B' A:(i,k) B:(j,k) C:(i,j)
58 //
59 #define MAT_MUL_T(_i, _k, _j, C, A, B) { \
60  int l,c,m; \
61  for (l=0; l<_i; l++) \
62  for (c=0; c<_j; c++) { \
63  C[l][c] = 0.; \
64  for (m=0; m<_k; m++) \
65  C[l][c] += A[l][m]*B[c][m]; \
66  } \
67  }
68 
69 
70 //
71 // C = A-B
72 //
73 #define MAT_SUB(_i, _j, C, A, B) { \
74  int l,c; \
75  for (l=0; l<_i; l++) \
76  for (c=0; c<_j; c++) \
77  C[l][c] = A[l][c] - B[l][c]; \
78  }
79 
86 #define MAT_MUL_VECT(_n, o, a, b) { \
87  int l,m; \
88  for (l = 0; l < _n; l++) { \
89  o[l] = 0; \
90  for (m = 0; m < _n; m++) { \
91  o[l] += a[l][m] * b[m]; \
92  } \
93  } \
94  }
95 
96 //
97 // invS = 1/det(S) com(S)'
98 //
99 #define MAT_INV33(_invS, _S) { \
100  const float m00 = _S[1][1]*_S[2][2] - _S[1][2]*_S[2][1]; \
101  const float m10 = _S[0][1]*_S[2][2] - _S[0][2]*_S[2][1]; \
102  const float m20 = _S[0][1]*_S[1][2] - _S[0][2]*_S[1][1]; \
103  const float m01 = _S[1][0]*_S[2][2] - _S[1][2]*_S[2][0]; \
104  const float m11 = _S[0][0]*_S[2][2] - _S[0][2]*_S[2][0]; \
105  const float m21 = _S[0][0]*_S[1][2] - _S[0][2]*_S[1][0]; \
106  const float m02 = _S[1][0]*_S[2][1] - _S[1][1]*_S[2][0]; \
107  const float m12 = _S[0][0]*_S[2][1] - _S[0][1]*_S[2][0]; \
108  const float m22 = _S[0][0]*_S[1][1] - _S[0][1]*_S[1][0]; \
109  float det = _S[0][0]*m00 - _S[1][0]*m10 + _S[2][0]*m20; \
110  if (fabs(det) < FLT_EPSILON) { \
111  /* If the determinant is too small then set it to epsilon preserving sign. */ \
112  warn_message("warning: %s:%d MAT_INV33 trying to invert non-invertable matrix '%s' and put result in '%s'.\n", __FILE__, __LINE__, #_S, #_invS); \
113  det = copysignf(FLT_EPSILON, det); \
114  } \
115  _invS[0][0] = m00 / det; \
116  _invS[1][0] = -m01 / det; \
117  _invS[2][0] = m02 / det; \
118  _invS[0][1] = -m10 / det; \
119  _invS[1][1] = m11 / det; \
120  _invS[2][1] = -m12 / det; \
121  _invS[0][2] = m20 / det; \
122  _invS[1][2] = -m21 / det; \
123  _invS[2][2] = m22 / det; \
124  }
125 
126 //
127 // C = A+B
128 //
129 #define MAT_SUM(_i, _j, C, A, B) { \
130  int l,c; \
131  for (l=0; l<_i; l++) \
132  for (c=0; c<_j; c++) \
133  C[l][c] = A[l][c] + B[l][c]; \
134  }
135 
136 
137 //
138 // C = A+(B*k)
139 //
140 #define MAT_SUM_c(_i, _j, C, A, B,k_) { \
141  int l,c; \
142  for (l=0; l<_i; l++) \
143  for (c=0; c<_j; c++) \
144  C[l][c] = A[l][c] + B[l][c]*k_; \
145  }
146 
147 //
148 // C = c*A*B A:(i,k) B:(k,j) C:(i,j)
149 //
150 #define MAT_MUL_c(_i, _k, _j, C, A, B,c_) { \
151  int l,c,m; \
152  for (l=0; l<_i; l++) \
153  for (c=0; c<_j; c++) { \
154  C[l][c] = 0.; \
155  for (m=0; m<_k; m++) \
156  C[l][c] += c_*A[l][m]*B[m][c]; \
157  } \
158  }
159 
160 
161 #define MAT_PRINT(_i, _j,A) { \
162  int l,c; \
163  printf("float "); \
164  printf(#A); \
165  printf(" = {\n"); \
166  for (l=0; l<_i; l++){ \
167  printf("{"); \
168  for (c=0; c<_j; c++){ \
169  if(c<(_j-1)){printf("%f,",A[l][c]);} \
170  else{printf("%f",A[l][c]);} \
171  } \
172  if(l<(_i-1)){ \
173  printf("} ,\n");} \
174  else{printf("}\n");} \
175  } \
176  printf("}\n"); \
177  }
178 
179 #ifdef __cplusplus
180 } /* extern "C" */
181 #endif
182 
183 #endif /* PPRZ_SIMPLE_MATRIX_H */