Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
pprz_algebra_float.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-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 
31 #ifndef PPRZ_ALGEBRA_FLOAT_H
32 #define PPRZ_ALGEBRA_FLOAT_H
33 
34 #ifdef __cplusplus
35 extern "C" {
36 #endif
37 
38 #include "pprz_algebra.h"
39 #include "message_pragmas.h"
40 
41 #include <math.h>
42 #include <float.h> // for FLT_MIN
43 
44 /* this seems to be missing for some arch */
45 #ifndef M_SQRT2
46 #define M_SQRT2 1.41421356237309504880
47 #endif
48 
49 struct FloatVect2 {
50  float x;
51  float y;
52 };
53 
54 struct FloatVect3 {
55  float x;
56  float y;
57  float z;
58 };
59 
63 struct FloatQuat {
64  float qi;
65  float qx;
66  float qy;
67  float qz;
68 };
69 
70 struct FloatMat33 {
71  float m[3 * 3];
72 };
73 
77 struct FloatRMat {
78  float m[3 * 3];
79 };
80 
84 struct FloatEulers {
85  float phi;
86  float theta;
87  float psi;
88 };
89 
93 struct FloatRates {
94  float p;
95  float q;
96  float r;
97 };
98 
99 #define FLOAT_ANGLE_NORMALIZE(_a) { \
100  while (_a > M_PI) _a -= (2.*M_PI); \
101  while (_a < -M_PI) _a += (2.*M_PI); \
102  }
103 
104 /*
105  * Returns the real part of the log of v in base of n
106  */
107 static inline float float_log_n(float v, float n)
108 {
109  if (fabsf(v) < 1e-4) { // avoid inf
110  return - 1.0E+30;
111  }
112  if (fabsf(n) < 1e-4) { // avoid nan
113  return 0;
114  }
115  return logf(fabsf(v)) / logf(n);
116 }
117 
118 //
119 //
120 // Vector algebra
121 //
122 //
123 
124 
125 /*
126  * Dimension 2 Vectors
127  */
128 
129 #define FLOAT_VECT2_ZERO(_v) VECT2_ASSIGN(_v, 0., 0.)
130 
131 /* macros also usable if _v is not a FloatVect2, but a different struct with x,y members */
132 #define FLOAT_VECT2_NORM(_v) sqrtf(VECT2_NORM2(_v))
133 
134 static inline float float_vect2_norm2(struct FloatVect2 *v)
135 {
136  return v->x * v->x + v->y * v->y;
137 }
138 
139 static inline float float_vect2_norm(struct FloatVect2 *v)
140 {
141  return sqrtf(float_vect2_norm2(v));
142 }
143 
145 static inline void float_vect2_normalize(struct FloatVect2 *v)
146 {
147  const float n = float_vect2_norm(v);
148  if (n > 1e-4f) {
149  v->x /= n;
150  v->y /= n;
151  }
152 }
153 
154 #define FLOAT_VECT2_NORMALIZE(_v) float_vect2_normalize(&(_v))
155 
156 
157 /*
158  * Dimension 3 Vectors
159  */
160 
161 #define FLOAT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0., 0., 0.)
162 
163 /* macros also usable if _v is not a FloatVect3, but a different struct with x,y,z members */
164 #define FLOAT_VECT3_NORM(_v) sqrtf(VECT3_NORM2(_v))
165 
166 static inline float float_vect3_norm2(struct FloatVect3 *v)
167 {
168  return v->x * v->x + v->y * v->y + v->z * v->z;
169 }
170 
171 static inline float float_vect3_norm(struct FloatVect3 *v)
172 {
173  return sqrtf(float_vect3_norm2(v));
174 }
175 
177 static inline void float_vect3_normalize(struct FloatVect3 *v)
178 {
179  const float n = float_vect3_norm(v);
180  if (n > 1e-4f) {
181  v->x /= n;
182  v->y /= n;
183  v->z /= n;
184  }
185 }
186 
187 #define FLOAT_VECT3_NORMALIZE(_v) float_vect3_normalize(&(_v))
188 
189 
190 
191 #define FLOAT_RATES_ZERO(_r) { \
192  RATES_ASSIGN(_r, 0., 0., 0.); \
193  }
194 
195 #define FLOAT_RATES_NORM(_v) (sqrtf((_v).p*(_v).p + (_v).q*(_v).q + (_v).r*(_v).r))
196 
197 #define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2) { \
198  _ro.p = _s1 * _r1.p + _s2 * _r2.p; \
199  _ro.q = _s1 * _r1.q + _s2 * _r2.q; \
200  _ro.r = _s1 * _r1.r + _s2 * _r2.r; \
201  }
202 
203 
204 extern void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv,
205  float dt);
206 
207 extern void float_rates_integrate_fi(struct FloatRates *r, struct FloatRates *dr,
208  float dt);
209 
210 extern void float_rates_of_euler_dot(struct FloatRates *r, struct FloatEulers *e,
211  struct FloatEulers *edot);
212 
213 /* defines for backwards compatibility */
214 #define FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt) WARNING("FLOAT_VECT3_INTEGRATE_FI macro is deprecated, use the lower case function instead") float_vect3_integrate_fi(&(_vo), &(_dv), _dt)
215 #define FLOAT_RATES_INTEGRATE_FI(_ra, _racc, _dt) WARNING("FLOAT_RATES_INTEGRATE_FI macro is deprecated, use the lower case function instead") float_rates_integrate_fi(&(_ra), &(_racc), _dt)
216 #define FLOAT_RATES_OF_EULER_DOT(_ra, _e, _ed) WARNING("FLOAT_RATES_OF_EULER_DOT macro is deprecated, use the lower case function instead") float_rates_of_euler_dot(&(_ra), &(_e), &(_ed))
217 
218 
219 /*
220  * 3x3 matrices
221  */
222 #define FLOAT_MAT33_ZERO(_m) { \
223  MAT33_ELMT(_m, 0, 0) = 0.; \
224  MAT33_ELMT(_m, 0, 1) = 0.; \
225  MAT33_ELMT(_m, 0, 2) = 0.; \
226  MAT33_ELMT(_m, 1, 0) = 0.; \
227  MAT33_ELMT(_m, 1, 1) = 0.; \
228  MAT33_ELMT(_m, 1, 2) = 0.; \
229  MAT33_ELMT(_m, 2, 0) = 0.; \
230  MAT33_ELMT(_m, 2, 1) = 0.; \
231  MAT33_ELMT(_m, 2, 2) = 0.; \
232  }
233 
234 #define FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22) { \
235  MAT33_ELMT(_m, 0, 0) = _d00; \
236  MAT33_ELMT(_m, 0, 1) = 0.; \
237  MAT33_ELMT(_m, 0, 2) = 0.; \
238  MAT33_ELMT(_m, 1, 0) = 0.; \
239  MAT33_ELMT(_m, 1, 1) = _d11; \
240  MAT33_ELMT(_m, 1, 2) = 0.; \
241  MAT33_ELMT(_m, 2, 0) = 0.; \
242  MAT33_ELMT(_m, 2, 1) = 0.; \
243  MAT33_ELMT(_m, 2, 2) = _d22; \
244  }
245 
246 
247 //
248 //
249 // Rotation Matrices
250 //
251 //
252 
253 
255 static inline void float_rmat_identity(struct FloatRMat *rm)
256 {
257  FLOAT_MAT33_DIAG(*rm, 1., 1., 1.);
258 }
259 
263 extern void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b);
264 
268 extern void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b,
269  struct FloatRMat *m_b2c);
270 
274 extern void float_rmat_comp_inv(struct FloatRMat *m_a2b, struct FloatRMat *m_a2c,
275  struct FloatRMat *m_b2c);
276 
278 extern float float_rmat_norm(struct FloatRMat *rm);
279 
283 extern void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b,
284  struct FloatVect3 *va);
285 
289 extern void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a,
290  struct FloatVect3 *va);
291 
295 extern void float_rmat_mult(struct FloatEulers *rb, struct FloatRMat *m_a2b,
296  struct FloatEulers *ra);
297 
301 extern void float_rmat_transp_mult(struct FloatEulers *rb, struct FloatRMat *m_b2a,
302  struct FloatEulers *ra);
303 
307 extern void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b,
308  struct FloatRates *ra);
309 
313 extern void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a,
314  struct FloatRates *ra);
315 
317 extern void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle);
318 
331 extern void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e);
332 extern void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e);
333 #define float_rmat_of_eulers float_rmat_of_eulers_321
334 
335 extern void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q);
337 extern void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt);
338 extern float float_rmat_reorthogonalize(struct FloatRMat *rm);
339 
340 /* defines for backwards compatibility */
341 #define FLOAT_RMAT_INV(_m_b2a, _m_a2b) WARNING("FLOAT_RMAT_INV macro is deprecated, use the lower case function instead") float_rmat_inv(&(_m_b2a), &(_m_a2b))
342 #define FLOAT_RMAT_NORM(_m) WARNING("FLOAT_RMAT_NORM macro is deprecated, use the lower case function instead") float_rmat_norm(&(_m))
343 #define FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c) WARNING("FLOAT_RMAT_COMP macro is deprecated, use the lower case function instead") float_rmat_comp(&(_m_a2c), &(_m_a2b), &(_m_b2c))
344 #define FLOAT_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c) WARNING("FLOAT_RMAT_COMP_INV macro is deprecated, use the lower case function instead") float_rmat_comp_inv(&(_m_a2b), &(_m_a2c), &(_m_b2c))
345 #define FLOAT_RMAT_VMULT(_vb, _m_a2b, _va) WARNING("FLOAT_RMAT_VMULT macro is deprecated, use the lower case function instead") float_rmat_vmult(&(_vb), &(_m_a2b), &(_va))
346 #define FLOAT_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va) WARNING("FLOAT_RMAT_TRANSP_VMULT macro is deprecated, use the lower case function instead") float_rmat_transp_vmult(&(_vb), &(_m_b2a), &(_va))
347 #define FLOAT_RMAT_RATEMULT(_rb, _m_a2b, _ra) WARNING("FLOAT_RMAT_RATEMULT macro is deprecated, use the lower case function instead") float_rmat_ratemult(&(_rb), &(_m_a2b), &(_ra))
348 #define FLOAT_RMAT_TRANSP_RATEMULT(_rb, _m_b2a, _ra) WARNING("FLOAT_RMAT_TRANSP_RATEMULT macro is deprecated, use the lower case function instead") float_rmat_ratemult(&(_rb), &(_m_b2a), &(_ra))
349 #define FLOAT_RMAT_OF_AXIS_ANGLE(_rm, _uv, _an) WARNING("FLOAT_RMAT_OF_AXIS_ANGLE macro is deprecated, use the lower case function instead") float_rmat_of_axis_angle(&(_rm), &(_uv), _an)
350 #define FLOAT_RMAT_OF_EULERS(_rm, _e) WARNING("FLOAT_RMAT_OF_EULERS macro is deprecated, use the lower case function instead") float_rmat_of_eulers_321(&(_rm), &(_e))
351 #define FLOAT_RMAT_OF_EULERS_321(_rm, _e) WARNING("FLOAT_RMAT_OF_EULERS_321 macro is deprecated, use the lower case function instead") float_rmat_of_eulers_321(&(_rm), &(_e))
352 #define FLOAT_RMAT_OF_EULERS_312(_rm, _e) WARNING("FLOAT_RMAT_OF_EULERS_312 macro is deprecated, use the lower case function instead") float_rmat_of_eulers_312(&(_rm), &(_e))
353 #define FLOAT_RMAT_OF_QUAT(_rm, _q) WARNING("FLOAT_RMAT_OF_QUAT macro is deprecated, use the lower case function instead") float_rmat_of_quat(&(_rm), &(_q))
354 #define FLOAT_RMAT_INTEGRATE_FI(_rm, _omega, _dt) WARNING("FLOAT_RMAT_INTEGRATE_FI macro is deprecated, use the lower case function instead") float_rmat_integrate_fi(&(_rm), &(_omega), &(_dt))
355 
356 
357 
358 //
359 //
360 // Quaternion algebras
361 //
362 //
363 
365 static inline void float_quat_identity(struct FloatQuat *q)
366 {
367  q->qi = 1.0;
368  q->qx = 0;
369  q->qy = 0;
370  q->qz = 0;
371 }
372 
373 #define FLOAT_QUAT_NORM2(_q) (SQUARE((_q).qi) + SQUARE((_q).qx) + SQUARE((_q).qy) + SQUARE((_q).qz))
374 
375 static inline float float_quat_norm(struct FloatQuat *q)
376 {
377  return sqrtf(SQUARE(q->qi) + SQUARE(q->qx) + SQUARE(q->qy) + SQUARE(q->qz));
378 }
379 
380 static inline void float_quat_normalize(struct FloatQuat *q)
381 {
382  float qnorm = float_quat_norm(q);
383  if (qnorm > FLT_MIN) {
384  q->qi = q->qi / qnorm;
385  q->qx = q->qx / qnorm;
386  q->qy = q->qy / qnorm;
387  q->qz = q->qz / qnorm;
388  }
389 }
390 
391 static inline void float_quat_invert(struct FloatQuat *qo, struct FloatQuat *qi)
392 {
393  QUAT_INVERT(*qo, *qi);
394 }
395 
396 static inline void float_quat_wrap_shortest(struct FloatQuat *q)
397 {
398  if (q->qi < 0.) {
399  QUAT_EXPLEMENTARY(*q, *q);
400  }
401 }
402 
403 #define FLOAT_QUAT_EXTRACT(_vo, _qi) QUAT_EXTRACT_Q(_vo, _qi)
404 
405 
409 extern void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c);
410 
414 extern void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c);
415 
419 extern void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c);
420 
424 extern void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c);
425 
429 extern void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c);
430 
434 extern void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c);
435 
441 extern void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q);
442 
448 extern void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q);
449 
452 extern void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt);
453 
455 extern void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, float dt);
456 
458 extern void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt);
459 
463 extern void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in);
464 
466 extern void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e);
467 extern void float_quat_of_eulers_zxy(struct FloatQuat *q, struct FloatEulers *e);
468 extern void float_quat_of_eulers_yxz(struct FloatQuat *q, struct FloatEulers *e);
469 
474 extern void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle);
475 
479 extern void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov);
480 
482 extern void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm);
483 
485 extern void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, struct FloatQuat *quat);
486 
487 
488 /* defines for backwards compatibility */
489 #define FLOAT_QUAT_ZERO(_q) WARNING("FLOAT_QUAT_ZERO macro is deprecated, use the lower case function instead") float_quat_identity(&(_q))
490 #define FLOAT_QUAT_INVERT(_qo, _qi) WARNING("FLOAT_QUAT_INVERT macro is deprecated, use the lower case function instead") float_quat_invert(&(_qo), &(_qi))
491 #define FLOAT_QUAT_WRAP_SHORTEST(_q) WARNING("FLOAT_QUAT_WRAP_SHORTEST macro is deprecated, use the lower case function instead") float_quat_wrap_shortest(&(_q))
492 #define FLOAT_QUAT_NORM(_q) WARNING("FLOAT_QUAT_NORM macro is deprecated, use the lower case function instead") float_quat_norm(&(_q))
493 #define FLOAT_QUAT_NORMALIZE(_q) WARNING("FLOAT_QUAT_NORMALIZE macro is deprecated, use the lower case function instead") float_quat_normalize(&(_q))
494 #define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c) WARNING("FLOAT_QUAT_COMP macro is deprecated, use the lower case function instead") float_quat_comp(&(_a2c), &(_a2b), &(_b2c))
495 #define FLOAT_QUAT_MULT(_a2c, _a2b, _b2c) WARNING("FLOAT_QUAT_MULT macro is deprecated, use the lower case function instead") float_quat_comp(&(_a2c), &(_a2b), &(_b2c))
496 #define FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c) WARNING("FLOAT_QUAT_INV_COMP macro is deprecated, use the lower case function instead") float_quat_inv_comp(&(_b2c), &(_a2b), &(_a2c))
497 #define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c) WARNING("FLOAT_QUAT_COMP_INV macro is deprecated, use the lower case function instead") float_quat_comp_inv(&(_a2b), &(_a2c), &(_b2c))
498 #define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) WARNING("FLOAT_QUAT_COMP_NORM_SHORTEST macro is deprecated, use the lower case function instead") float_quat_comp_norm_shortest(&(_a2c), &(_a2b), &(_b2c))
499 #define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) WARNING("FLOAT_QUAT_COMP_INV_NORM_SHORTEST macro is deprecated, use the lower case function instead") float_quat_comp_inv_norm_shortest(&(_a2b), &(_a2c), &(_b2c))
500 #define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) WARNING("FLOAT_QUAT_INV_COMP_NORM_SHORTEST macro is deprecated, use the lower case function instead") float_quat_inv_comp_norm_shortest(&(_b2c), &(_a2b), &(_a2c))
501 #define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt) WARNING("FLOAT_QUAT_DIFFERENTIAL macro is deprecated, use the lower case function instead") float_quat_differential(&(q_out), &(w), dt)
502 #define FLOAT_QUAT_INTEGRATE(_q, _omega, _dt) WARNING("FLOAT_QUAT_INTEGRATE macro is deprecated, use the lower case function instead") float_quat_integrate(&(_q), &(_omega), _dt)
503 #define FLOAT_QUAT_VMULT(v_out, q, v_in) WARNING("FLOAT_QUAT_VMULT macro is deprecated, use the lower case function instead") float_quat_vmult(&(v_out), &(q), &(v_in))
504 #define FLOAT_QUAT_DERIVATIVE(_qd, _r, _q) WARNING("FLOAT_QUAT_DERIVATIVE macro is deprecated, use the lower case function instead") float_quat_derivative(&(_qd), &(_r), &(_q))
505 #define FLOAT_QUAT_DERIVATIVE_LAGRANGE(_qd, _r, _q) WARNING("FLOAT_QUAT_DERIVATIVE_LAGRANGE macro is deprecated, use the lower case function instead") float_quat_derivative_lagrange(&(_qd), &(_r), &(_q))
506 #define FLOAT_QUAT_OF_EULERS(_q, _e) WARNING("FLOAT_QUAT_OF_EULERS macro is deprecated, use the lower case function instead") float_quat_of_eulers(&(_q), &(_e))
507 #define FLOAT_QUAT_OF_AXIS_ANGLE(_q, _uv, _an) WARNING("FLOAT_QUAT_OF_AXIS_ANGLE macro is deprecated, use the lower case function instead") float_quat_of_axis_angle(&(_q), &(_uv), _an)
508 #define FLOAT_QUAT_OF_ORIENTATION_VECT(_q, _ov) WARNING("FLOAT_QUAT_OF_ORIENTATION_VECT macro is deprecated, use the lower case function instead") float_quat_of_orientation_vect(&(_q), &(_ov))
509 #define FLOAT_QUAT_OF_RMAT(_q, _r) WARNING("FLOAT_QUAT_OF_RMAT macro is deprecated, use the lower case function instead") float_quat_of_rmat(&(_q), &(_r))
510 
511 
512 
513 //
514 //
515 // Euler angles
516 //
517 //
518 
519 #define FLOAT_EULERS_ZERO(_e) EULERS_ASSIGN(_e, 0., 0., 0.);
520 
521 static inline float float_eulers_norm(struct FloatEulers *e)
522 {
523  return sqrtf(SQUARE(e->phi) + SQUARE(e->theta) + SQUARE(e->psi));
524 }
525 extern void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm);
526 extern void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q);
527 extern void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q);
528 extern void float_eulers_of_quat_yxz(struct FloatEulers *e, struct FloatQuat *q);
529 
530 /* defines for backwards compatibility */
531 #define FLOAT_EULERS_OF_RMAT(_e, _rm) WARNING("FLOAT_EULERS_OF_RMAT macro is deprecated, use the lower case function instead") float_eulers_of_rmat(&(_e), &(_rm))
532 #define FLOAT_EULERS_OF_QUAT(_e, _q) WARNING("FLOAT_EULERS_OF_QUAT macro is deprecated, use the lower case function instead") float_eulers_of_quat(&(_e), &(_q))
533 #define FLOAT_EULERS_NORM(_e) WARNING("FLOAT_EULERS_NORM macro is deprecated, use the lower case function instead") float_eulers_norm(&(_e))
534 
535 //
536 //
537 // Generic vector algebra
538 //
539 //
540 
542 static inline void float_vect_zero(float *a, const int n)
543 {
544  int i;
545  for (i = 0; i < n; i++) { a[i] = 0.; }
546 }
547 
549 static inline void float_vect_copy(float *a, const float *b, const int n)
550 {
551  int i;
552  for (i = 0; i < n; i++) { a[i] = b[i]; }
553 }
554 
556 static inline void float_vect_sum(float *o, const float *a, const float *b, const int n)
557 {
558  int i;
559  for (i = 0; i < n; i++) { o[i] = a[i] + b[i]; }
560 }
561 
563 static inline void float_vect_diff(float *o, const float *a, const float *b, const int n)
564 {
565  int i;
566  for (i = 0; i < n; i++) { o[i] = a[i] - b[i]; }
567 }
568 
570 static inline void float_vect_mul(float *o, const float *a, const float *b, const int n)
571 {
572  int i;
573  for (i = 0; i < n; i++) { o[i] = a[i] * b[i]; }
574 }
575 
577 static inline void float_vect_add(float *a, const float *b, const int n)
578 {
579  int i;
580  for (i = 0; i < n; i++) { a[i] += b[i]; }
581 }
582 
584 static inline void float_vect_sub(float *a, const float *b, const int n)
585 {
586  int i;
587  for (i = 0; i < n; i++) { a[i] -= b[i]; }
588 }
589 
591 static inline void float_vect_smul(float *o, const float *a, const float s, const int n)
592 {
593  int i;
594  for (i = 0; i < n; i++) { o[i] = a[i] * s; }
595 }
596 
598 static inline void float_vect_sdiv(float *o, const float *a, const float s, const int n)
599 {
600  int i;
601  if (fabs(s) > 1e-5) {
602  for (i = 0; i < n; i++) { o[i] = a[i] / s; }
603  }
604 }
605 
607 static inline float float_vect_norm(const float *a, const int n)
608 {
609  int i;
610  float sum = 0;
611  for (i = 0; i < n; i++) { sum += a[i] * a[i]; }
612  return sqrtf(sum);
613 }
614 
616 static inline void float_vect_scale(float *a, const float s, const int n)
617 {
618  int i;
619  for (i = 0; i < n; i++) { a[i] *= s; }
620 }
621 
623 static inline float float_vect_dot_product(const float *a, const float *b, const int n)
624 {
625  int i;
626  float dot = 0.f;
627  for (i = 0; i < n; i++) { dot += a[i] * b[i]; }
628  return dot;
629 }
630 
631 //
632 //
633 // Generic matrix algebra
634 //
635 //
636 
638 #define MAKE_MATRIX_PTR(_ptr, _mat, _rows) \
639  float * _ptr[_rows]; \
640  { \
641  int __i; \
642  for (__i = 0; __i < _rows; __i++) { _ptr[__i] = &_mat[__i][0]; } \
643  }
644 
645 extern void float_mat_invert(float **o, float **mat, int n);
646 extern void float_mat_exp(float **a, float **o, int n);
647 extern float float_mat_norm_li(float **o, int m, int n);
648 
650 static inline void float_mat_zero(float **a, int m, int n)
651 {
652  int i, j;
653  for (i = 0; i < m; i++) {
654  for (j = 0; j < n; j++) { a[i][j] = 0.; }
655  }
656 }
657 
659 static inline void float_mat_copy(float **a, float **b, int m, int n)
660 {
661  int i, j;
662  for (i = 0; i < m; i++) {
663  for (j = 0; j < n; j++) { a[i][j] = b[i][j]; }
664  }
665 }
666 
667 
668 
670 static inline void float_mat_sum(float **o, float **a, float **b, int m, int n)
671 {
672  int i, j;
673  for (i = 0; i < m; i++) {
674  for (j = 0; j < n; j++) { o[i][j] = a[i][j] + b[i][j]; }
675  }
676 }
677 
679 static inline void float_mat_diff(float **o, float **a, float **b, int m, int n)
680 {
681  int i, j;
682  for (i = 0; i < m; i++) {
683  for (j = 0; j < n; j++) { o[i][j] = a[i][j] - b[i][j]; }
684  }
685 }
686 
688 static inline void float_mat_transpose_square(float **a, int n)
689 {
690  int i, j;
691  for (i = 0; i < n; i++) {
692  for (j = 0; j < i; j++) {
693  float t = a[i][j];
694  a[i][j] = a[j][i];
695  a[j][i] = t;
696  }
697  }
698 }
699 
700 
702 static inline void float_mat_transpose(float **o, float **a, int n, int m)
703 {
704  int i, j;
705  for (i = 0; i < n; i++) {
706  for (j = 0; j < m; j++) {
707  o[j][i] = a[i][j];
708  }
709  }
710 }
711 
718 static inline void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
719 {
720  int i, j, k;
721  for (i = 0; i < m; i++) {
722  for (j = 0; j < l; j++) {
723  o[i][j] = 0.;
724  for (k = 0; k < n; k++) {
725  o[i][j] += a[i][k] * b[k][j];
726  }
727  }
728  }
729 }
730 
737 static inline void float_mat_mul_transpose(float **o, float **a, float **b, int m, int n, int l)
738 {
739  int i, j, k;
740  for (i = 0; i < m; i++) {
741  for (j = 0; j < l; j++) {
742  o[i][j] = 0.;
743  for (k = 0; k < n; k++) {
744  o[i][j] += a[i][k] * b[j][k];
745  }
746  }
747  }
748 }
749 
761 static inline void float_mat_mul_copy(float **o, float **a, float **b, int m, int n, int l)
762 {
763  float temp[m][l];
764  int i, j, k;
765  for (i = 0; i < m; i++) {
766  for (j = 0; j < l; j++) {
767  temp[i][j] = 0.;
768  for (k = 0; k < n; k++) {
769  temp[i][j] += a[i][k] * b[k][j];
770  }
771  }
772  }
773  MAKE_MATRIX_PTR(_o, o, m);
774  MAKE_MATRIX_PTR(_temp, temp, m);
775  float_mat_copy(_o, _temp, m, l);
776 }
777 
778 
785 static inline void float_mat_vect_mul(float *o, float **a, float *b, int m, int n)
786 {
787  int i, j;
788  for (i = 0; i < m; i++) {
789  o[i] = 0;
790  for (j = 0; j < n; j++) {
791  o[i] += a[i][j] * b[j];
792  }
793  }
794 }
795 
801 static inline void float_mat_scale(float **a, float k, int m, int n)
802 {
803  int i, j;
804  for (i = 0; i < m; i++) {
805  for (j = 0; j < n; j++) {
806  a[i][j] *= k;
807  }
808  }
809 }
810 
817 static inline void float_mat_sum_scaled(float **a, float **b, float k, int m, int n)
818 {
819  int i, j;
820  for (i = 0; i < m; i++) {
821  for (j = 0; j < n; j++) {
822  a[i][j] += k * b[i][j];
823  }
824  }
825 }
826 
827 
834 static inline void float_mat_minor(float **o, float **a, int m, int n, int d)
835 {
836  int i, j;
837  float_mat_zero(o, m, n);
838  for (i = 0; i < d; i++) { o[i][i] = 1.0; }
839  for (i = d; i < m; i++) {
840  for (j = d; j < n; j++) {
841  o[i][j] = a[i][j];
842  }
843  }
844 }
845 
847 static inline void float_mat_vmul(float **o, float *v, int n)
848 {
849  int i, j;
850  for (i = 0; i < n; i++) {
851  for (j = 0; j < n; j++) {
852  o[i][j] = -2. * v[i] * v[j];
853  }
854  }
855  for (i = 0; i < n; i++) {
856  o[i][i] += 1.;
857  }
858 }
859 
861 static inline void float_mat_col(float *o, float **a, int m, int c)
862 {
863  int i;
864  for (i = 0; i < m; i++) {
865  o[i] = a[i][c];
866  }
867 }
868 
870 static inline void float_mat_diagonal_scal(float **o, float v, int n)
871 {
872  int i, j;
873  for (i = 0 ; i < n; i++) {
874  for (j = 0 ; j < n; j++) {
875  if (i == j) {
876  o[i][j] = v;
877  } else {
878  o[i][j] = 0.0;
879  }
880  }
881  }
882 }
883 
884 
886 static inline void float_mat_div_scalar(float **o, float **a, float scalar, int m, int n)
887 {
888  int i, j;
889  for (i = 0; i < m; i++) {
890  for (j = 0; j < n; j++) {
891  o[i][j] = a[i][j] / scalar;
892  }
893  }
894 }
895 
897 static inline void float_mat_mul_scalar(float **o, float **a, float scalar, int m, int n)
898 {
899  int i, j;
900  for (i = 0; i < m; i++) {
901  for (j = 0; j < n; j++) {
902  o[i][j] = a[i][j] * scalar;
903  }
904  }
905 }
906 
907 
908 
909 extern bool float_mat_inv_2d(float inv_out[4], float mat_in[4]);
910 extern void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in);
911 extern bool float_mat_inv_3d(float inv_out[3][3], float mat_in[3][3]);
912 extern void float_mat3_mult(struct FloatVect3 *vect_out, float mat[3][3], struct FloatVect3 vect_in);
913 extern bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4]);
914 
915 extern void float_vect3_bound_in_2d(struct FloatVect3 *vect3, float bound);
916 extern void float_vect3_bound_in_3d(struct FloatVect3 *vect3, float bound);
917 extern void float_vect3_scale_in_2d(struct FloatVect3 *vect3, float norm_des);
918 extern void float_vect2_bound_in_2d(struct FloatVect2 *vect2, float bound);
919 extern void float_vect2_scale_in_2d(struct FloatVect2 *vect2, float norm_des);
920 
921 #ifdef __cplusplus
922 } /* extern "C" */
923 #endif
924 
925 #endif /* PPRZ_ALGEBRA_FLOAT_H */
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
float m[3 *3]
float psi
in radians
float m[3 *3]
void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e)
Rotation matrix from 321 Euler angles (float).
static void float_quat_normalize(struct FloatQuat *q)
static void float_vect_sub(float *a, const float *b, const int n)
a -= b
static void float_vect_add(float *a, const float *b, const int n)
a += b
void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv, float dt)
in place first order integration of a 3D-vector
void float_rmat_mult(struct FloatEulers *rb, struct FloatRMat *m_a2b, struct FloatEulers *ra)
rotate angle by rotation matrix.
static void float_mat_mul_transpose(float **o, float **a, float **b, int m, int n, int l)
o = a * b'
void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle)
Quaternion from unit vector and angle.
static void float_vect_sum(float *o, const float *a, const float *b, const int n)
o = a + b
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
float float_rmat_reorthogonalize(struct FloatRMat *rm)
void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions with normalization.
void float_mat3_mult(struct FloatVect3 *vect_out, float mat[3][3], struct FloatVect3 vect_in)
Multiply 3D matrix with vector.
static void float_vect_zero(float *a, const int n)
a = 0
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
static float float_vect2_norm2(struct FloatVect2 *v)
static void float_mat_vmul(float **o, float *v, int n)
o = I - v v^T
static void float_mat_diff(float **o, float **a, float **b, int m, int n)
o = a - b
void float_eulers_of_quat_yxz(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'YXZ' This function calculates from a quaternion the Euler angles with the order YXZ,...
void float_vect3_scale_in_2d(struct FloatVect3 *vect3, float norm_des)
void float_vect3_bound_in_2d(struct FloatVect3 *vect3, float bound)
void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions.
void float_vect2_scale_in_2d(struct FloatVect2 *vect2, float norm_des)
static void float_vect_mul(float *o, const float *a, const float *b, const int n)
o = a * b (element wise)
void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm)
Quaternion from rotation matrix.
static void float_vect_smul(float *o, const float *a, const float s, const int n)
o = a * s
void float_quat_of_eulers_zxy(struct FloatQuat *q, struct FloatEulers *e)
quat from euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
static void float_mat_sum(float **o, float **a, float **b, int m, int n)
o = a + b
void float_rmat_transp_mult(struct FloatEulers *rb, struct FloatRMat *m_b2a, struct FloatEulers *ra)
rotate angle by transposed rotation matrix.
void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
void float_rmat_comp_inv(struct FloatRMat *m_a2b, struct FloatRMat *m_a2c, struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
static void float_mat_col(float *o, float **a, int m, int c)
o = c-th column of matrix a[m x n]
static void float_vect_sdiv(float *o, const float *a, const float s, const int n)
o = a / s
void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
rotate anglular rates by rotation matrix.
static void float_rmat_identity(struct FloatRMat *rm)
initialises a rotation matrix to identity
void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e)
void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
static void float_mat_vect_mul(float *o, float **a, float *b, int m, int n)
o = a * b
void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b)
Inverse/transpose of a rotation matrix.
static void float_vect_scale(float *a, const float s, const int n)
a *= s
void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
bool float_mat_inv_2d(float inv_out[4], float mat_in[4])
2x2 matrix inverse
void float_vect2_bound_in_2d(struct FloatVect2 *vect2, float bound)
static void float_mat_mul_scalar(float **o, float **a, float scalar, int m, int n)
Multiply a matrix by a scalar.
void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt)
in place quaternion integration with constant rotational velocity
void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity with Lagrange multiplier.
void float_vect3_bound_in_3d(struct FloatVect3 *vect3, float bound)
static void float_mat_sum_scaled(float **a, float **b, float k, int m, int n)
a += k*b, where k is a scalar value
void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions with normalization.
static void float_mat_mul_copy(float **o, float **a, float **b, int m, int n, int l)
o = a * b
static void float_mat_copy(float **a, float **b, int m, int n)
a = b
static float float_eulers_norm(struct FloatEulers *e)
void float_quat_of_eulers_yxz(struct FloatQuat *q, struct FloatEulers *e)
quat from euler rotation 'YXZ' This function calculates a quaternion from Euler angles with the order...
void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt)
in place first order integration of a rotation matrix
void float_rates_of_euler_dot(struct FloatRates *r, struct FloatEulers *e, struct FloatEulers *edot)
static float float_vect_dot_product(const float *a, const float *b, const int n)
a.b
float float_mat_norm_li(float **o, int m, int n)
static void float_quat_wrap_shortest(struct FloatQuat *q)
static void float_vect_copy(float *a, const float *b, const int n)
a = b
void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov)
Quaternion from orientation vector.
bool float_mat_inv_3d(float inv_out[3][3], float mat_in[3][3])
3x3 matrix inverse
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
Quaternion from Euler angles.
static void float_mat_zero(float **a, int m, int n)
a = 0
static void float_mat_minor(float **o, float **a, int m, int n, int d)
matrix minor
static void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
o = a * b
static void float_vect_diff(float *o, const float *a, const float *b, const int n)
o = a - b
static float float_vect2_norm(struct FloatVect2 *v)
void float_rates_integrate_fi(struct FloatRates *r, struct FloatRates *dr, float dt)
in place first order integration of angular rates
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
static void float_vect2_normalize(struct FloatVect2 *v)
normalize 2D vector in place
void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
#define FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22)
static void float_mat_transpose(float **o, float **a, int n, int m)
transpose non-square matrix
float float_rmat_norm(struct FloatRMat *rm)
Norm of a rotation matrix.
void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm)
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
void float_mat_invert(float **o, float **mat, int n)
Calculate inverse of any n x n matrix (passed as C array) o = mat^-1 Algorithm verified with Matlab.
void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, struct FloatQuat *quat)
Tilt twist decomposition of quaternion.
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
static void float_quat_invert(struct FloatQuat *qo, struct FloatQuat *qi)
bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4])
4x4 Matrix inverse
static float float_vect3_norm(struct FloatVect3 *v)
static float float_vect_norm(const float *a, const int n)
||a||
void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, float dt)
in place first order quaternion integration with constant rotational velocity
static void float_mat_transpose_square(float **a, int n)
transpose square matrix
void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in)
Multiply 2D matrix with vector.
void float_mat_exp(float **a, float **o, int n)
static float float_log_n(float v, float n)
void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle)
initialises a rotation matrix from unit vector axis and angle
static float float_vect3_norm2(struct FloatVect3 *v)
static void float_vect3_normalize(struct FloatVect3 *v)
normalize 3D vector in place
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
static void float_mat_diagonal_scal(float **o, float v, int n)
Make an n x n identity matrix (for matrix passed as array)
void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt)
Delta rotation quaternion with constant angular rates.
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
static float float_quat_norm(struct FloatQuat *q)
static void float_mat_scale(float **a, float k, int m, int n)
a *= k, where k is a scalar value
static void float_mat_div_scalar(float **o, float **a, float scalar, int m, int n)
Divide a matrix by a scalar.
void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity.
euler angles
Roation quaternion.
rotation matrix
angular rates
#define SQUARE(_a)
Definition: pprz_algebra.h:48
#define QUAT_INVERT(_qo, _qi)
Definition: pprz_algebra.h:627
#define QUAT_EXPLEMENTARY(b, a)
Definition: pprz_algebra.h:603
static uint32_t s
Paparazzi generic algebra macros.
float b
Definition: wedgebug.c:202