Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
pprz_algebra.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2011 Antoine Drouin
3  * 2008-2014 The Paparazzi Team
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, see
19  * <http://www.gnu.org/licenses/>.
20  */
21 
36 #ifndef PPRZ_ALGEBRA_H
37 #define PPRZ_ALGEBRA_H
38 
39 #ifdef __cplusplus
40 extern "C" {
41 #endif
42 
43 #include <float.h> /* for FLT_EPSILON */
44 #include <string.h> /* for memcpy */
45 #include "std.h" /* for ABS */
46 #include "math.h" /* for log and fabs */
47 
48 #define SQUARE(_a) ((_a)*(_a))
49 
50 //
51 //
52 // Vector algebra
53 //
54 //
55 
56 
57 /*
58  * Dimension 2 vectors
59  */
60 
61 /* a = {x, y} */
62 #define VECT2_ASSIGN(_a, _x, _y) { \
63  (_a).x = (_x); \
64  (_a).y = (_y); \
65  }
66 
67 /* a = b */
68 #define VECT2_COPY(_a, _b) { \
69  (_a).x = (_b).x; \
70  (_a).y = (_b).y; \
71  }
72 
73 /* a += b */
74 #define VECT2_ADD(_a, _b) { \
75  (_a).x += (_b).x; \
76  (_a).y += (_b).y; \
77  }
78 
79 /* a -= b */
80 #define VECT2_SUB(_a, _b) { \
81  (_a).x -= (_b).x; \
82  (_a).y -= (_b).y; \
83  }
84 
85 /* c = a + b */
86 #define VECT2_SUM(_c, _a, _b) { \
87  (_c).x = (_a).x + (_b).x; \
88  (_c).y = (_a).y + (_b).y; \
89  }
90 
91 /* c = a - b */
92 #define VECT2_DIFF(_c, _a, _b) { \
93  (_c).x = (_a).x - (_b).x; \
94  (_c).y = (_a).y - (_b).y; \
95  }
96 
97 /* _vo = _vi * _s */
98 #define VECT2_SMUL(_vo, _vi, _s) { \
99  (_vo).x = (_vi).x * (_s); \
100  (_vo).y = (_vi).y * (_s); \
101  }
102 
103 /* _vo = _vi / _s */
104 #define VECT2_SDIV(_vo, _vi, _s) { \
105  (_vo).x = (_vi).x / (_s); \
106  (_vo).y = (_vi).y / (_s); \
107  }
108 
109 /* _v = Bound(_v, _min, _max) */
110 #define VECT2_STRIM(_v, _min, _max) { \
111  (_v).x = (_v).x < _min ? _min : (_v).x > _max ? _max : (_v).x; \
112  (_v).y = (_v).y < _min ? _min : (_v).y > _max ? _max : (_v).y; \
113  }
114 
115 /* _vo=v1*v2 */
116 #define VECT2_DOT_PRODUCT(_v1, _v2) ((_v1).x*(_v2).x + (_v1).y*(_v2).y)
117 
118 #define VECT2_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y)
119 
120 /*
121  * Dimension 3 vectors
122  */
123 
124 /* a = {x, y, z} */
125 #define VECT3_ASSIGN(_a, _x, _y, _z) { \
126  (_a).x = (_x); \
127  (_a).y = (_y); \
128  (_a).z = (_z); \
129  }
130 
131 
132 /* a = {abs(x), abs(y), abs(z)} */
133 #define VECT3_ASSIGN_ABS(_a, _x, _y, _z) { \
134  (_a).x = ABS(_x); \
135  (_a).y = ABS(_y); \
136  (_a).z = ABS(_z); \
137  }
138 
139 /* a = b */
140 #define VECT3_COPY(_a, _b) { \
141  (_a).x = (_b).x; \
142  (_a).y = (_b).y; \
143  (_a).z = (_b).z; \
144  }
145 
146 /* a += b */
147 #define VECT3_ADD(_a, _b) { \
148  (_a).x += (_b).x; \
149  (_a).y += (_b).y; \
150  (_a).z += (_b).z; \
151  }
152 
153 /* a -= b */
154 #define VECT3_SUB(_a, _b) { \
155  (_a).x -= (_b).x; \
156  (_a).y -= (_b).y; \
157  (_a).z -= (_b).z; \
158  }
159 
160 /* c = a + b */
161 #define VECT3_SUM(_c, _a, _b) { \
162  (_c).x = (_a).x + (_b).x; \
163  (_c).y = (_a).y + (_b).y; \
164  (_c).z = (_a).z + (_b).z; \
165  }
166 
167 /* a += b*s */
168 #define VECT3_ADD_SCALED(_a, _b, _s) { \
169  (_a).x += ((_b).x * (_s)); \
170  (_a).y += ((_b).y * (_s)); \
171  (_a).z += ((_b).z * (_s)); \
172  }
173 
174 /* c = a + _s * b */
175 #define VECT3_SUM_SCALED(_c, _a, _b, _s) { \
176  (_c).x = (_a).x + (_s)*(_b).x; \
177  (_c).y = (_a).y + (_s)*(_b).y; \
178  (_c).z = (_a).z + (_s)*(_b).z; \
179  }
180 
181 /* c = a - b */
182 #define VECT3_DIFF(_c, _a, _b) { \
183  (_c).x = (_a).x - (_b).x; \
184  (_c).y = (_a).y - (_b).y; \
185  (_c).z = (_a).z - (_b).z; \
186  }
187 
188 /* _vo = _vi * _s */
189 #define VECT3_SMUL(_vo, _vi, _s) { \
190  (_vo).x = (_vi).x * (_s); \
191  (_vo).y = (_vi).y * (_s); \
192  (_vo).z = (_vi).z * (_s); \
193  }
194 
195 /* _vo = _vi / _s */
196 #define VECT3_SDIV(_vo, _vi, _s) { \
197  (_vo).x = (_vi).x / (_s); \
198  (_vo).y = (_vi).y / (_s); \
199  (_vo).z = (_vi).z / (_s); \
200  }
201 
202 /* _v = Bound(_v, _min, _max) */
203 #define VECT3_STRIM(_v, _min, _max) { \
204  (_v).x = (_v).x < _min ? _min : (_v).x > _max ? _max : (_v).x; \
205  (_v).y = (_v).y < _min ? _min : (_v).y > _max ? _max : (_v).y; \
206  (_v).z = (_v).z < _min ? _min : (_v).z > _max ? _max : (_v).z; \
207  }
208 
209 /* */
210 #define VECT3_EW_DIV(_vo, _va, _vb) { \
211  (_vo).x = (_va).x / (_vb).x; \
212  (_vo).y = (_va).y / (_vb).y; \
213  (_vo).z = (_va).z / (_vb).z; \
214  }
215 
216 /* */
217 #define VECT3_EW_MUL(_vo, _va, _vb) { \
218  (_vo).x = (_va).x * (_vb).x; \
219  (_vo).y = (_va).y * (_vb).y; \
220  (_vo).z = (_va).z * (_vb).z; \
221  }
222 
223 /* */
224 #define VECT3_BOUND_CUBE(_v, _min, _max) { \
225  if ((_v).x > (_max)) (_v).x = (_max); else if ((_v).x < (_min)) (_v).x = (_min); \
226  if ((_v).y > (_max)) (_v).y = (_max); else if ((_v).y < (_min)) (_v).y = (_min); \
227  if ((_v).z > (_max)) (_v).z = (_max); else if ((_v).z < (_min)) (_v).z = (_min); \
228  }
229 
230 /* */
231 #define VECT3_BOUND_BOX(_v, _v_min, _v_max) { \
232  if ((_v).x > (_v_max).x) (_v).x = (_v_max).x; else if ((_v).x < (_v_min).x) (_v).x = (_v_min).x; \
233  if ((_v).y > (_v_max).y) (_v).y = (_v_max).y; else if ((_v).y < (_v_min).y) (_v).y = (_v_min).y; \
234  if ((_v).z > (_v_max).z) (_v).z = (_v_max).z; else if ((_v).z < (_v_min).z) (_v).z = (_v_min).z; \
235  }
236 
237 /* */
238 #define VECT3_ABS(_vo, _vi) { \
239  (_vo).x = ABS((_vi).x); \
240  (_vo).y = ABS((_vi).y); \
241  (_vo).z = ABS((_vi).z); \
242  }
243 
244 #define VECT3_CROSS_PRODUCT(_vo, _v1, _v2) { \
245  (_vo).x = (_v1).y*(_v2).z - (_v1).z*(_v2).y; \
246  (_vo).y = (_v1).z*(_v2).x - (_v1).x*(_v2).z; \
247  (_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x; \
248  }
249 
250 #define VECT3_DOT_PRODUCT(_v1, _v2) ((_v1).x*(_v2).x + (_v1).y*(_v2).y + (_v1).z*(_v2).z)
251 
252 #define VECT3_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z)
253 
254 #define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2) { \
255  (_vo).x = (_r1).q*(_v2).z - (_r1).r*(_v2).y; \
256  (_vo).y = (_r1).r*(_v2).x - (_r1).p*(_v2).z; \
257  (_vo).z = (_r1).p*(_v2).y - (_r1).q*(_v2).x; \
258  }
259 
260 
261 //
262 //
263 // Euler angles
264 //
265 //
266 
267 
268 #define EULERS_COPY(_a, _b) { \
269  (_a).phi = (_b).phi; \
270  (_a).theta = (_b).theta; \
271  (_a).psi = (_b).psi; \
272  }
273 
274 #define EULERS_ASSIGN(_e, _phi, _theta, _psi) { \
275  (_e).phi = (_phi); \
276  (_e).theta = (_theta); \
277  (_e).psi = (_psi); \
278  }
279 
280 /* a += b */
281 #define EULERS_ADD(_a, _b) { \
282  (_a).phi += (_b).phi; \
283  (_a).theta += (_b).theta; \
284  (_a).psi += (_b).psi; \
285  }
286 
287 /* a += b */
288 #define EULERS_SUB(_a, _b) { \
289  (_a).phi -= (_b).phi; \
290  (_a).theta -= (_b).theta; \
291  (_a).psi -= (_b).psi; \
292  }
293 
294 /* c = a - b */
295 #define EULERS_DIFF(_c, _a, _b) { \
296  (_c).phi = (_a).phi - (_b).phi; \
297  (_c).theta = (_a).theta - (_b).theta; \
298  (_c).psi = (_a).psi - (_b).psi; \
299  }
300 
301 
302 /* _vo = _vi * _s */
303 #define EULERS_SMUL(_eo, _ei, _s) { \
304  (_eo).phi = (_ei).phi * (_s); \
305  (_eo).theta = (_ei).theta * (_s); \
306  (_eo).psi = (_ei).psi * (_s); \
307  }
308 
309 /* _vo = _vi / _s */
310 #define EULERS_SDIV(_eo, _ei, _s) { \
311  (_eo).phi = (_ei).phi / (_s); \
312  (_eo).theta = (_ei).theta / (_s); \
313  (_eo).psi = (_ei).psi / (_s); \
314  }
315 
316 /* _v = Bound(_v, _min, _max) */
317 #define EULERS_BOUND_CUBE(_v, _min, _max) { \
318  (_v).phi = (_v).phi < (_min) ? (_min) : (_v).phi > (_max) ? (_max) : (_v).phi; \
319  (_v).theta = (_v).theta < (_min) ? (_min) : (_v).theta > (_max) ? (_max) : (_v).theta; \
320  (_v).psi = (_v).psi < (_min) ? (_min) : (_v).psi > (_max) ? (_max) : (_v).psi; \
321  }
322 
323 //
324 //
325 // Rates
326 //
327 //
328 
329 /* ra = {p, q, r} */
330 #define RATES_ASSIGN(_ra, _p, _q, _r) { \
331  (_ra).p = (_p); \
332  (_ra).q = (_q); \
333  (_ra).r = (_r); \
334  }
335 
336 /* a = b */
337 #define RATES_COPY(_a, _b) { \
338  (_a).p = (_b).p; \
339  (_a).q = (_b).q; \
340  (_a).r = (_b).r; \
341  }
342 
343 /* a += b */
344 #define RATES_ADD(_a, _b) { \
345  (_a).p += (_b).p; \
346  (_a).q += (_b).q; \
347  (_a).r += (_b).r; \
348  }
349 
350 /* a -= b */
351 #define RATES_SUB(_a, _b) { \
352  (_a).p -= (_b).p; \
353  (_a).q -= (_b).q; \
354  (_a).r -= (_b).r; \
355  }
356 
357 /* c = a + b */
358 #define RATES_SUM(_c, _a, _b) { \
359  (_c).p = (_a).p + (_b).p; \
360  (_c).q = (_a).q + (_b).q; \
361  (_c).r = (_a).r + (_b).r; \
362  }
363 
364 /* c = a + _s * b */
365 #define RATES_SUM_SCALED(_c, _a, _b, _s) { \
366  (_c).p = (_a).p + (_s)*(_b).p; \
367  (_c).q = (_a).q + (_s)*(_b).q; \
368  (_c).r = (_a).r + (_s)*(_b).r; \
369  }
370 
371 /* c = a - b */
372 #define RATES_DIFF(_c, _a, _b) { \
373  (_c).p = (_a).p - (_b).p; \
374  (_c).q = (_a).q - (_b).q; \
375  (_c).r = (_a).r - (_b).r; \
376  }
377 
378 /* _ro = _ri * _s */
379 #define RATES_SMUL(_ro, _ri, _s) { \
380  (_ro).p = (_ri).p * (_s); \
381  (_ro).q = (_ri).q * (_s); \
382  (_ro).r = (_ri).r * (_s); \
383  }
384 
385 /* _ro = _ri / _s */
386 #define RATES_SDIV(_ro, _ri, _s) { \
387  (_ro).p = (_ri).p / (_s) ; \
388  (_ro).q = (_ri).q / (_s); \
389  (_ro).r = (_ri).r / (_s); \
390  }
391 
392 /* Element wise vector multiplication */
393 #define RATES_EWMULT_RSHIFT(c, a, b, _s) { \
394  (c).p = ((a).p * (b).p) >> (_s); \
395  (c).q = ((a).q * (b).q) >> (_s); \
396  (c).r = ((a).r * (b).r) >> (_s); \
397  }
398 
399 
400 /* _v = Bound(_v, _min, _max) */
401 #define RATES_BOUND_CUBE(_v, _min, _max) { \
402  (_v).p = (_v).p < (_min) ? (_min) : (_v).p > (_max) ? (_max) : (_v).p; \
403  (_v).q = (_v).q < (_min) ? (_min) : (_v).q > (_max) ? (_max) : (_v).q; \
404  (_v).r = (_v).r < (_min) ? (_min) : (_v).r > (_max) ? (_max) : (_v).r; \
405  }
406 
407 #define RATES_BOUND_BOX(_v, _v_min, _v_max) { \
408  if ((_v).p > (_v_max).p) (_v).p = (_v_max).p; else if ((_v).p < (_v_min).p) (_v).p = (_v_min).p; \
409  if ((_v).q > (_v_max).q) (_v).q = (_v_max).q; else if ((_v).q < (_v_min).q) (_v).q = (_v_min).q; \
410  if ((_v).r > (_v_max).r) (_v).r = (_v_max).r; else if ((_v).r < (_v_min).r) (_v).r = (_v_min).r; \
411  }
412 
413 #define RATES_BOUND_BOX_ABS(_v, _v_max) { \
414  if ((_v).p > (_v_max).p) (_v).p = (_v_max).p; else if ((_v).p < -(_v_max).p) (_v).p = -(_v_max).p; \
415  if ((_v).q > (_v_max).q) (_v).q = (_v_max).q; else if ((_v).q < -(_v_max).q) (_v).q = -(_v_max).q; \
416  if ((_v).r > (_v_max).r) (_v).r = (_v_max).r; else if ((_v).r < -(_v_max).r) (_v).r = -(_v_max).r; \
417  }
418 
419 #define RATES_ADD_SCALED_VECT(_ro, _v, _s) { \
420  (_ro).p += (_v).x * (_s); \
421  (_ro).q += (_v).y * (_s); \
422  (_ro).r += (_v).z * (_s); \
423  }
424 
425 //
426 //
427 // Matrix
428 //
429 //
430 
431 
432 /*
433  * 3x3 matrices
434  */
435 /* accessor : row and col range from 0 to 2 */
436 #define MAT33_ELMT(_m, _row, _col) ((_m).m[(_row)*3+(_col)])
437 
438 #define MAT33_COPY(_mat1,_mat2) { \
439  MAT33_ELMT((_mat1),0,0) = MAT33_ELMT((_mat2),0,0); \
440  MAT33_ELMT((_mat1),0,1) = MAT33_ELMT((_mat2),0,1); \
441  MAT33_ELMT((_mat1),0,2) = MAT33_ELMT((_mat2),0,2); \
442  MAT33_ELMT((_mat1),1,0) = MAT33_ELMT((_mat2),1,0); \
443  MAT33_ELMT((_mat1),1,1) = MAT33_ELMT((_mat2),1,1); \
444  MAT33_ELMT((_mat1),1,2) = MAT33_ELMT((_mat2),1,2); \
445  MAT33_ELMT((_mat1),2,0) = MAT33_ELMT((_mat2),2,0); \
446  MAT33_ELMT((_mat1),2,1) = MAT33_ELMT((_mat2),2,1); \
447  MAT33_ELMT((_mat1),2,2) = MAT33_ELMT((_mat2),2,2); \
448  }
449 
450 #define MAT33_MULT_SCALAR(_mat1,_scalar) { \
451  MAT33_ELMT((_mat1),0,0) = MAT33_ELMT((_mat1),0,0)*_scalar; \
452  MAT33_ELMT((_mat1),0,1) = MAT33_ELMT((_mat1),0,1)*_scalar; \
453  MAT33_ELMT((_mat1),0,2) = MAT33_ELMT((_mat1),0,2)*_scalar; \
454  MAT33_ELMT((_mat1),1,0) = MAT33_ELMT((_mat1),1,0)*_scalar; \
455  MAT33_ELMT((_mat1),1,1) = MAT33_ELMT((_mat1),1,1)*_scalar; \
456  MAT33_ELMT((_mat1),1,2) = MAT33_ELMT((_mat1),1,2)*_scalar; \
457  MAT33_ELMT((_mat1),2,0) = MAT33_ELMT((_mat1),2,0)*_scalar; \
458  MAT33_ELMT((_mat1),2,1) = MAT33_ELMT((_mat1),2,1)*_scalar; \
459  MAT33_ELMT((_mat1),2,2) = MAT33_ELMT((_mat1),2,2)*_scalar; \
460  }
461 
462 /* multiply _vin by _mat, store in _vout */
463 #define MAT33_VECT3_MUL(_vout, _mat, _vin) { \
464  (_vout).x = MAT33_ELMT((_mat), 0, 0) * (_vin).x + \
465  MAT33_ELMT((_mat), 0, 1) * (_vin).y + \
466  MAT33_ELMT((_mat), 0, 2) * (_vin).z; \
467  (_vout).y = MAT33_ELMT((_mat), 1, 0) * (_vin).x + \
468  MAT33_ELMT((_mat), 1, 1) * (_vin).y + \
469  MAT33_ELMT((_mat), 1, 2) * (_vin).z; \
470  (_vout).z = MAT33_ELMT((_mat), 2, 0) * (_vin).x + \
471  MAT33_ELMT((_mat), 2, 1) * (_vin).y + \
472  MAT33_ELMT((_mat), 2, 2) * (_vin).z; \
473  }
474 
475 /* multiply _vin by transpose of _mat, store in _vout */
476 #define MAT33_VECT3_TRANSP_MUL(_vout, _mat, _vin) { \
477  (_vout).x = MAT33_ELMT((_mat), 0, 0) * (_vin).x + \
478  MAT33_ELMT((_mat), 1, 0) * (_vin).y + \
479  MAT33_ELMT((_mat), 2, 0) * (_vin).z; \
480  (_vout).y = MAT33_ELMT((_mat), 0, 1) * (_vin).x + \
481  MAT33_ELMT((_mat), 1, 1) * (_vin).y + \
482  MAT33_ELMT((_mat), 2, 1) * (_vin).z; \
483  (_vout).z = MAT33_ELMT((_mat), 0, 2) * (_vin).x + \
484  MAT33_ELMT((_mat), 1, 2) * (_vin).y + \
485  MAT33_ELMT((_mat), 2, 2) * (_vin).z; \
486  }
487 
488 /* invS = 1/det(S) com(S)' */
489 #define MAT33_INV(_minv, _m) { \
490  const float m00 = MAT33_ELMT((_m),1,1)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),1,2)*MAT33_ELMT((_m),2,1); \
491  const float m10 = MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),2,1); \
492  const float m20 = MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),1,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),1,1); \
493  const float m01 = MAT33_ELMT((_m),1,0)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),1,2)*MAT33_ELMT((_m),2,0); \
494  const float m11 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),2,0); \
495  const float m21 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),1,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),1,0); \
496  const float m02 = MAT33_ELMT((_m),1,0)*MAT33_ELMT((_m),2,1) - MAT33_ELMT((_m),1,1)*MAT33_ELMT((_m),2,0); \
497  const float m12 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),2,1) - MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),2,0); \
498  const float m22 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),1,1) - MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),1,0); \
499  const float det = MAT33_ELMT((_m),0,0)*m00 - MAT33_ELMT((_m),1,0)*m10 + MAT33_ELMT((_m),2,0)*m20; \
500  if (fabs(det) > FLT_EPSILON) { \
501  MAT33_ELMT((_minv),0,0) = m00 / det; \
502  MAT33_ELMT((_minv),1,0) = -m01 / det; \
503  MAT33_ELMT((_minv),2,0) = m02 / det; \
504  MAT33_ELMT((_minv),0,1) = -m10 / det; \
505  MAT33_ELMT((_minv),1,1) = m11 / det; \
506  MAT33_ELMT((_minv),2,1) = -m12 / det; \
507  MAT33_ELMT((_minv),0,2) = m20 / det; \
508  MAT33_ELMT((_minv),1,2) = -m21 / det; \
509  MAT33_ELMT((_minv),2,2) = m22 / det; \
510  } \
511  }
512 
513 /* set _row of _mat with _vin multiplied by scalar _s */
514 #define MAT33_ROW_VECT3_SMUL(_mat, _row, _vin, _s) { \
515  MAT33_ELMT((_mat), _row, 0) = (_vin).x * (_s); \
516  MAT33_ELMT((_mat), _row, 1) = (_vin).y * (_s); \
517  MAT33_ELMT((_mat), _row, 2) = (_vin).z * (_s); \
518  }
519 
520 /* outer product of _v_a and _v_b, resulting in a 3x3 matrix */
521 #define VECT3_VECT3_TRANS_MUL(_mat, _v_a, _v_b) { \
522  MAT33_ELMT((_mat),0,0) = (_v_a).x*(_v_b).x; \
523  MAT33_ELMT((_mat),0,1) = (_v_a).x*(_v_b).y; \
524  MAT33_ELMT((_mat),0,2) = (_v_a).x*(_v_b).z; \
525  MAT33_ELMT((_mat),1,0) = (_v_a).y*(_v_b).x; \
526  MAT33_ELMT((_mat),1,1) = (_v_a).y*(_v_b).y; \
527  MAT33_ELMT((_mat),1,2) = (_v_a).y*(_v_b).z; \
528  MAT33_ELMT((_mat),2,0) = (_v_a).z*(_v_b).x; \
529  MAT33_ELMT((_mat),2,1) = (_v_a).z*(_v_b).y; \
530  MAT33_ELMT((_mat),2,2) = (_v_a).z*(_v_b).z; \
531  }
532 
533 /* elementwise subtraction of two 3x3 matrices */
534 #define MAT33_MAT33_DIFF(_mat1, _mat2, _mat3) { \
535  MAT33_ELMT((_mat1),0,0) = MAT33_ELMT((_mat2),0,0)-MAT33_ELMT((_mat3),0,0); \
536  MAT33_ELMT((_mat1),0,1) = MAT33_ELMT((_mat2),0,1)-MAT33_ELMT((_mat3),0,1); \
537  MAT33_ELMT((_mat1),0,2) = MAT33_ELMT((_mat2),0,2)-MAT33_ELMT((_mat3),0,2); \
538  MAT33_ELMT((_mat1),1,0) = MAT33_ELMT((_mat2),1,0)-MAT33_ELMT((_mat3),1,0); \
539  MAT33_ELMT((_mat1),1,1) = MAT33_ELMT((_mat2),1,1)-MAT33_ELMT((_mat3),1,1); \
540  MAT33_ELMT((_mat1),1,2) = MAT33_ELMT((_mat2),1,2)-MAT33_ELMT((_mat3),1,2); \
541  MAT33_ELMT((_mat1),2,0) = MAT33_ELMT((_mat2),2,0)-MAT33_ELMT((_mat3),2,0); \
542  MAT33_ELMT((_mat1),2,1) = MAT33_ELMT((_mat2),2,1)-MAT33_ELMT((_mat3),2,1); \
543  MAT33_ELMT((_mat1),2,2) = MAT33_ELMT((_mat2),2,2)-MAT33_ELMT((_mat3),2,2); \
544  }
545 
546 /* elementwise addition of two 3x3 matrices */
547 #define MAT33_MAT33_SUM(_mat1, _mat2, _mat3) { \
548  MAT33_ELMT((_mat1),0,0) = MAT33_ELMT((_mat2),0,0)+MAT33_ELMT((_mat3),0,0); \
549  MAT33_ELMT((_mat1),0,1) = MAT33_ELMT((_mat2),0,1)+MAT33_ELMT((_mat3),0,1); \
550  MAT33_ELMT((_mat1),0,2) = MAT33_ELMT((_mat2),0,2)+MAT33_ELMT((_mat3),0,2); \
551  MAT33_ELMT((_mat1),1,0) = MAT33_ELMT((_mat2),1,0)+MAT33_ELMT((_mat3),1,0); \
552  MAT33_ELMT((_mat1),1,1) = MAT33_ELMT((_mat2),1,1)+MAT33_ELMT((_mat3),1,1); \
553  MAT33_ELMT((_mat1),1,2) = MAT33_ELMT((_mat2),1,2)+MAT33_ELMT((_mat3),1,2); \
554  MAT33_ELMT((_mat1),2,0) = MAT33_ELMT((_mat2),2,0)+MAT33_ELMT((_mat3),2,0); \
555  MAT33_ELMT((_mat1),2,1) = MAT33_ELMT((_mat2),2,1)+MAT33_ELMT((_mat3),2,1); \
556  MAT33_ELMT((_mat1),2,2) = MAT33_ELMT((_mat2),2,2)+MAT33_ELMT((_mat3),2,2); \
557  }
558 
559 /* transpose of a 3x3 matrix */
560 #define MAT33_TRANS(_mat1,_mat2) { \
561  MAT33_ELMT((_mat1),0,0) = MAT33_ELMT((_mat2),0,0); \
562  MAT33_ELMT((_mat1),0,1) = MAT33_ELMT((_mat2),1,0); \
563  MAT33_ELMT((_mat1),0,2) = MAT33_ELMT((_mat2),2,0); \
564  MAT33_ELMT((_mat1),1,0) = MAT33_ELMT((_mat2),0,1); \
565  MAT33_ELMT((_mat1),1,1) = MAT33_ELMT((_mat2),1,1); \
566  MAT33_ELMT((_mat1),1,2) = MAT33_ELMT((_mat2),2,1); \
567  MAT33_ELMT((_mat1),2,0) = MAT33_ELMT((_mat2),0,2); \
568  MAT33_ELMT((_mat1),2,1) = MAT33_ELMT((_mat2),1,2); \
569  MAT33_ELMT((_mat1),2,2) = MAT33_ELMT((_mat2),2,2); \
570  }
571 
572 
573 //
574 //
575 // Quaternion algebras
576 //
577 //
578 
579 /* _q = [_i _x _y _z] */
580 #define QUAT_ASSIGN(_q, _i, _x, _y, _z) { \
581  (_q).qi = (_i); \
582  (_q).qx = (_x); \
583  (_q).qy = (_y); \
584  (_q).qz = (_z); \
585  }
586 
587 /* _qc = _qa - _qb */
588 #define QUAT_DIFF(_qc, _qa, _qb) { \
589  (_qc).qi = (_qa).qi - (_qb).qi; \
590  (_qc).qx = (_qa).qx - (_qb).qx; \
591  (_qc).qy = (_qa).qy - (_qb).qy; \
592  (_qc).qz = (_qa).qz - (_qb).qz; \
593  }
594 
595 /* _qo = _qi */
596 #define QUAT_COPY(_qo, _qi) { \
597  (_qo).qi = (_qi).qi; \
598  (_qo).qx = (_qi).qx; \
599  (_qo).qy = (_qi).qy; \
600  (_qo).qz = (_qi).qz; \
601  }
602 
603 #define QUAT_EXPLEMENTARY(b,a) { \
604  (b).qi = -(a).qi; \
605  (b).qx = -(a).qx; \
606  (b).qy = -(a).qy; \
607  (b).qz = -(a).qz; \
608  }
609 
610 /* _qo = _qi * _s */
611 #define QUAT_SMUL(_qo, _qi, _s) { \
612  (_qo).qi = (_qi).qi * (_s); \
613  (_qo).qx = (_qi).qx * (_s); \
614  (_qo).qy = (_qi).qy * (_s); \
615  (_qo).qz = (_qi).qz * (_s); \
616  }
617 
618 /* _qo = _qo + _qi */
619 #define QUAT_ADD(_qo, _qi) { \
620  (_qo).qi += (_qi).qi; \
621  (_qo).qx += (_qi).qx; \
622  (_qo).qy += (_qi).qy; \
623  (_qo).qz += (_qi).qz; \
624  }
625 
626 /* _qo = [qi -qx -qy -qz] */
627 #define QUAT_INVERT(_qo, _qi) { \
628  (_qo).qi = (_qi).qi; \
629  (_qo).qx = -(_qi).qx; \
630  (_qo).qy = -(_qi).qy; \
631  (_qo).qz = -(_qi).qz; \
632  }
633 
634 /* _vo=[ qx qy qz] */
635 #define QUAT_EXTRACT_Q(_vo, _qi) { \
636  (_vo).x=(_qi).qx; \
637  (_vo).y=(_qi).qy; \
638  (_vo).z=(_qi).qz; \
639  }
640 
641 /* _qo = _qo / _s */
642 #define QUAT_SDIV(_qo, _qi, _s) { \
643  (_qo).qi = (_qi).qi / (_s); \
644  (_qo).qx = (_qi).qx / (_s); \
645  (_qo).qy = (_qi).qy / (_s); \
646  (_qo).qz = (_qi).qz / (_s); \
647  }
648 
649 /* return = _qa * _qb */
650 #define QUAT_DOT_PRODUCT(_qa, _qb) ((_qa).qi * (_qb).qi + (_qa).qx * (_qb).qx + (_qa).qy * (_qb).qy + (_qa).qz * (_qb).qz)
651 
652 //
653 //
654 // Rotation Matrices
655 //
656 //
657 
658 
659 /* accessor : row and col range from 0 to 2 */
660 #define RMAT_ELMT(_rm, _row, _col) MAT33_ELMT(_rm, _row, _col)
661 
662 /* trace */
663 #define RMAT_TRACE(_rm) (RMAT_ELMT(_rm, 0, 0)+RMAT_ELMT(_rm, 1, 1)+RMAT_ELMT(_rm, 2, 2))
664 
665 
666 #define RMAT_DIFF(_c, _a, _b) { \
667  (_c).m[0] = (_a).m[0] - (_b).m[0]; \
668  (_c).m[1] = (_a).m[1] - (_b).m[1]; \
669  (_c).m[2] = (_a).m[2] - (_b).m[2]; \
670  (_c).m[3] = (_a).m[3] - (_b).m[3]; \
671  (_c).m[4] = (_a).m[4] - (_b).m[4]; \
672  (_c).m[5] = (_a).m[5] - (_b).m[5]; \
673  (_c).m[6] = (_a).m[6] - (_b).m[6]; \
674  (_c).m[7] = (_a).m[7] - (_b).m[7]; \
675  (_c).m[8] = (_a).m[8] - (_b).m[8]; \
676  }
677 
678 /* multiply _vin by _rmat, store in _vout */
679 #define RMAT_VECT3_MUL(_vout, _rmat, _vin) { \
680  (_vout).x = RMAT_ELMT((_rmat), 0, 0) * (_vin).x + \
681  RMAT_ELMT((_rmat), 0, 1) * (_vin).y + \
682  RMAT_ELMT((_rmat), 0, 2) * (_vin).z; \
683  (_vout).y = RMAT_ELMT((_rmat), 1, 0) * (_vin).x + \
684  RMAT_ELMT((_rmat), 1, 1) * (_vin).y + \
685  RMAT_ELMT((_rmat), 1, 2) * (_vin).z; \
686  (_vout).z = RMAT_ELMT((_rmat), 2, 0) * (_vin).x + \
687  RMAT_ELMT((_rmat), 2, 1) * (_vin).y + \
688  RMAT_ELMT((_rmat), 2, 2) * (_vin).z; \
689  }
690 
691 #define RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin) { \
692  (_vout).x = RMAT_ELMT((_rmat), 0, 0) * (_vin).x + \
693  RMAT_ELMT((_rmat), 1, 0) * (_vin).y + \
694  RMAT_ELMT((_rmat), 2, 0) * (_vin).z; \
695  (_vout).y = RMAT_ELMT((_rmat), 0, 1) * (_vin).x + \
696  RMAT_ELMT((_rmat), 1, 1) * (_vin).y + \
697  RMAT_ELMT((_rmat), 2, 1) * (_vin).z; \
698  (_vout).z = RMAT_ELMT((_rmat), 0, 2) * (_vin).x + \
699  RMAT_ELMT((_rmat), 1, 2) * (_vin).y + \
700  RMAT_ELMT((_rmat), 2, 2) * (_vin).z; \
701  }
702 
703 
704 #define RMAT_COPY(_o, _i) { memcpy(&(_o), &(_i), sizeof(_o));}
705 
706 
707 
708 
709 #define EULERS_FLOAT_OF_BFP(_ef, _ei) { \
710  (_ef).phi = ANGLE_FLOAT_OF_BFP((_ei).phi); \
711  (_ef).theta = ANGLE_FLOAT_OF_BFP((_ei).theta); \
712  (_ef).psi = ANGLE_FLOAT_OF_BFP((_ei).psi); \
713  }
714 
715 #define EULERS_BFP_OF_REAL(_ei, _ef) { \
716  (_ei).phi = ANGLE_BFP_OF_REAL((_ef).phi); \
717  (_ei).theta = ANGLE_BFP_OF_REAL((_ef).theta); \
718  (_ei).psi = ANGLE_BFP_OF_REAL((_ef).psi); \
719  }
720 
721 #define RMAT_BFP_OF_REAL(_ei, _ef) { \
722  (_ei).m[0] = TRIG_BFP_OF_REAL((_ef).m[0]); \
723  (_ei).m[1] = TRIG_BFP_OF_REAL((_ef).m[1]); \
724  (_ei).m[2] = TRIG_BFP_OF_REAL((_ef).m[2]); \
725  (_ei).m[3] = TRIG_BFP_OF_REAL((_ef).m[3]); \
726  (_ei).m[4] = TRIG_BFP_OF_REAL((_ef).m[4]); \
727  (_ei).m[5] = TRIG_BFP_OF_REAL((_ef).m[5]); \
728  (_ei).m[6] = TRIG_BFP_OF_REAL((_ef).m[6]); \
729  (_ei).m[7] = TRIG_BFP_OF_REAL((_ef).m[7]); \
730  (_ei).m[8] = TRIG_BFP_OF_REAL((_ef).m[8]); \
731  }
732 
733 #define RMAT_FLOAT_OF_BFP(_ef, _ei) { \
734  (_ef).m[0] = TRIG_FLOAT_OF_BFP((_ei).m[0]); \
735  (_ef).m[1] = TRIG_FLOAT_OF_BFP((_ei).m[1]); \
736  (_ef).m[2] = TRIG_FLOAT_OF_BFP((_ei).m[2]); \
737  (_ef).m[3] = TRIG_FLOAT_OF_BFP((_ei).m[3]); \
738  (_ef).m[4] = TRIG_FLOAT_OF_BFP((_ei).m[4]); \
739  (_ef).m[5] = TRIG_FLOAT_OF_BFP((_ei).m[5]); \
740  (_ef).m[6] = TRIG_FLOAT_OF_BFP((_ei).m[6]); \
741  (_ef).m[7] = TRIG_FLOAT_OF_BFP((_ei).m[7]); \
742  (_ef).m[8] = TRIG_FLOAT_OF_BFP((_ei).m[8]); \
743  }
744 
745 #define QUAT_FLOAT_OF_BFP(_qf, _qi) { \
746  (_qf).qi = QUAT1_FLOAT_OF_BFP((_qi).qi); \
747  (_qf).qx = QUAT1_FLOAT_OF_BFP((_qi).qx); \
748  (_qf).qy = QUAT1_FLOAT_OF_BFP((_qi).qy); \
749  (_qf).qz = QUAT1_FLOAT_OF_BFP((_qi).qz); \
750  }
751 
752 #define QUAT_BFP_OF_REAL(_qi, _qf) { \
753  (_qi).qi = QUAT1_BFP_OF_REAL((_qf).qi); \
754  (_qi).qx = QUAT1_BFP_OF_REAL((_qf).qx); \
755  (_qi).qy = QUAT1_BFP_OF_REAL((_qf).qy); \
756  (_qi).qz = QUAT1_BFP_OF_REAL((_qf).qz); \
757  }
758 
759 #define RATES_FLOAT_OF_BFP(_rf, _ri) { \
760  (_rf).p = RATE_FLOAT_OF_BFP((_ri).p); \
761  (_rf).q = RATE_FLOAT_OF_BFP((_ri).q); \
762  (_rf).r = RATE_FLOAT_OF_BFP((_ri).r); \
763  }
764 
765 #define RATES_BFP_OF_REAL(_ri, _rf) { \
766  (_ri).p = RATE_BFP_OF_REAL((_rf).p); \
767  (_ri).q = RATE_BFP_OF_REAL((_rf).q); \
768  (_ri).r = RATE_BFP_OF_REAL((_rf).r); \
769  }
770 
771 #define POSITIONS_FLOAT_OF_BFP(_ef, _ei) { \
772  (_ef).x = POS_FLOAT_OF_BFP((_ei).x); \
773  (_ef).y = POS_FLOAT_OF_BFP((_ei).y); \
774  (_ef).z = POS_FLOAT_OF_BFP((_ei).z); \
775  }
776 
777 #define POSITIONS_BFP_OF_REAL(_ef, _ei) { \
778  (_ef).x = POS_BFP_OF_REAL((_ei).x); \
779  (_ef).y = POS_BFP_OF_REAL((_ei).y); \
780  (_ef).z = POS_BFP_OF_REAL((_ei).z); \
781  }
782 
783 #define SPEEDS_FLOAT_OF_BFP(_ef, _ei) { \
784  (_ef).x = SPEED_FLOAT_OF_BFP((_ei).x); \
785  (_ef).y = SPEED_FLOAT_OF_BFP((_ei).y); \
786  (_ef).z = SPEED_FLOAT_OF_BFP((_ei).z); \
787  }
788 
789 #define SPEEDS_BFP_OF_REAL(_ef, _ei) { \
790  (_ef).x = SPEED_BFP_OF_REAL((_ei).x); \
791  (_ef).y = SPEED_BFP_OF_REAL((_ei).y); \
792  (_ef).z = SPEED_BFP_OF_REAL((_ei).z); \
793  }
794 
795 #define ACCELS_FLOAT_OF_BFP(_ef, _ei) { \
796  (_ef).x = ACCEL_FLOAT_OF_BFP((_ei).x); \
797  (_ef).y = ACCEL_FLOAT_OF_BFP((_ei).y); \
798  (_ef).z = ACCEL_FLOAT_OF_BFP((_ei).z); \
799  }
800 
801 #define ACCELS_BFP_OF_REAL(_ef, _ei) { \
802  (_ef).x = ACCEL_BFP_OF_REAL((_ei).x); \
803  (_ef).y = ACCEL_BFP_OF_REAL((_ei).y); \
804  (_ef).z = ACCEL_BFP_OF_REAL((_ei).z); \
805  }
806 
807 #define MAGS_FLOAT_OF_BFP(_ef, _ei) { \
808  (_ef).x = MAG_FLOAT_OF_BFP((_ei).x); \
809  (_ef).y = MAG_FLOAT_OF_BFP((_ei).y); \
810  (_ef).z = MAG_FLOAT_OF_BFP((_ei).z); \
811  }
812 
813 #define MAGS_BFP_OF_REAL(_ef, _ei) { \
814  (_ef).x = MAG_BFP_OF_REAL((_ei).x); \
815  (_ef).y = MAG_BFP_OF_REAL((_ei).y); \
816  (_ef).z = MAG_BFP_OF_REAL((_ei).z); \
817  }
818 
819 #ifdef __cplusplus
820 } /* extern "C" */
821 #endif
822 
823 #endif /* PPRZ_ALGEBRA_H */