Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
35extern "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
49struct FloatVect2 {
50 float x;
51 float y;
52};
53
54struct FloatVect3 {
55 float x;
56 float y;
57 float z;
58};
59
63struct FloatQuat {
64 float qi;
65 float qx;
66 float qy;
67 float qz;
68};
69
70struct FloatMat33 {
71 float m[3 * 3];
72};
73
77struct FloatRMat {
78 float m[3 * 3];
79};
80
85 float phi;
86 float theta;
87 float psi;
88};
89
93struct 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 */
107static 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
134static inline float float_vect2_norm2(struct FloatVect2 *v)
135{
136 return v->x * v->x + v->y * v->y;
137}
138
139static inline float float_vect2_norm(struct FloatVect2 *v)
140{
141 return sqrtf(float_vect2_norm2(v));
142}
143
145static 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
166static 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
171static inline float float_vect3_norm(struct FloatVect3 *v)
172{
173 return sqrtf(float_vect3_norm2(v));
174}
175
177static 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
204extern void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv,
205 float dt);
206
207extern void float_rates_integrate_fi(struct FloatRates *r, struct FloatRates *dr,
208 float dt);
209
210extern 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
255static inline void float_rmat_identity(struct FloatRMat *rm)
256{
257 FLOAT_MAT33_DIAG(*rm, 1., 1., 1.);
258}
259
263extern void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b);
264
268extern void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b,
269 struct FloatRMat *m_b2c);
270
274extern void float_rmat_comp_inv(struct FloatRMat *m_a2b, struct FloatRMat *m_a2c,
275 struct FloatRMat *m_b2c);
276
278extern float float_rmat_norm(struct FloatRMat *rm);
279
283extern void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b,
284 struct FloatVect3 *va);
285
289extern void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a,
290 struct FloatVect3 *va);
291
295extern void float_rmat_mult(struct FloatEulers *rb, struct FloatRMat *m_a2b,
296 struct FloatEulers *ra);
297
301extern void float_rmat_transp_mult(struct FloatEulers *rb, struct FloatRMat *m_b2a,
302 struct FloatEulers *ra);
303
307extern void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b,
308 struct FloatRates *ra);
309
313extern void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a,
314 struct FloatRates *ra);
315
317extern void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle);
318
331extern void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e);
332extern void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e);
333#define float_rmat_of_eulers float_rmat_of_eulers_321
334
335extern void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q);
337extern void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt);
338extern 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
365static 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
375static 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
380static 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
391static inline void float_quat_invert(struct FloatQuat *qo, struct FloatQuat *qi)
392{
393 QUAT_INVERT(*qo, *qi);
394}
395
396static 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
409extern void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c);
410
414extern void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c);
415
419extern void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c);
420
424extern void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c);
425
429extern void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c);
430
434extern void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c);
435
441extern void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q);
442
448extern void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q);
449
452extern void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt);
453
455extern void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, float dt);
456
458extern void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt);
459
463extern void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in);
464
466extern void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e);
467extern void float_quat_of_eulers_zxy(struct FloatQuat *q, struct FloatEulers *e);
468extern void float_quat_of_eulers_yxz(struct FloatQuat *q, struct FloatEulers *e);
469
474extern void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle);
475
479extern void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov);
480
482extern void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm);
483
485extern 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
521static inline float float_eulers_norm(struct FloatEulers *e)
522{
523 return sqrtf(SQUARE(e->phi) + SQUARE(e->theta) + SQUARE(e->psi));
524}
525extern void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm);
526extern void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q);
527extern void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q);
528extern 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
542static 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
549static 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
556static 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
563static 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
570static 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
577static 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
584static 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
591static 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
598static 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
607static 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
616static 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
623static 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
645extern void float_mat_invert(float **o, float **mat, int n);
646extern void float_mat_exp(float **a, float **o, int n);
647extern float float_mat_norm_li(float **o, int m, int n);
648
650static 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
659static 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
670static 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
679static 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
688static 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
702static 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
718static 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
737static 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
761static 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
785static 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
801static 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
817static 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
834static 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
847static 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
861static 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
870static 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
886static 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
897static 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
909extern bool float_mat_inv_2d(float inv_out[4], float mat_in[4]);
910extern void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in);
911extern bool float_mat_inv_3d(float inv_out[3][3], float mat_in[3][3]);
912extern void float_mat3_mult(struct FloatVect3 *vect_out, float mat[3][3], struct FloatVect3 vect_in);
913extern bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4]);
914
915extern void float_vect3_bound_in_2d(struct FloatVect3 *vect3, float bound);
916extern void float_vect3_bound_in_3d(struct FloatVect3 *vect3, float bound);
917extern void float_vect3_scale_in_2d(struct FloatVect3 *vect3, float norm_des);
918extern void float_vect2_bound_in_2d(struct FloatVect2 *vect2, float bound);
919extern 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)
#define QUAT_INVERT(_qo, _qi)
#define QUAT_EXPLEMENTARY(b, a)
static uint32_t s
uint16_t foo
Definition main_demo5.c:58
Paparazzi generic algebra macros.
float b
Definition wedgebug.c:202