Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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-2011 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  */
21 
30 #ifndef PPRZ_ALGEBRA_FLOAT_H
31 #define PPRZ_ALGEBRA_FLOAT_H
32 
33 #include "pprz_algebra.h"
34 
35 #include <math.h>
36 #include <float.h> // for FLT_MIN
37 
38 /* this seems to be missing for some arch */
39 #ifndef M_SQRT2
40 #define M_SQRT2 1.41421356237309504880
41 #endif
42 
43 struct FloatVect2 {
44  float x;
45  float y;
46 };
47 
48 struct FloatVect3 {
49  float x;
50  float y;
51  float z;
52 };
53 
57 struct FloatQuat {
58  float qi;
59  float qx;
60  float qy;
61  float qz;
62 };
63 
64 struct FloatMat33 {
65  float m[3*3];
66 };
67 
71 struct FloatRMat {
72  float m[3*3];
73 };
74 
78 struct FloatEulers {
79  float phi;
80  float theta;
81  float psi;
82 };
83 
87 struct FloatRates {
88  float p;
89  float q;
90  float r;
91 };
92 
93 #define FLOAT_ANGLE_NORMALIZE(_a) { \
94  while (_a > M_PI) _a -= (2.*M_PI); \
95  while (_a < -M_PI) _a += (2.*M_PI); \
96  }
97 
98 //
99 //
100 // Vector algebra
101 //
102 //
103 
104 
105 /*
106  * Dimension 2 Vectors
107  */
108 
109 #define FLOAT_VECT2_ZERO(_v) VECT2_ASSIGN(_v, 0., 0.)
110 
111 /* a = {x, y} */
112 #define FLOAT_VECT2_ASSIGN(_a, _x, _y) VECT2_ASSIGN(_a, _x, _y)
113 
114 /* a = b */
115 #define FLOAT_VECT2_COPY(_a, _b) VECT2_COPY(_a, _b)
116 
117 /* a += b */
118 #define FLOAT_VECT2_ADD(_a, _b) VECT2_ADD(_a, _b)
119 
120 /* c = a + b */
121 #define FLOAT_VECT2_SUM(_c, _a, _b) VECT2_SUM(_c, _a, _b)
122 
123 /* c = a - b */
124 #define FLOAT_VECT2_DIFF(_c, _a, _b) VECT2_DIFF(_c, _a, _b)
125 
126 /* a -= b */
127 #define FLOAT_VECT2_SUB(_a, _b) VECT2_SUB(_a, _b)
128 
129 /* _vo = _vi * _s */
130 #define FLOAT_VECT2_SMUL(_vo, _vi, _s) VECT2_SMUL(_vo, _vi, _s)
131 
132 #define FLOAT_VECT2_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y)
133 
134 #define FLOAT_VECT2_NORM(_n, _v) { \
135  _n = sqrtf(FLOAT_VECT2_NORM2(_v)); \
136  }
137 
138 #define FLOAT_VECT2_NORMALIZE(_v) { \
139  const float n = sqrtf(FLOAT_VECT2_NORM2(_v)); \
140  FLOAT_VECT2_SMUL(_v, _v, 1./n); \
141  }
142 
143 #define FLOAT_VECT2_DOT_PRODUCT(_v1, _v2) ((_v1).x*(_v2).x + (_v1).y*(_v2).y)
144 
145 /*
146  * Dimension 3 Vectors
147  */
148 
149 #define FLOAT_VECT3_SUM(_a, _b, _c) VECT3_SUM(_a, _b, _c)
150 #define FLOAT_VECT3_SDIV(_a, _b, _s) VECT3_SDIV(_a, _b, _s)
151 #define FLOAT_VECT3_COPY(_a, _b) VECT3_COPY(_a, _b)
152 
153 #define FLOAT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0., 0., 0.)
154 
155 #define FLOAT_VECT3_ASSIGN(_a, _x, _y, _z) VECT3_ASSIGN(_a, _x, _y, _z)
156 
157 /* c = a - b */
158 #define FLOAT_VECT3_DIFF(_c, _a, _b) VECT3_DIFF(_c, _a, _b)
159 
160 /* a -= b */
161 #define FLOAT_VECT3_SUB(_a, _b) VECT3_SUB(_a, _b)
162 
163 #define FLOAT_VECT3_ADD(_a, _b) VECT3_ADD(_a, _b)
164 
165 /* _vo = _vi * _s */
166 #define FLOAT_VECT3_SMUL(_vo, _vi, _s) VECT3_SMUL(_vo, _vi, _s)
167 
168 #define FLOAT_VECT3_MUL(_v1, _v2) VECT3_MUL(_v1, _v2)
169 
170 #define FLOAT_VECT3_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z)
171 
172 #define FLOAT_VECT3_NORM(_v) (sqrtf(FLOAT_VECT3_NORM2(_v)))
173 
174 #define FLOAT_VECT3_DOT_PRODUCT( _v1, _v2) ((_v1).x*(_v2).x + (_v1).y*(_v2).y + (_v1).z*(_v2).z)
175 
176 #define FLOAT_VECT3_CROSS_PRODUCT(_vo, _v1, _v2) { \
177  (_vo).x = (_v1).y*(_v2).z - (_v1).z*(_v2).y; \
178  (_vo).y = (_v1).z*(_v2).x - (_v1).x*(_v2).z; \
179  (_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x; \
180  }
181 
182 #define FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt) { \
183  (_vo).x += (_dv).x * (_dt); \
184  (_vo).y += (_dv).y * (_dt); \
185  (_vo).z += (_dv).z * (_dt); \
186  }
187 
188 
189 
190 #define FLOAT_VECT3_NORMALIZE(_v) { \
191  const float n = FLOAT_VECT3_NORM(_v); \
192  FLOAT_VECT3_SMUL(_v, _v, 1./n); \
193  }
194 
195 #define FLOAT_RATES_ZERO(_r) { \
196  RATES_ASSIGN(_r, 0., 0., 0.); \
197  }
198 
199 #define FLOAT_RATES_NORM(_v) (sqrtf((_v).p*(_v).p + (_v).q*(_v).q + (_v).r*(_v).r))
200 
201 #define FLOAT_RATES_ADD_SCALED_VECT(_ro, _v, _s) { \
202  _ro.p += _v.x * _s; \
203  _ro.q += _v.y * _s; \
204  _ro.r += _v.z * _s; \
205  }
206 
207 
208 #define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2) { \
209  _ro.p = _s1 * _r1.p + _s2 * _r2.p; \
210  _ro.q = _s1 * _r1.q + _s2 * _r2.q; \
211  _ro.r = _s1 * _r1.r + _s2 * _r2.r; \
212  }
213 
214 
215 #define FLOAT_RATES_SCALE(_ro,_s) { \
216  _ro.p *= _s; \
217  _ro.q *= _s; \
218  _ro.r *= _s; \
219  }
220 
221 #define FLOAT_RATES_INTEGRATE_FI(_ra, _racc, _dt) { \
222  (_ra).p += (_racc).p * (_dt); \
223  (_ra).q += (_racc).q * (_dt); \
224  (_ra).r += (_racc).r * (_dt); \
225  }
226 
227 #define FLOAT_RATES_OF_EULER_DOT(_ra, _e, _ed) { \
228  _ra.p = _ed.phi - sinf(_e.theta) *_ed.psi; \
229  _ra.q = cosf(_e.phi)*_ed.theta + sinf(_e.phi)*cosf(_e.theta)*_ed.psi; \
230  _ra.r = -sinf(_e.phi)*_ed.theta + cosf(_e.phi)*cosf(_e.theta)*_ed.psi; \
231  }
232 
233 
234 
235 /*
236  * 3x3 matrices
237  */
238 #define FLOAT_MAT33_ZERO(_m) { \
239  MAT33_ELMT(_m, 0, 0) = 0.; \
240  MAT33_ELMT(_m, 0, 1) = 0.; \
241  MAT33_ELMT(_m, 0, 2) = 0.; \
242  MAT33_ELMT(_m, 1, 0) = 0.; \
243  MAT33_ELMT(_m, 1, 1) = 0.; \
244  MAT33_ELMT(_m, 1, 2) = 0.; \
245  MAT33_ELMT(_m, 2, 0) = 0.; \
246  MAT33_ELMT(_m, 2, 1) = 0.; \
247  MAT33_ELMT(_m, 2, 2) = 0.; \
248  }
249 
250 #define FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22) { \
251  MAT33_ELMT(_m, 0, 0) = _d00; \
252  MAT33_ELMT(_m, 0, 1) = 0.; \
253  MAT33_ELMT(_m, 0, 2) = 0.; \
254  MAT33_ELMT(_m, 1, 0) = 0.; \
255  MAT33_ELMT(_m, 1, 1) = _d11; \
256  MAT33_ELMT(_m, 1, 2) = 0.; \
257  MAT33_ELMT(_m, 2, 0) = 0.; \
258  MAT33_ELMT(_m, 2, 1) = 0.; \
259  MAT33_ELMT(_m, 2, 2) = _d22; \
260  }
261 
262 
263 //
264 //
265 // Rotation Matrices
266 //
267 //
268 
269 
270 /* initialises a matrix to identity */
271 #define FLOAT_RMAT_ZERO(_rm) FLOAT_MAT33_DIAG(_rm, 1., 1., 1.)
272 
273 /* initialises a rotation matrix from unit vector axis and angle */
274 #define FLOAT_RMAT_OF_AXIS_ANGLE(_rm, _uv, _an) { \
275  \
276  const float ux2 = _uv.x*_uv.x; \
277  const float uy2 = _uv.y*_uv.y; \
278  const float uz2 = _uv.z*_uv.z; \
279  const float uxuy = _uv.x*_uv.y; \
280  const float uyuz = _uv.y*_uv.z; \
281  const float uxuz = _uv.x*_uv.z; \
282  const float can = cosf(_an); \
283  const float san = sinf(_an); \
284  const float one_m_can = (1. - can); \
285  \
286  RMAT_ELMT(_rm, 0, 0) = ux2 + (1.-ux2)*can; \
287  RMAT_ELMT(_rm, 0, 1) = uxuy*one_m_can + _uv.z*san; \
288  RMAT_ELMT(_rm, 0, 2) = uxuz*one_m_can - _uv.y*san; \
289  RMAT_ELMT(_rm, 1, 0) = uxuy*one_m_can - _uv.z*san; \
290  RMAT_ELMT(_rm, 1, 1) = uy2 + (1.-uy2)*can; \
291  RMAT_ELMT(_rm, 1, 2) = uyuz*one_m_can + _uv.x*san; \
292  RMAT_ELMT(_rm, 2, 0) = uxuz*one_m_can + _uv.y*san; \
293  RMAT_ELMT(_rm, 2, 1) = uyuz*one_m_can - _uv.x*san; \
294  RMAT_ELMT(_rm, 2, 2) = uz2 + (1.-uz2)*can; \
295  \
296  }
297 
298 /* multiply _vin by _rmat, store in _vout */
299 #define FLOAT_RMAT_VECT3_MUL(_vout, _rmat, _vin) RMAT_VECT3_MUL(_vout, _rmat, _vin)
300 #define FLOAT_RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin) RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin)
301 
302 #define FLOAT_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va) { \
303  (_vb).p = ( (_m_b2a).m[0]*(_va).p + (_m_b2a).m[3]*(_va).q + (_m_b2a).m[6]*(_va).r); \
304  (_vb).q = ( (_m_b2a).m[1]*(_va).p + (_m_b2a).m[4]*(_va).q + (_m_b2a).m[7]*(_va).r); \
305  (_vb).r = ( (_m_b2a).m[2]*(_va).p + (_m_b2a).m[5]*(_va).q + (_m_b2a).m[8]*(_va).r); \
306  }
307 
308 #define FLOAT_RMAT_RATEMULT(_vb, _m_a2b, _va) { \
309  (_vb).p = ( (_m_a2b).m[0]*(_va).p + (_m_a2b).m[1]*(_va).q + (_m_a2b).m[2]*(_va).r); \
310  (_vb).q = ( (_m_a2b).m[3]*(_va).p + (_m_a2b).m[4]*(_va).q + (_m_a2b).m[5]*(_va).r); \
311  (_vb).r = ( (_m_a2b).m[6]*(_va).p + (_m_a2b).m[7]*(_va).q + (_m_a2b).m[8]*(_va).r); \
312  }
313 
314 /* _m_a2c = _m_a2b comp _m_b2c , aka _m_a2c = _m_b2c * _m_a2b */
315 #define FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c) { \
316  _m_a2c.m[0] = ((_m_b2c).m[0]*(_m_a2b).m[0] + (_m_b2c).m[1]*(_m_a2b).m[3] + (_m_b2c).m[2]*(_m_a2b).m[6]); \
317  _m_a2c.m[1] = ((_m_b2c).m[0]*(_m_a2b).m[1] + (_m_b2c).m[1]*(_m_a2b).m[4] + (_m_b2c).m[2]*(_m_a2b).m[7]); \
318  _m_a2c.m[2] = ((_m_b2c).m[0]*(_m_a2b).m[2] + (_m_b2c).m[1]*(_m_a2b).m[5] + (_m_b2c).m[2]*(_m_a2b).m[8]); \
319  _m_a2c.m[3] = ((_m_b2c).m[3]*(_m_a2b).m[0] + (_m_b2c).m[4]*(_m_a2b).m[3] + (_m_b2c).m[5]*(_m_a2b).m[6]); \
320  _m_a2c.m[4] = ((_m_b2c).m[3]*(_m_a2b).m[1] + (_m_b2c).m[4]*(_m_a2b).m[4] + (_m_b2c).m[5]*(_m_a2b).m[7]); \
321  _m_a2c.m[5] = ((_m_b2c).m[3]*(_m_a2b).m[2] + (_m_b2c).m[4]*(_m_a2b).m[5] + (_m_b2c).m[5]*(_m_a2b).m[8]); \
322  _m_a2c.m[6] = ((_m_b2c).m[6]*(_m_a2b).m[0] + (_m_b2c).m[7]*(_m_a2b).m[3] + (_m_b2c).m[8]*(_m_a2b).m[6]); \
323  _m_a2c.m[7] = ((_m_b2c).m[6]*(_m_a2b).m[1] + (_m_b2c).m[7]*(_m_a2b).m[4] + (_m_b2c).m[8]*(_m_a2b).m[7]); \
324  _m_a2c.m[8] = ((_m_b2c).m[6]*(_m_a2b).m[2] + (_m_b2c).m[7]*(_m_a2b).m[5] + (_m_b2c).m[8]*(_m_a2b).m[8]); \
325  }
326 
327 
328 /* _m_a2b = _m_a2c comp_inv _m_b2c , aka _m_a2b = inv(_m_b2c) * _m_a2c */
329 #define FLOAT_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c) { \
330  _m_a2b.m[0] = ((_m_b2c).m[0]*(_m_a2c).m[0] + (_m_b2c).m[3]*(_m_a2c).m[3] + (_m_b2c).m[6]*(_m_a2c).m[6]); \
331  _m_a2b.m[1] = ((_m_b2c).m[0]*(_m_a2c).m[1] + (_m_b2c).m[3]*(_m_a2c).m[4] + (_m_b2c).m[6]*(_m_a2c).m[7]); \
332  _m_a2b.m[2] = ((_m_b2c).m[0]*(_m_a2c).m[2] + (_m_b2c).m[3]*(_m_a2c).m[5] + (_m_b2c).m[6]*(_m_a2c).m[8]); \
333  _m_a2b.m[3] = ((_m_b2c).m[1]*(_m_a2c).m[0] + (_m_b2c).m[4]*(_m_a2c).m[3] + (_m_b2c).m[7]*(_m_a2c).m[6]); \
334  _m_a2b.m[4] = ((_m_b2c).m[1]*(_m_a2c).m[1] + (_m_b2c).m[4]*(_m_a2c).m[4] + (_m_b2c).m[7]*(_m_a2c).m[7]); \
335  _m_a2b.m[5] = ((_m_b2c).m[1]*(_m_a2c).m[2] + (_m_b2c).m[4]*(_m_a2c).m[5] + (_m_b2c).m[7]*(_m_a2c).m[8]); \
336  _m_a2b.m[6] = ((_m_b2c).m[2]*(_m_a2c).m[0] + (_m_b2c).m[5]*(_m_a2c).m[3] + (_m_b2c).m[8]*(_m_a2c).m[6]); \
337  _m_a2b.m[7] = ((_m_b2c).m[2]*(_m_a2c).m[1] + (_m_b2c).m[5]*(_m_a2c).m[4] + (_m_b2c).m[8]*(_m_a2c).m[7]); \
338  _m_a2b.m[8] = ((_m_b2c).m[2]*(_m_a2c).m[2] + (_m_b2c).m[5]*(_m_a2c).m[5] + (_m_b2c).m[8]*(_m_a2c).m[8]); \
339  }
340 
341 
342 /* _m_b2a = inv(_m_a2b) = transp(_m_a2b) */
343 #define FLOAT_RMAT_INV(_m_b2a, _m_a2b) { \
344  RMAT_ELMT(_m_b2a, 0, 0) = RMAT_ELMT(_m_a2b, 0, 0); \
345  RMAT_ELMT(_m_b2a, 0, 1) = RMAT_ELMT(_m_a2b, 1, 0); \
346  RMAT_ELMT(_m_b2a, 0, 2) = RMAT_ELMT(_m_a2b, 2, 0); \
347  RMAT_ELMT(_m_b2a, 1, 0) = RMAT_ELMT(_m_a2b, 0, 1); \
348  RMAT_ELMT(_m_b2a, 1, 1) = RMAT_ELMT(_m_a2b, 1, 1); \
349  RMAT_ELMT(_m_b2a, 1, 2) = RMAT_ELMT(_m_a2b, 2, 1); \
350  RMAT_ELMT(_m_b2a, 2, 0) = RMAT_ELMT(_m_a2b, 0, 2); \
351  RMAT_ELMT(_m_b2a, 2, 1) = RMAT_ELMT(_m_a2b, 1, 2); \
352  RMAT_ELMT(_m_b2a, 2, 2) = RMAT_ELMT(_m_a2b, 2, 2); \
353  }
354 
355 #define FLOAT_RMAT_NORM(_m) ( \
356  sqrtf(SQUARE((_m).m[0])+ SQUARE((_m).m[1])+ SQUARE((_m).m[2])+ \
357  SQUARE((_m).m[3])+ SQUARE((_m).m[4])+ SQUARE((_m).m[5])+ \
358  SQUARE((_m).m[6])+ SQUARE((_m).m[7])+ SQUARE((_m).m[8])) \
359  )
360 
361 #define FLOAT_RMAT_OF_EULERS(_rm, _e) FLOAT_RMAT_OF_EULERS_321(_rm, _e)
362 
363 /* C n->b rotation matrix */
364 #define FLOAT_RMAT_OF_EULERS_321(_rm, _e) { \
365  \
366  const float sphi = sinf((_e).phi); \
367  const float cphi = cosf((_e).phi); \
368  const float stheta = sinf((_e).theta); \
369  const float ctheta = cosf((_e).theta); \
370  const float spsi = sinf((_e).psi); \
371  const float cpsi = cosf((_e).psi); \
372  \
373  RMAT_ELMT(_rm, 0, 0) = ctheta*cpsi; \
374  RMAT_ELMT(_rm, 0, 1) = ctheta*spsi; \
375  RMAT_ELMT(_rm, 0, 2) = -stheta; \
376  RMAT_ELMT(_rm, 1, 0) = sphi*stheta*cpsi - cphi*spsi; \
377  RMAT_ELMT(_rm, 1, 1) = sphi*stheta*spsi + cphi*cpsi; \
378  RMAT_ELMT(_rm, 1, 2) = sphi*ctheta; \
379  RMAT_ELMT(_rm, 2, 0) = cphi*stheta*cpsi + sphi*spsi; \
380  RMAT_ELMT(_rm, 2, 1) = cphi*stheta*spsi - sphi*cpsi; \
381  RMAT_ELMT(_rm, 2, 2) = cphi*ctheta; \
382  \
383  }
384 
385 
386 #define FLOAT_RMAT_OF_EULERS_312(_rm, _e) { \
387  \
388  const float sphi = sinf((_e).phi); \
389  const float cphi = cosf((_e).phi); \
390  const float stheta = sinf((_e).theta); \
391  const float ctheta = cosf((_e).theta); \
392  const float spsi = sinf((_e).psi); \
393  const float cpsi = cosf((_e).psi); \
394  \
395  RMAT_ELMT(_rm, 0, 0) = ctheta*cpsi - sphi * stheta * spsi; \
396  RMAT_ELMT(_rm, 0, 1) = ctheta*spsi + sphi * stheta * cpsi; \
397  RMAT_ELMT(_rm, 0, 2) = -cphi * stheta; \
398  RMAT_ELMT(_rm, 1, 0) = -cphi * spsi; \
399  RMAT_ELMT(_rm, 1, 1) = cphi * cpsi; \
400  RMAT_ELMT(_rm, 1, 2) = sphi; \
401  RMAT_ELMT(_rm, 2, 0) = stheta*cpsi + sphi*ctheta*spsi; \
402  RMAT_ELMT(_rm, 2, 1) = stheta*spsi - sphi*ctheta*cpsi; \
403  RMAT_ELMT(_rm, 2, 2) = cphi*ctheta; \
404  \
405  }
406 
407 
408 /* C n->b rotation matrix */
409 #define FLOAT_RMAT_OF_QUAT(_rm, _q) { \
410  const float _a = M_SQRT2*(_q).qi; \
411  const float _b = M_SQRT2*(_q).qx; \
412  const float _c = M_SQRT2*(_q).qy; \
413  const float _d = M_SQRT2*(_q).qz; \
414  const float a2_1 = _a*_a-1; \
415  const float ab = _a*_b; \
416  const float ac = _a*_c; \
417  const float ad = _a*_d; \
418  const float bc = _b*_c; \
419  const float bd = _b*_d; \
420  const float cd = _c*_d; \
421  RMAT_ELMT(_rm, 0, 0) = a2_1+_b*_b; \
422  RMAT_ELMT(_rm, 0, 1) = bc+ad; \
423  RMAT_ELMT(_rm, 0, 2) = bd-ac; \
424  RMAT_ELMT(_rm, 1, 0) = bc-ad; \
425  RMAT_ELMT(_rm, 1, 1) = a2_1+_c*_c; \
426  RMAT_ELMT(_rm, 1, 2) = cd+ab; \
427  RMAT_ELMT(_rm, 2, 0) = bd+ac; \
428  RMAT_ELMT(_rm, 2, 1) = cd-ab; \
429  RMAT_ELMT(_rm, 2, 2) = a2_1+_d*_d; \
430  }
431 
432 /* in place first order integration of a rotation matrix */
433 #define FLOAT_RMAT_INTEGRATE_FI(_rm, _omega, _dt ) { \
434  struct FloatRMat exp_omega_dt = { \
435  { 1. , dt*omega.r, -dt*omega.q, \
436  -dt*omega.r, 1. , dt*omega.p, \
437  dt*omega.q, -dt*omega.p, 1. }}; \
438  struct FloatRMat R_tdt; \
439  FLOAT_RMAT_COMP(R_tdt, _rm, exp_omega_dt); \
440  memcpy(&(_rm), &R_tdt, sizeof(R_tdt)); \
441  }
442 
443 
444 static inline float renorm_factor(float n) {
445  if (n < 1.5625f && n > 0.64f)
446  return .5 * (3-n);
447  else if (n < 100.0f && n > 0.01f)
448  return 1. / sqrtf(n);
449  else
450  return 0.;
451 }
452 
453 static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
454 
455  const struct FloatVect3 r0 = {RMAT_ELMT(*rm, 0,0),
456  RMAT_ELMT(*rm, 0,1),
457  RMAT_ELMT(*rm, 0,2)};
458  const struct FloatVect3 r1 = {RMAT_ELMT(*rm, 1,0),
459  RMAT_ELMT(*rm, 1,1),
460  RMAT_ELMT(*rm, 1,2)};
461  float _err = -0.5*FLOAT_VECT3_DOT_PRODUCT(r0, r1);
462  struct FloatVect3 r0_t;
463  VECT3_SUM_SCALED(r0_t, r0, r1, _err);
464  struct FloatVect3 r1_t;
465  VECT3_SUM_SCALED(r1_t, r1, r0, _err);
466  struct FloatVect3 r2_t;
467  FLOAT_VECT3_CROSS_PRODUCT(r2_t, r0_t, r1_t);
468  float s = renorm_factor(FLOAT_VECT3_NORM2(r0_t));
469  MAT33_ROW_VECT3_SMUL(*rm, 0, r0_t, s);
470  s = renorm_factor(FLOAT_VECT3_NORM2(r1_t));
471  MAT33_ROW_VECT3_SMUL(*rm, 1, r1_t, s);
472  s = renorm_factor(FLOAT_VECT3_NORM2(r2_t));
473  MAT33_ROW_VECT3_SMUL(*rm, 2, r2_t, s);
474 
475  return _err;
476 
477 }
478 
479 //
480 //
481 // Quaternion algebras
482 //
483 //
484 
485 #define FLOAT_QUAT_ZERO(_q) QUAT_ASSIGN(_q, 1., 0., 0., 0.)
486 
487 #define FLOAT_QUAT_ASSIGN(_qi, _i, _x, _y, _z) QUAT_ASSIGN(_qi, _i, _x, _y, _z)
488 
489 /* _q += _qa */
490 #define FLOAT_QUAT_ADD(_qo, _qi) QUAT_ADD(_qo, _qi)
491 
492 /* _qo = _qi * _s */
493 #define FLOAT_QUAT_SMUL(_qo, _qi, _s) QUAT_SMUL(_qo, _qi, _s)
494 
495 /* _qo = _qo / _s */
496 #define FLOAT_QUAT_SDIV( _qo, _qi, _s) QUAT_SDIV(_qo, _qi, _s)
497 
498 /* */
499 #define FLOAT_QUAT_EXPLEMENTARY(b,a) QUAT_EXPLEMENTARY(b,a)
500 
501 /* */
502 #define FLOAT_QUAT_COPY(_qo, _qi) QUAT_COPY(_qo, _qi)
503 
504 #define FLOAT_QUAT_NORM(_q) (sqrtf(SQUARE((_q).qi) + SQUARE((_q).qx)+ \
505  SQUARE((_q).qy) + SQUARE((_q).qz)))
506 
507 #define FLOAT_QUAT_NORMALIZE(_q) { \
508  float qnorm = FLOAT_QUAT_NORM(_q); \
509  if (qnorm > FLT_MIN) { \
510  (_q).qi = (_q).qi / qnorm; \
511  (_q).qx = (_q).qx / qnorm; \
512  (_q).qy = (_q).qy / qnorm; \
513  (_q).qz = (_q).qz / qnorm; \
514  } \
515  }
516 
517 /* */
518 #define FLOAT_QUAT_EXTRACT(_vo, _qi) QUAT_EXTRACT_Q(_vo, _qi)
519 
520 /* Be careful : after invert make a normalization */
521 #define FLOAT_QUAT_INVERT(_qo, _qi) QUAT_INVERT(_qo, _qi)
522 
523 #define FLOAT_QUAT_WRAP_SHORTEST(_q) { \
524  if ((_q).qi < 0.) \
525  QUAT_EXPLEMENTARY(_q,_q); \
526  }
527 
528 /*
529  *
530  * Rotation Matrix using quaternions
531  *
532  */
533 
534  /*
535  * The (non commutative) quaternion product * then reads
536  *
537  * [ p0.q0 - p.q ]
538  * p * q = [ ]
539  * [ p0.q + q0.p + p x q ]
540  *
541  */
542 
543  /* (qi)-1 * vi * qi represents R_q of n->b on vectors vi
544  *
545  * "FLOAT_QUAT_EXTRACT : Extracted of the vector part"
546  */
547 
548 #define FLOAT_QUAT_RMAT_N2B(_n2b, _qi, _vi){ \
549  \
550  struct FloatQuat quatinv; \
551  struct FloatVect3 quat3, v1, v2; \
552  float qi; \
553  \
554  FLOAT_QUAT_INVERT(quatinv, _qi); \
555  FLOAT_QUAT_NORMALIZE(quatinv); \
556  \
557  FLOAT_QUAT_EXTRACT(quat3, quatinv); \
558  qi = - FLOAT_VECT3_DOT_PRODUCT(quat3, _vi); \
559  FLOAT_VECT3_CROSS_PRODUCT(v1, quat3, _vi); \
560  FLOAT_VECT3_SMUL(v2, _vi, (quatinv.qi)) ; \
561  FLOAT_VECT3_ADD(v2, v1); \
562  \
563  FLOAT_QUAT_EXTRACT(quat3, _qi); \
564  FLOAT_VECT3_CROSS_PRODUCT(_n2b, v2, quat3); \
565  FLOAT_VECT3_SMUL(v1, v2, (_qi).qi); \
566  FLOAT_VECT3_ADD(_n2b,v1); \
567  FLOAT_VECT3_SMUL(v1, quat3, qi); \
568  FLOAT_VECT3_ADD(_n2b,v1); \
569 }
570 
571  /*
572  * qi * vi * (qi)-1 represents R_q of b->n on vectors vi
573  */
574 #define FLOAT_QUAT_RMAT_B2N(_b2n,_qi,_vi){ \
575  \
576  struct FloatQuat _quatinv; \
577  \
578  \
579  FLOAT_QUAT_INVERT(_quatinv, _qi); \
580  FLOAT_QUAT_NORMALIZE(_quatinv); \
581  \
582  FLOAT_QUAT_RMAT_N2B(_b2n, _quatinv, _vi); \
583 }
584 
585  /* Right multiplication by a quaternion
586  *
587  * vi * qi
588  *
589  */
590 #define FLOAT_QUAT_VMUL_RIGHT(_mright,_qi,_vi){ \
591  \
592  struct FloatVect3 quat3, v1, v2; \
593  float qi; \
594  \
595  FLOAT_QUAT_EXTRACT(quat3, _qi); \
596  qi = - FLOAT_VECT3_DOT_PRODUCT(_vi, quat3); \
597  FLOAT_VECT3_CROSS_PRODUCT(v1, _vi, quat3); \
598  FLOAT_VECT3_SMUL(v2, _vi, (_qi.qi)); \
599  FLOAT_VECT3_ADD(v2, v1); \
600  FLOAT_QUAT_ASSIGN(_mright, qi, v2.x, v2.y, v2.z);\
601 }
602 
603 
604  /* Left multiplication by a quaternion
605  *
606  * qi * vi
607  *
608  */
609 #define FLOAT_QUAT_VMUL_LEFT(_mleft,_qi,_vi){ \
610  \
611  struct FloatVect3 quat3, v1, v2; \
612  float qi; \
613  \
614  FLOAT_QUAT_EXTRACT(quat3, _qi); \
615  qi = - FLOAT_VECT3_DOT_PRODUCT(quat3, _vi); \
616  FLOAT_VECT3_CROSS_PRODUCT(v1, quat3, _vi); \
617  FLOAT_VECT3_SMUL(v2, _vi, (_qi.qi)); \
618  FLOAT_VECT3_ADD(v2, v1); \
619  FLOAT_QUAT_ASSIGN(_mleft, qi, v2.x, v2.y, v2.z);\
620 }
621 
622 
623 /* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
624 #define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \
625  FLOAT_QUAT_COMP(_a2c, _a2b, _b2c); \
626  FLOAT_QUAT_WRAP_SHORTEST(_a2c); \
627  FLOAT_QUAT_NORMALIZE(_a2c); \
628  }
629 
630 /* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
631 #define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c) { \
632  (_a2c).qi = (_a2b).qi*(_b2c).qi - (_a2b).qx*(_b2c).qx - (_a2b).qy*(_b2c).qy - (_a2b).qz*(_b2c).qz; \
633  (_a2c).qx = (_a2b).qi*(_b2c).qx + (_a2b).qx*(_b2c).qi + (_a2b).qy*(_b2c).qz - (_a2b).qz*(_b2c).qy; \
634  (_a2c).qy = (_a2b).qi*(_b2c).qy - (_a2b).qx*(_b2c).qz + (_a2b).qy*(_b2c).qi + (_a2b).qz*(_b2c).qx; \
635  (_a2c).qz = (_a2b).qi*(_b2c).qz + (_a2b).qx*(_b2c).qy - (_a2b).qy*(_b2c).qx + (_a2b).qz*(_b2c).qi; \
636  }
637 
638 #define FLOAT_QUAT_MULT(_a2c, _a2b, _b2c) FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
639 
640 /* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
641 #define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) { \
642  FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c); \
643  FLOAT_QUAT_WRAP_SHORTEST(_a2b); \
644  FLOAT_QUAT_NORMALIZE(_a2b); \
645  }
646 
647 /* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
648 #define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c) { \
649  (_a2b).qi = (_a2c).qi*(_b2c).qi + (_a2c).qx*(_b2c).qx + (_a2c).qy*(_b2c).qy + (_a2c).qz*(_b2c).qz; \
650  (_a2b).qx = -(_a2c).qi*(_b2c).qx + (_a2c).qx*(_b2c).qi - (_a2c).qy*(_b2c).qz + (_a2c).qz*(_b2c).qy; \
651  (_a2b).qy = -(_a2c).qi*(_b2c).qy + (_a2c).qx*(_b2c).qz + (_a2c).qy*(_b2c).qi - (_a2c).qz*(_b2c).qx; \
652  (_a2b).qz = -(_a2c).qi*(_b2c).qz - (_a2c).qx*(_b2c).qy + (_a2c).qy*(_b2c).qx + (_a2c).qz*(_b2c).qi; \
653  }
654 
655 /* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
656 #define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) { \
657  FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c); \
658  FLOAT_QUAT_WRAP_SHORTEST(_b2c); \
659  FLOAT_QUAT_NORMALIZE(_b2c); \
660  }
661 
662 /* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
663 #define FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c) { \
664  (_b2c).qi = (_a2b).qi*(_a2c).qi + (_a2b).qx*(_a2c).qx + (_a2b).qy*(_a2c).qy + (_a2b).qz*(_a2c).qz; \
665  (_b2c).qx = (_a2b).qi*(_a2c).qx - (_a2b).qx*(_a2c).qi - (_a2b).qy*(_a2c).qz + (_a2b).qz*(_a2c).qy; \
666  (_b2c).qy = (_a2b).qi*(_a2c).qy + (_a2b).qx*(_a2c).qz - (_a2b).qy*(_a2c).qi - (_a2b).qz*(_a2c).qx; \
667  (_b2c).qz = (_a2b).qi*(_a2c).qz - (_a2b).qx*(_a2c).qy + (_a2b).qy*(_a2c).qx - (_a2b).qz*(_a2c).qi; \
668  }
669 
670 #define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt) { \
671  const float v_norm = sqrt((w).p*(w).p + (w).q*(w).q + (w).r*(w).r); \
672  const float c2 = cos(dt*v_norm/2.0); \
673  const float s2 = sin(dt*v_norm/2.0); \
674  if (v_norm < 1e-8) { \
675  (q_out).qi = 1; \
676  (q_out).qx = 0; \
677  (q_out).qy = 0; \
678  (q_out).qz = 0; \
679  } else { \
680  (q_out).qi = c2; \
681  (q_out).qx = (w).p/v_norm * s2; \
682  (q_out).qy = (w).q/v_norm * s2; \
683  (q_out).qz = (w).r/v_norm * s2; \
684  } \
685  }
686 
687 /* in place quaternion integration with constante rotational velocity */
688 #define FLOAT_QUAT_INTEGRATE(_q, _omega, _dt) { \
689  const float no = FLOAT_RATES_NORM(_omega); \
690  if (no > FLT_MIN) { \
691  const float a = 0.5*no*(_dt); \
692  const float ca = cosf(a); \
693  const float sa_ov_no = sinf(a)/no; \
694  const float dp = sa_ov_no*(_omega).p; \
695  const float dq = sa_ov_no*(_omega).q; \
696  const float dr = sa_ov_no*(_omega).r; \
697  const float qi = (_q).qi; \
698  const float qx = (_q).qx; \
699  const float qy = (_q).qy; \
700  const float qz = (_q).qz; \
701  (_q).qi = ca*qi - dp*qx - dq*qy - dr*qz; \
702  (_q).qx = dp*qi + ca*qx + dr*qy - dq*qz; \
703  (_q).qy = dq*qi - dr*qx + ca*qy + dp*qz; \
704  (_q).qz = dr*qi + dq*qx - dp*qy + ca*qz; \
705  } \
706  }
707 
708 #define FLOAT_QUAT_VMULT(v_out, q, v_in) { \
709  const float qi2_M1_2 = (q).qi*(q).qi - 0.5; \
710  const float qiqx = (q).qi*(q).qx; \
711  const float qiqy = (q).qi*(q).qy; \
712  const float qiqz = (q).qi*(q).qz; \
713  float m01 = (q).qx*(q).qy; /* aka qxqy */ \
714  float m02 = (q).qx*(q).qz; /* aka qxqz */ \
715  float m12 = (q).qy*(q).qz; /* aka qyqz */ \
716 \
717  const float m00 = qi2_M1_2 + (q).qx*(q).qx; \
718  const float m10 = m01 - qiqz; \
719  const float m20 = m02 + qiqy; \
720  const float m21 = m12 - qiqx; \
721  m01 += qiqz; \
722  m02 -= qiqy; \
723  m12 += qiqx; \
724  const float m11 = qi2_M1_2 + (q).qy*(q).qy; \
725  const float m22 = qi2_M1_2 + (q).qz*(q).qz; \
726  (v_out).x = 2*(m00 * (v_in).x + m01 * (v_in).y + m02 * (v_in).z); \
727  (v_out).y = 2*(m10 * (v_in).x + m11 * (v_in).y + m12 * (v_in).z); \
728  (v_out).z = 2*(m20 * (v_in).x + m21 * (v_in).y + m22 * (v_in).z); \
729  }
730 
731 /* _qd = -0.5*omega(_r) * _q */
732 #define FLOAT_QUAT_DERIVATIVE(_qd, _r, _q) { \
733  (_qd).qi = -0.5*( (_r).p*(_q).qx + (_r).q*(_q).qy + (_r).r*(_q).qz); \
734  (_qd).qx = -0.5*(-(_r).p*(_q).qi - (_r).r*(_q).qy + (_r).q*(_q).qz); \
735  (_qd).qy = -0.5*(-(_r).q*(_q).qi + (_r).r*(_q).qx - (_r).p*(_q).qz); \
736  (_qd).qz = -0.5*(-(_r).r*(_q).qi - (_r).q*(_q).qx + (_r).p*(_q).qy); \
737  }
738 
739 /* _qd = -0.5*omega(_r) * _q */
740 #define FLOAT_QUAT_DERIVATIVE_LAGRANGE(_qd, _r, _q) { \
741  const float K_LAGRANGE = 1.; \
742  const float c = K_LAGRANGE * ( 1 - FLOAT_QUAT_NORM(_q)) / -0.5; \
743  (_qd).qi = -0.5*( c*(_q).qi + (_r).p*(_q).qx + (_r).q*(_q).qy + (_r).r*(_q).qz); \
744  (_qd).qx = -0.5*(-(_r).p*(_q).qi + c*(_q).qx - (_r).r*(_q).qy + (_r).q*(_q).qz); \
745  (_qd).qy = -0.5*(-(_r).q*(_q).qi + (_r).r*(_q).qx + c*(_q).qy - (_r).p*(_q).qz); \
746  (_qd).qz = -0.5*(-(_r).r*(_q).qi - (_r).q*(_q).qx + (_r).p*(_q).qy + c*(_q).qz); \
747  }
748 
749 #define FLOAT_QUAT_OF_EULERS(_q, _e) { \
750  \
751  const float phi2 = (_e).phi/2.0; \
752  const float theta2 = (_e).theta/2.0; \
753  const float psi2 = (_e).psi/2.0; \
754  \
755  const float s_phi2 = sinf( phi2 ); \
756  const float c_phi2 = cosf( phi2 ); \
757  const float s_theta2 = sinf( theta2 ); \
758  const float c_theta2 = cosf( theta2 ); \
759  const float s_psi2 = sinf( psi2 ); \
760  const float c_psi2 = cosf( psi2 ); \
761  \
762  (_q).qi = c_phi2 * c_theta2 * c_psi2 + s_phi2 * s_theta2 * s_psi2; \
763  (_q).qx = -c_phi2 * s_theta2 * s_psi2 + s_phi2 * c_theta2 * c_psi2; \
764  (_q).qy = c_phi2 * s_theta2 * c_psi2 + s_phi2 * c_theta2 * s_psi2; \
765  (_q).qz = c_phi2 * c_theta2 * s_psi2 - s_phi2 * s_theta2 * c_psi2; \
766  \
767  }
768 
769 #define FLOAT_QUAT_OF_AXIS_ANGLE(_q, _uv, _an) { \
770  const float san = sinf((_an)/2.); \
771  (_q).qi = cosf((_an)/2.); \
772  (_q).qx = san * (_uv).x; \
773  (_q).qy = san * (_uv).y; \
774  (_q).qz = san * (_uv).z; \
775  }
776 
777 #define FLOAT_QUAT_OF_ORIENTATION_VECT(_q, _ov) { \
778  const float ov_norm = sqrtf((_ov).x*(_ov).x + (_ov).y*(_ov).y + (_ov).z*(_ov).z); \
779  if (ov_norm < 1e-8) { \
780  (_q).qi = 1; \
781  (_q).qx = 0; \
782  (_q).qy = 0; \
783  (_q).qz = 0; \
784  } else { \
785  const float s2_normalized = sinf(ov_norm/2.0) / ov_norm; \
786  (_q).qi = cosf(ov_norm/2.0); \
787  (_q).qx = (_ov).x * s2_normalized; \
788  (_q).qy = (_ov).y * s2_normalized; \
789  (_q).qz = (_ov).z * s2_normalized; \
790  } \
791  }
792 
793 #define FLOAT_QUAT_OF_RMAT(_q, _r) { \
794  const float tr = RMAT_TRACE((_r)); \
795  if (tr > 0) { \
796  const float two_qi = sqrtf(1.+tr); \
797  const float four_qi = 2. * two_qi; \
798  (_q).qi = 0.5 * two_qi; \
799  (_q).qx = (RMAT_ELMT((_r), 1, 2)-RMAT_ELMT((_r), 2, 1))/four_qi; \
800  (_q).qy = (RMAT_ELMT((_r), 2, 0)-RMAT_ELMT((_r), 0, 2))/four_qi; \
801  (_q).qz = (RMAT_ELMT((_r), 0, 1)-RMAT_ELMT((_r), 1, 0))/four_qi; \
802  /*printf("tr > 0\n");*/ \
803  } \
804  else { \
805  if (RMAT_ELMT((_r), 0, 0) > RMAT_ELMT((_r), 1, 1) && \
806  RMAT_ELMT((_r), 0, 0) > RMAT_ELMT((_r), 2, 2)) { \
807  const float two_qx = sqrtf(RMAT_ELMT((_r), 0, 0) -RMAT_ELMT((_r), 1, 1) \
808  -RMAT_ELMT((_r), 2, 2) + 1); \
809  const float four_qx = 2. * two_qx; \
810  (_q).qi = (RMAT_ELMT((_r), 1, 2)-RMAT_ELMT((_r), 2, 1))/four_qx; \
811  (_q).qx = 0.5 * two_qx; \
812  (_q).qy = (RMAT_ELMT((_r), 0, 1)+RMAT_ELMT((_r), 1, 0))/four_qx; \
813  (_q).qz = (RMAT_ELMT((_r), 2, 0)+RMAT_ELMT((_r), 0, 2))/four_qx; \
814  /*printf("m00 largest\n");*/ \
815  } \
816  else if (RMAT_ELMT((_r), 1, 1) > RMAT_ELMT((_r), 2, 2)) { \
817  const float two_qy = \
818  sqrtf(RMAT_ELMT((_r), 1, 1) - RMAT_ELMT((_r), 0, 0) - RMAT_ELMT((_r), 2, 2) + 1); \
819  const float four_qy = 2. * two_qy; \
820  (_q).qi = (RMAT_ELMT((_r), 2, 0) - RMAT_ELMT((_r), 0, 2))/four_qy; \
821  (_q).qx = (RMAT_ELMT((_r), 0, 1) + RMAT_ELMT((_r), 1, 0))/four_qy; \
822  (_q).qy = 0.5 * two_qy; \
823  (_q).qz = (RMAT_ELMT((_r), 1, 2) + RMAT_ELMT((_r), 2, 1))/four_qy; \
824  /*printf("m11 largest\n");*/ \
825  } \
826  else { \
827  const float two_qz = \
828  sqrtf(RMAT_ELMT((_r), 2, 2) - RMAT_ELMT((_r), 0, 0) - RMAT_ELMT((_r), 1, 1) + 1); \
829  const float four_qz = 2. * two_qz; \
830  (_q).qi = (RMAT_ELMT((_r), 0, 1)- RMAT_ELMT((_r), 1, 0))/four_qz; \
831  (_q).qx = (RMAT_ELMT((_r), 2, 0)+ RMAT_ELMT((_r), 0, 2))/four_qz; \
832  (_q).qy = (RMAT_ELMT((_r), 1, 2)+ RMAT_ELMT((_r), 2, 1))/four_qz; \
833  (_q).qz = 0.5 * two_qz; \
834  /*printf("m22 largest\n");*/ \
835  } \
836  } \
837  }
838 
839 
840 
841 //
842 //
843 // Euler angles
844 //
845 //
846 
847 #define FLOAT_EULERS_ZERO(_e) EULERS_ASSIGN(_e, 0., 0., 0.);
848 
849 #define FLOAT_EULERS_NORM(_e) (sqrtf(SQUARE((_e).phi)+SQUARE((_e).theta)+SQUARE((_e).psi))) ;
850 
851 #define FLOAT_EULERS_OF_RMAT(_e, _rm) { \
852  \
853  const float dcm00 = (_rm).m[0]; \
854  const float dcm01 = (_rm).m[1]; \
855  const float dcm02 = (_rm).m[2]; \
856  const float dcm12 = (_rm).m[5]; \
857  const float dcm22 = (_rm).m[8]; \
858  (_e).phi = atan2f( dcm12, dcm22 ); \
859  (_e).theta = -asinf( dcm02 ); \
860  (_e).psi = atan2f( dcm01, dcm00 ); \
861  \
862  }
863 
864 #define FLOAT_EULERS_OF_QUAT(_e, _q) { \
865  \
866  const float qx2 = (_q).qx*(_q).qx; \
867  const float qy2 = (_q).qy*(_q).qy; \
868  const float qz2 = (_q).qz*(_q).qz; \
869  const float qiqx = (_q).qi*(_q).qx; \
870  const float qiqy = (_q).qi*(_q).qy; \
871  const float qiqz = (_q).qi*(_q).qz; \
872  const float qxqy = (_q).qx*(_q).qy; \
873  const float qxqz = (_q).qx*(_q).qz; \
874  const float qyqz = (_q).qy*(_q).qz; \
875  const float dcm00 = 1.0 - 2.*( qy2 + qz2 ); \
876  const float dcm01 = 2.*( qxqy + qiqz ); \
877  const float dcm02 = 2.*( qxqz - qiqy ); \
878  const float dcm12 = 2.*( qyqz + qiqx ); \
879  const float dcm22 = 1.0 - 2.*( qx2 + qy2 ); \
880  \
881  (_e).phi = atan2f( dcm12, dcm22 ); \
882  (_e).theta = -asinf( dcm02 ); \
883  (_e).psi = atan2f( dcm01, dcm00 ); \
884  \
885  }
886 
887 
888 //
889 //
890 // Generic vector algebra
891 //
892 //
893 
895 static inline void float_vect_zero(float * a, const int n) {
896  int i;
897  for (i = 0; i < n; i++) { a[i] = 0.; }
898 }
899 
901 static inline void float_vect_copy(float * a, const float * b, const int n) {
902  int i;
903  for (i = 0; i < n; i++) { a[i] = b[i]; }
904 }
905 
907 static inline void float_vect_sum(float * o, const float * a, const float * b, const int n) {
908  int i;
909  for (i = 0; i < n; i++) { o[i] = a[i] + b[i]; }
910 }
911 
913 static inline void float_vect_diff(float * o, const float * a, const float * b, const int n) {
914  int i;
915  for (i = 0; i < n; i++) { o[i] = a[i] - b[i]; }
916 }
917 
919 static inline void float_vect_mul(float * o, const float * a, const float * b, const int n) {
920  int i;
921  for (i = 0; i < n; i++) { o[i] = a[i] * b[i]; }
922 }
923 
925 static inline void float_vect_add(float * a, const float * b, const int n) {
926  int i;
927  for (i = 0; i < n; i++) { a[i] += b[i]; }
928 }
929 
931 static inline void float_vect_sub(float * a, const float * b, const int n) {
932  int i;
933  for (i = 0; i < n; i++) { a[i] -= b[i]; }
934 }
935 
937 static inline void float_vect_smul(float * o, const float * a, const float s, const int n) {
938  int i;
939  for (i = 0; i < n; i++) { o[i] = a[i] * s; }
940 }
941 
943 static inline void float_vect_sdiv(float * o, const float * a, const float s, const int n) {
944  int i;
945  if ( fabs(s) > 1e-5 ) {
946  for (i = 0; i < n; i++) { o[i] = a[i] / s; }
947  }
948 }
949 
951 static inline float float_vect_norm(const float * a, const int n) {
952  int i;
953  float sum = 0;
954  for (i = 0; i < n; i++) { sum += a[i] * a[i]; }
955  return sqrtf(sum);
956 }
957 
958 //
959 //
960 // Generic matrix algebra
961 //
962 //
963 
965 #define MAKE_MATRIX_PTR(_ptr, _mat, _rows) \
966  float * _ptr[_rows]; \
967  { \
968  int i; \
969  for (i = 0; i < _rows; i++) { _ptr[i] = &_mat[i][0]; } \
970  }
971 
973 static inline void float_mat_zero(float ** a, int m, int n) {
974  int i,j;
975  for (i = 0; i < m; i++) {
976  for (j = 0; j < n; j++) { a[i][j] = 0.; }
977  }
978 }
979 
981 static inline void float_mat_copy(float ** a, float ** b, int m, int n) {
982  int i,j;
983  for (i = 0; i < m; i++) {
984  for (j = 0; j < n; j++) { a[i][j] = b[i][j]; }
985  }
986 }
987 
989 static inline void float_mat_sum(float ** o, float ** a, float ** b, int m, int n) {
990  int i,j;
991  for (i = 0; i < m; i++) {
992  for (j = 0; j < n; j++) { o[i][j] = a[i][j] + b[i][j]; }
993  }
994 }
995 
997 static inline void float_mat_diff(float ** o, float ** a, float ** b, int m, int n) {
998  int i,j;
999  for (i = 0; i < m; i++) {
1000  for (j = 0; j < n; j++) { o[i][j] = a[i][j] - b[i][j]; }
1001  }
1002 }
1003 
1005 static inline void float_mat_transpose(float ** a, int n) {
1006  int i,j;
1007  for (i = 0; i < n; i++) {
1008  for (j = 0; j < i; j++) {
1009  float t = a[i][j];
1010  a[i][j] = a[j][i];
1011  a[j][i] = t;
1012  }
1013  }
1014 }
1015 
1022 static inline void float_mat_mul(float ** o, float ** a, float ** b, int m, int n, int l) {
1023  int i,j,k;
1024  for (i = 0; i < m; i++) {
1025  for (j = 0; j < l; j++) {
1026  o[i][j] = 0.;
1027  for (k = 0; k < n; k++) {
1028  o[i][j] += a[i][k] * b[k][j];
1029  }
1030  }
1031  }
1032 }
1033 
1040 static inline void float_mat_minor(float ** o, float ** a, int m, int n, int d) {
1041  int i,j;
1042  float_mat_zero(o, m, n);
1043  for (i = 0; i < d; i++) { o[i][i] = 1.0; }
1044  for (i = d; i < m; i++) {
1045  for (j = d; j < n; j++) {
1046  o[i][j] = a[i][j];
1047  }
1048  }
1049 }
1050 
1052 static inline void float_mat_vmul(float ** o, float * v, int n)
1053 {
1054  int i,j;
1055  for (i = 0; i < n; i++) {
1056  for (j = 0; j < n; j++) {
1057  o[i][j] = -2. * v[i] * v[j];
1058  }
1059  }
1060  for (i = 0; i < n; i++) {
1061  o[i][i] += 1.;
1062  }
1063 }
1064 
1066 static inline void float_mat_col(float * o, float ** a, int m, int c) {
1067  int i;
1068  for (i = 0; i < m; i++) {
1069  o[i] = a[i][c];
1070  }
1071 }
1072 
1073 #endif /* PPRZ_ALGEBRA_FLOAT_H */
rotation matrix
#define FLOAT_VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
static void float_vect_zero(float *a, const int n)
a = 0
#define VECT3_SUM_SCALED(_c, _a, _b, _s)
Definition: pprz_algebra.h:164
float m[3 *3]
angular rates
float psi
in radians
static void float_mat_diff(float **o, float **a, float **b, int m, int n)
o = a - b
float m[3 *3]
#define FLOAT_VECT3_DOT_PRODUCT(_v1, _v2)
float theta
in radians
static void float_mat_vmul(float **o, float *v, int n)
o = I - v v^T
static void float_vect_sum(float *o, const float *a, const float *b, const int n)
o = a + b
euler angles
float p
in rad/s
Roation quaternion.
#define MAT33_ROW_VECT3_SMUL(_mat, _row, _vin, _s)
Definition: pprz_algebra.h:482
static void float_mat_minor(float **o, float **a, int m, int n, int d)
matrix minor
static float float_vect_norm(const float *a, const int n)
||a||
float phi
in radians
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_copy(float *a, const float *b, const int n)
a = b
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:573
float r
in rad/s
static void float_vect_sdiv(float *o, const float *a, const float s, const int n)
o = a / s
static void float_vect_sub(float *a, const float *b, const int n)
a -= b
static void float_vect_diff(float *o, const float *a, const float *b, const int n)
o = a - b
static void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
o = a * b
static void float_vect_add(float *a, const float *b, const int n)
a += b
static void float_mat_zero(float **a, int m, int n)
a = 0
static float renorm_factor(float n)
static void float_mat_transpose(float **a, int n)
transpose square matrix
static float float_rmat_reorthogonalize(struct FloatRMat *rm)
static void float_mat_sum(float **o, float **a, float **b, int m, int n)
o = a + b
float q
in rad/s
static void float_vect_mul(float *o, const float *a, const float *b, const int n)
o = a * b (element wise)
static void float_mat_copy(float **a, float **b, int m, int n)
a = b
static void float_vect_smul(float *o, const float *a, const float s, const int n)
o = a * s
#define FLOAT_VECT3_NORM2(_v)