Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
40extern "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 */