Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
40 #include <math.h>
41 #include <float.h> // for FLT_MIN
42 
43 /* this seems to be missing for some arch */
44 #ifndef M_SQRT2
45 #define M_SQRT2 1.41421356237309504880
46 #endif
47 
48 struct FloatVect2 {
49  float x;
50  float y;
51 };
52 
53 struct FloatVect3 {
54  float x;
55  float y;
56  float z;
57 };
58 
62 struct FloatQuat {
63  float qi;
64  float qx;
65  float qy;
66  float qz;
67 };
68 
69 struct FloatMat33 {
70  float m[3 * 3];
71 };
72 
76 struct FloatRMat {
77  float m[3 * 3];
78 };
79 
83 struct FloatEulers {
84  float phi;
85  float theta;
86  float psi;
87 };
88 
92 struct FloatRates {
93  float p;
94  float q;
95  float r;
96 };
97 
98 #define FLOAT_ANGLE_NORMALIZE(_a) { \
99  while (_a > M_PI) _a -= (2.*M_PI); \
100  while (_a < -M_PI) _a += (2.*M_PI); \
101  }
102 
103 //
104 //
105 // Vector algebra
106 //
107 //
108 
109 
110 /*
111  * Dimension 2 Vectors
112  */
113 
114 #define FLOAT_VECT2_ZERO(_v) VECT2_ASSIGN(_v, 0., 0.)
115 
116 /* macros also usable if _v is not a FloatVect2, but a different struct with x,y members */
117 #define FLOAT_VECT2_NORM(_v) sqrtf(VECT2_NORM2(_v))
118 
119 static inline float float_vect2_norm2(struct FloatVect2 *v)
120 {
121  return v->x * v->x + v->y * v->y;
122 }
123 
124 static inline float float_vect2_norm(struct FloatVect2 *v)
125 {
126  return sqrtf(float_vect2_norm2(v));
127 }
128 
130 static inline void float_vect2_normalize(struct FloatVect2 *v)
131 {
132  const float n = float_vect2_norm(v);
133  if (n > 0) {
134  v->x /= n;
135  v->y /= n;
136  }
137 }
138 
139 #define FLOAT_VECT2_NORMALIZE(_v) float_vect2_normalize(&(_v))
140 
141 
142 /*
143  * Dimension 3 Vectors
144  */
145 
146 #define FLOAT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0., 0., 0.)
147 
148 /* macros also usable if _v is not a FloatVect3, but a different struct with x,y,z members */
149 #define FLOAT_VECT3_NORM(_v) sqrtf(VECT3_NORM2(_v))
150 
151 static inline float float_vect3_norm2(struct FloatVect3 *v)
152 {
153  return v->x * v->x + v->y * v->y + v->z * v->z;
154 }
155 
156 static inline float float_vect3_norm(struct FloatVect3 *v)
157 {
158  return sqrtf(float_vect3_norm2(v));
159 }
160 
162 static inline void float_vect3_normalize(struct FloatVect3 *v)
163 {
164  const float n = float_vect3_norm(v);
165  if (n > 0) {
166  v->x /= n;
167  v->y /= n;
168  v->z /= n;
169  }
170 }
171 
172 #define FLOAT_VECT3_NORMALIZE(_v) float_vect3_normalize(&(_v))
173 
174 
175 
176 #define FLOAT_RATES_ZERO(_r) { \
177  RATES_ASSIGN(_r, 0., 0., 0.); \
178  }
179 
180 #define FLOAT_RATES_NORM(_v) (sqrtf((_v).p*(_v).p + (_v).q*(_v).q + (_v).r*(_v).r))
181 
182 #define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2) { \
183  _ro.p = _s1 * _r1.p + _s2 * _r2.p; \
184  _ro.q = _s1 * _r1.q + _s2 * _r2.q; \
185  _ro.r = _s1 * _r1.r + _s2 * _r2.r; \
186  }
187 
188 
189 extern void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv,
190  float dt);
191 
192 extern void float_rates_integrate_fi(struct FloatRates *r, struct FloatRates *dr,
193  float dt);
194 
195 extern void float_rates_of_euler_dot(struct FloatRates *r, struct FloatEulers *e,
196  struct FloatEulers *edot);
197 
198 /* defines for backwards compatibility */
199 #define FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt) float_vect3_integrate_fi(&(_vo), &(_dv), _dt)
200 #define FLOAT_RATES_INTEGRATE_FI(_ra, _racc, _dt) float_rates_integrate_fi(&(_ra), &(_racc), _dt)
201 #define FLOAT_RATES_OF_EULER_DOT(_ra, _e, _ed) float_rates_of_euler_dot(&(_ra), &(_e), &(_ed))
202 
203 
204 /*
205  * 3x3 matrices
206  */
207 #define FLOAT_MAT33_ZERO(_m) { \
208  MAT33_ELMT(_m, 0, 0) = 0.; \
209  MAT33_ELMT(_m, 0, 1) = 0.; \
210  MAT33_ELMT(_m, 0, 2) = 0.; \
211  MAT33_ELMT(_m, 1, 0) = 0.; \
212  MAT33_ELMT(_m, 1, 1) = 0.; \
213  MAT33_ELMT(_m, 1, 2) = 0.; \
214  MAT33_ELMT(_m, 2, 0) = 0.; \
215  MAT33_ELMT(_m, 2, 1) = 0.; \
216  MAT33_ELMT(_m, 2, 2) = 0.; \
217  }
218 
219 #define FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22) { \
220  MAT33_ELMT(_m, 0, 0) = _d00; \
221  MAT33_ELMT(_m, 0, 1) = 0.; \
222  MAT33_ELMT(_m, 0, 2) = 0.; \
223  MAT33_ELMT(_m, 1, 0) = 0.; \
224  MAT33_ELMT(_m, 1, 1) = _d11; \
225  MAT33_ELMT(_m, 1, 2) = 0.; \
226  MAT33_ELMT(_m, 2, 0) = 0.; \
227  MAT33_ELMT(_m, 2, 1) = 0.; \
228  MAT33_ELMT(_m, 2, 2) = _d22; \
229  }
230 
231 
232 //
233 //
234 // Rotation Matrices
235 //
236 //
237 
238 
240 static inline void float_rmat_identity(struct FloatRMat *rm)
241 {
242  FLOAT_MAT33_DIAG(*rm, 1., 1., 1.);
243 }
244 
248 extern void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b);
249 
253 extern void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b,
254  struct FloatRMat *m_b2c);
255 
259 extern void float_rmat_comp_inv(struct FloatRMat *m_a2b, struct FloatRMat *m_a2c,
260  struct FloatRMat *m_b2c);
261 
263 extern float float_rmat_norm(struct FloatRMat *rm);
264 
268 extern void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b,
269  struct FloatVect3 *va);
270 
274 extern void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a,
275  struct FloatVect3 *va);
276 
280 extern void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b,
281  struct FloatRates *ra);
282 
286 extern void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a,
287  struct FloatRates *ra);
288 
290 extern void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle);
291 
304 extern void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e);
305 extern void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e);
306 #define float_rmat_of_eulers float_rmat_of_eulers_321
307 
308 extern void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q);
310 extern void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt);
311 extern float float_rmat_reorthogonalize(struct FloatRMat *rm);
312 
313 /* defines for backwards compatibility */
314 #define FLOAT_RMAT_INV(_m_b2a, _m_a2b) float_rmat_inv(&(_m_b2a), &(_m_a2b))
315 #define FLOAT_RMAT_NORM(_m) float_rmat_norm(&(_m))
316 #define FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c) float_rmat_comp(&(_m_a2c), &(_m_a2b), &(_m_b2c))
317 #define FLOAT_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c) float_rmat_comp_inv(&(_m_a2b), &(_m_a2c), &(_m_b2c))
318 #define FLOAT_RMAT_VMULT(_vb, _m_a2b, _va) float_rmat_vmult(&(_vb), &(_m_a2b), &(_va))
319 #define FLOAT_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va) float_rmat_transp_vmult(&(_vb), &(_m_b2a), &(_va))
320 #define FLOAT_RMAT_RATEMULT(_rb, _m_a2b, _ra) float_rmat_ratemult(&(_rb), &(_m_a2b), &(_ra))
321 #define FLOAT_RMAT_TRANSP_RATEMULT(_rb, _m_b2a, _ra) float_rmat_ratemult(&(_rb), &(_m_b2a), &(_ra))
322 #define FLOAT_RMAT_OF_AXIS_ANGLE(_rm, _uv, _an) float_rmat_of_axis_angle(&(_rm), &(_uv), _an)
323 #define FLOAT_RMAT_OF_EULERS(_rm, _e) float_rmat_of_eulers_321(&(_rm), &(_e))
324 #define FLOAT_RMAT_OF_EULERS_321(_rm, _e) float_rmat_of_eulers_321(&(_rm), &(_e))
325 #define FLOAT_RMAT_OF_EULERS_312(_rm, _e) float_rmat_of_eulers_312(&(_rm), &(_e))
326 #define FLOAT_RMAT_OF_QUAT(_rm, _q) float_rmat_of_quat(&(_rm), &(_q))
327 #define FLOAT_RMAT_INTEGRATE_FI(_rm, _omega, _dt) float_rmat_integrate_fi(&(_rm), &(_omega), &(_dt))
328 
329 
330 
331 //
332 //
333 // Quaternion algebras
334 //
335 //
336 
338 static inline void float_quat_identity(struct FloatQuat *q)
339 {
340  q->qi = 1.0;
341  q->qx = 0;
342  q->qy = 0;
343  q->qz = 0;
344 }
345 
346 #define FLOAT_QUAT_NORM2(_q) (SQUARE((_q).qi) + SQUARE((_q).qx) + SQUARE((_q).qy) + SQUARE((_q).qz))
347 
348 static inline float float_quat_norm(struct FloatQuat *q)
349 {
350  return sqrtf(SQUARE(q->qi) + SQUARE(q->qx) + SQUARE(q->qy) + SQUARE(q->qz));
351 }
352 
353 static inline void float_quat_normalize(struct FloatQuat *q)
354 {
355  float qnorm = float_quat_norm(q);
356  if (qnorm > FLT_MIN) {
357  q->qi = q->qi / qnorm;
358  q->qx = q->qx / qnorm;
359  q->qy = q->qy / qnorm;
360  q->qz = q->qz / qnorm;
361  }
362 }
363 
364 static inline void float_quat_invert(struct FloatQuat *qo, struct FloatQuat *qi)
365 {
366  QUAT_INVERT(*qo, *qi);
367 }
368 
369 static inline void float_quat_wrap_shortest(struct FloatQuat *q)
370 {
371  if (q->qi < 0.) {
372  QUAT_EXPLEMENTARY(*q, *q);
373  }
374 }
375 
376 #define FLOAT_QUAT_EXTRACT(_vo, _qi) QUAT_EXTRACT_Q(_vo, _qi)
377 
378 
382 extern void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c);
383 
387 extern void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c);
388 
392 extern void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c);
393 
397 extern void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c);
398 
402 extern void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c);
403 
407 extern void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c);
408 
414 extern void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q);
415 
421 extern void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q);
422 
425 extern void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt);
426 
428 extern void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, float dt);
429 
431 extern void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt);
432 
436 extern void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in);
437 
439 extern void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e);
440 
442 extern void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle);
443 
447 extern void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov);
448 
450 extern void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm);
451 
452 
453 /* defines for backwards compatibility */
454 #define FLOAT_QUAT_ZERO(_q) float_quat_identity(&(_q))
455 #define FLOAT_QUAT_INVERT(_qo, _qi) float_quat_invert(&(_qo), &(_qi))
456 #define FLOAT_QUAT_WRAP_SHORTEST(_q) float_quat_wrap_shortest(&(_q))
457 #define FLOAT_QUAT_NORM(_q) float_quat_norm(&(_q))
458 #define FLOAT_QUAT_NORMALIZE(_q) float_quat_normalize(&(_q))
459 #define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c) float_quat_comp(&(_a2c), &(_a2b), &(_b2c))
460 #define FLOAT_QUAT_MULT(_a2c, _a2b, _b2c) float_quat_comp(&(_a2c), &(_a2b), &(_b2c))
461 #define FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c) float_quat_inv_comp(&(_b2c), &(_a2b), &(_a2c))
462 #define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c) float_quat_comp_inv(&(_a2b), &(_a2c), &(_b2c))
463 #define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) float_quat_comp_norm_shortest(&(_a2c), &(_a2b), &(_b2c))
464 #define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) float_quat_comp_inv_norm_shortest(&(_a2b), &(_a2c), &(_b2c))
465 #define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) float_quat_inv_comp_norm_shortest(&(_b2c), &(_a2b), &(_a2c))
466 #define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt) float_quat_differential(&(q_out), &(w), dt)
467 #define FLOAT_QUAT_INTEGRATE(_q, _omega, _dt) float_quat_integrate(&(_q), &(_omega), _dt)
468 #define FLOAT_QUAT_VMULT(v_out, q, v_in) float_quat_vmult(&(v_out), &(q), &(v_in))
469 #define FLOAT_QUAT_DERIVATIVE(_qd, _r, _q) float_quat_derivative(&(_qd), &(_r), &(_q))
470 #define FLOAT_QUAT_DERIVATIVE_LAGRANGE(_qd, _r, _q) float_quat_derivative_lagrange(&(_qd), &(_r), &(_q))
471 #define FLOAT_QUAT_OF_EULERS(_q, _e) float_quat_of_eulers(&(_q), &(_e))
472 #define FLOAT_QUAT_OF_AXIS_ANGLE(_q, _uv, _an) float_quat_of_axis_angle(&(_q), &(_uv), _an)
473 #define FLOAT_QUAT_OF_ORIENTATION_VECT(_q, _ov) float_quat_of_orientation_vect(&(_q), &(_ov))
474 #define FLOAT_QUAT_OF_RMAT(_q, _r) float_quat_of_rmat(&(_q), &(_r))
475 
476 
477 
478 //
479 //
480 // Euler angles
481 //
482 //
483 
484 #define FLOAT_EULERS_ZERO(_e) EULERS_ASSIGN(_e, 0., 0., 0.);
485 
486 static inline float float_eulers_norm(struct FloatEulers *e)
487 {
488  return sqrtf(SQUARE(e->phi) + SQUARE(e->theta) + SQUARE(e->psi));
489 }
490 extern void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm);
491 extern void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q);
492 
493 /* defines for backwards compatibility */
494 #define FLOAT_EULERS_OF_RMAT(_e, _rm) float_eulers_of_rmat(&(_e), &(_rm))
495 #define FLOAT_EULERS_OF_QUAT(_e, _q) float_eulers_of_quat(&(_e), &(_q))
496 #define FLOAT_EULERS_NORM(_e) float_eulers_norm(&(_e))
497 
498 //
499 //
500 // Generic vector algebra
501 //
502 //
503 
505 static inline void float_vect_zero(float *a, const int n)
506 {
507  int i;
508  for (i = 0; i < n; i++) { a[i] = 0.; }
509 }
510 
512 static inline void float_vect_copy(float *a, const float *b, const int n)
513 {
514  int i;
515  for (i = 0; i < n; i++) { a[i] = b[i]; }
516 }
517 
519 static inline void float_vect_sum(float *o, const float *a, const float *b, const int n)
520 {
521  int i;
522  for (i = 0; i < n; i++) { o[i] = a[i] + b[i]; }
523 }
524 
526 static inline void float_vect_diff(float *o, const float *a, const float *b, const int n)
527 {
528  int i;
529  for (i = 0; i < n; i++) { o[i] = a[i] - b[i]; }
530 }
531 
533 static inline void float_vect_mul(float *o, const float *a, const float *b, const int n)
534 {
535  int i;
536  for (i = 0; i < n; i++) { o[i] = a[i] * b[i]; }
537 }
538 
540 static inline void float_vect_add(float *a, const float *b, const int n)
541 {
542  int i;
543  for (i = 0; i < n; i++) { a[i] += b[i]; }
544 }
545 
547 static inline void float_vect_sub(float *a, const float *b, const int n)
548 {
549  int i;
550  for (i = 0; i < n; i++) { a[i] -= b[i]; }
551 }
552 
554 static inline void float_vect_smul(float *o, const float *a, const float s, const int n)
555 {
556  int i;
557  for (i = 0; i < n; i++) { o[i] = a[i] * s; }
558 }
559 
561 static inline void float_vect_sdiv(float *o, const float *a, const float s, const int n)
562 {
563  int i;
564  if (fabs(s) > 1e-5) {
565  for (i = 0; i < n; i++) { o[i] = a[i] / s; }
566  }
567 }
568 
570 static inline float float_vect_norm(const float *a, const int n)
571 {
572  int i;
573  float sum = 0;
574  for (i = 0; i < n; i++) { sum += a[i] * a[i]; }
575  return sqrtf(sum);
576 }
577 
578 //
579 //
580 // Generic matrix algebra
581 //
582 //
583 
585 #define MAKE_MATRIX_PTR(_ptr, _mat, _rows) \
586  float * _ptr[_rows]; \
587  { \
588  int __i; \
589  for (__i = 0; __i < _rows; __i++) { _ptr[__i] = &_mat[__i][0]; } \
590  }
591 
593 static inline void float_mat_zero(float **a, int m, int n)
594 {
595  int i, j;
596  for (i = 0; i < m; i++) {
597  for (j = 0; j < n; j++) { a[i][j] = 0.; }
598  }
599 }
600 
602 static inline void float_mat_copy(float **a, float **b, int m, int n)
603 {
604  int i, j;
605  for (i = 0; i < m; i++) {
606  for (j = 0; j < n; j++) { a[i][j] = b[i][j]; }
607  }
608 }
609 
611 static inline void float_mat_sum(float **o, float **a, float **b, int m, int n)
612 {
613  int i, j;
614  for (i = 0; i < m; i++) {
615  for (j = 0; j < n; j++) { o[i][j] = a[i][j] + b[i][j]; }
616  }
617 }
618 
620 static inline void float_mat_diff(float **o, float **a, float **b, int m, int n)
621 {
622  int i, j;
623  for (i = 0; i < m; i++) {
624  for (j = 0; j < n; j++) { o[i][j] = a[i][j] - b[i][j]; }
625  }
626 }
627 
629 static inline void float_mat_transpose(float **a, int n)
630 {
631  int i, j;
632  for (i = 0; i < n; i++) {
633  for (j = 0; j < i; j++) {
634  float t = a[i][j];
635  a[i][j] = a[j][i];
636  a[j][i] = t;
637  }
638  }
639 }
640 
647 static inline void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
648 {
649  int i, j, k;
650  for (i = 0; i < m; i++) {
651  for (j = 0; j < l; j++) {
652  o[i][j] = 0.;
653  for (k = 0; k < n; k++) {
654  o[i][j] += a[i][k] * b[k][j];
655  }
656  }
657  }
658 }
659 
666 static inline void float_mat_minor(float **o, float **a, int m, int n, int d)
667 {
668  int i, j;
669  float_mat_zero(o, m, n);
670  for (i = 0; i < d; i++) { o[i][i] = 1.0; }
671  for (i = d; i < m; i++) {
672  for (j = d; j < n; j++) {
673  o[i][j] = a[i][j];
674  }
675  }
676 }
677 
679 static inline void float_mat_vmul(float **o, float *v, int n)
680 {
681  int i, j;
682  for (i = 0; i < n; i++) {
683  for (j = 0; j < n; j++) {
684  o[i][j] = -2. * v[i] * v[j];
685  }
686  }
687  for (i = 0; i < n; i++) {
688  o[i][i] += 1.;
689  }
690 }
691 
693 static inline void float_mat_col(float *o, float **a, int m, int c)
694 {
695  int i;
696  for (i = 0; i < m; i++) {
697  o[i] = a[i][c];
698  }
699 }
700 
701 #ifdef __cplusplus
702 } /* extern "C" */
703 #endif
704 
705 #endif /* PPRZ_ALGEBRA_FLOAT_H */
706 
static float float_eulers_norm(struct FloatEulers *e)
void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
static void float_vect_sum(float *o, const float *a, const float *b, const int n)
o = a + b
static void float_mat_transpose(float **a, int n)
transpose square matrix
void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b)
Inverse/transpose of a rotation matrix.
static float float_vect_norm(const float *a, const int n)
||a||
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
Quaternion from Euler angles.
float phi
in radians
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv, float dt)
in place first order integration of a 3D-vector
static void float_mat_copy(float **a, float **b, int m, int n)
a = b
static void float_vect_add(float *a, const float *b, const int n)
a += b
void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm)
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_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
#define QUAT_INVERT(_qo, _qi)
Definition: pprz_algebra.h:563
float r
in rad/s
static void float_vect2_normalize(struct FloatVect2 *v)
normalize 2D vector in place
float float_rmat_norm(struct FloatRMat *rm)
Norm of a rotation matrix.
#define QUAT_EXPLEMENTARY(b, a)
Definition: pprz_algebra.h:539
float psi
in radians
#define FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22)
static void float_mat_diff(float **o, float **a, float **b, int m, int n)
o = a - b
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_vect_sub(float *a, const float *b, const int n)
a -= b
static void float_mat_vmul(float **o, float *v, int n)
o = I - v v^T
#define SQUARE(_a)
Definition: pprz_algebra.h:47
static void float_rmat_identity(struct FloatRMat *rm)
initialises a rotation matrix to identity
float q
in rad/s
float p
in rad/s
void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions.
static void float_vect_sdiv(float *o, const float *a, const float s, const int n)
o = a / s
static float float_vect3_norm2(struct FloatVect3 *v)
euler angles
static void float_mat_minor(float **o, float **a, int m, int n, int d)
matrix minor
static void float_vect3_normalize(struct FloatVect3 *v)
normalize 3D vector in place
Roation quaternion.
void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt)
in place first order integration of a rotation matrix
static float float_vect3_norm(struct FloatVect3 *v)
void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e)
float theta
in radians
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
static void float_vect_mul(float *o, const float *a, const float *b, const int n)
o = a * b (element wise)
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_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
Paparazzi generic algebra macros.
void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt)
in place quaternion integration with constant rotational velocity
static void float_vect_zero(float *a, const int n)
a = 0
static void float_mat_zero(float **a, int m, int n)
a = 0
void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
static void float_quat_normalize(struct FloatQuat *q)
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_quat_invert(struct FloatQuat *qo, struct FloatQuat *qi)
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
static void float_vect_diff(float *o, const float *a, const float *b, const int n)
o = a - b
void float_rates_of_euler_dot(struct FloatRates *r, struct FloatEulers *e, struct FloatEulers *edot)
void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity.
float m[3 *3]
float float_rmat_reorthogonalize(struct FloatRMat *rm)
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e)
Rotation matrix from 321 Euler angles (float).
void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle)
initialises a rotation matrix from unit vector axis and angle
void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov)
Quaternion from orientation vector.
void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle)
Quaternion from unit vector and angle.
static void float_quat_wrap_shortest(struct FloatQuat *q)
void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions with normalization.
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
static void float_vect_copy(float *a, const float *b, const int n)
a = b
void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity with Lagrange multiplier.
rotation matrix
static void float_mat_sum(float **o, float **a, float **b, int m, int n)
o = a + b
float m[3 *3]
static float float_quat_norm(struct FloatQuat *q)
void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
rotate anglular rates by rotation matrix.
void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt)
Delta rotation quaternion with constant angular rates.
static float float_vect2_norm2(struct FloatVect2 *v)
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, float dt)
in place first order quaternion integration with constant rotational velocity
angular rates
static void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
o = a * b