Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros 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  * Dimension 2 Vectors
101  */
102 
103 #define FLOAT_VECT2_ZERO(_v) VECT2_ASSIGN(_v, 0., 0.)
104 
105 /* a = {x, y} */
106 #define FLOAT_VECT2_ASSIGN(_a, _x, _y) VECT2_ASSIGN(_a, _x, _y)
107 
108 /* a = b */
109 #define FLOAT_VECT2_COPY(_a, _b) VECT2_COPY(_a, _b)
110 
111 /* a += b */
112 #define FLOAT_VECT2_ADD(_a, _b) VECT2_ADD(_a, _b)
113 
114 /* c = a + b */
115 #define FLOAT_VECT2_SUM(_c, _a, _b) VECT2_SUM(_c, _a, _b)
116 
117 /* c = a - b */
118 #define FLOAT_VECT2_DIFF(_c, _a, _b) VECT2_DIFF(_c, _a, _b)
119 
120 /* a -= b */
121 #define FLOAT_VECT2_SUB(_a, _b) VECT2_SUB(_a, _b)
122 
123 /* _vo = _vi * _s */
124 #define FLOAT_VECT2_SMUL(_vo, _vi, _s) VECT2_SMUL(_vo, _vi, _s)
125 
126 
127 /*
128  * Dimension 3 Vectors
129  */
130 
131 #define FLOAT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0., 0., 0.)
132 
133 #define FLOAT_VECT3_ASSIGN(_a, _x, _y, _z) VECT3_ASSIGN(_a, _x, _y, _z)
134 
135 /* c = a - b */
136 #define FLOAT_VECT3_DIFF(_c, _a, _b) VECT3_DIFF(_c, _a, _b)
137 
138 /* a -= b */
139 #define FLOAT_VECT3_SUB(_a, _b) VECT3_SUB(_a, _b)
140 
141 /* _vo = _vi * _s */
142 #define FLOAT_VECT3_SMUL(_vo, _vi, _s) VECT3_SMUL(_vo, _vi, _s)
143 
144 #define FLOAT_VECT3_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z)
145 
146 #define FLOAT_VECT3_NORM(_v) (sqrtf(FLOAT_VECT3_NORM2(_v)))
147 
148 #define FLOAT_VECT3_DOT_PRODUCT(_v1, _v2) ((_v1).x*(_v2).x + (_v1).y*(_v2).y + (_v1).z*(_v2).z)
149 
150 #define FLOAT_VECT3_CROSS_PRODUCT(_vo, _v1, _v2) { \
151  (_vo).x = (_v1).y*(_v2).z - (_v1).z*(_v2).y; \
152  (_vo).y = (_v1).z*(_v2).x - (_v1).x*(_v2).z; \
153  (_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x; \
154  }
155 
156 #define FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt) { \
157  (_vo).x += (_dv).x * (_dt); \
158  (_vo).y += (_dv).y * (_dt); \
159  (_vo).z += (_dv).z * (_dt); \
160  }
161 
162 
163 
164 #define FLOAT_VECT3_NORMALIZE(_v) { \
165  const float n = FLOAT_VECT3_NORM(_v); \
166  FLOAT_VECT3_SMUL(_v, _v, 1./n); \
167  }
168 
169 #define FLOAT_RATES_ZERO(_r) { \
170  RATES_ASSIGN(_r, 0., 0., 0.); \
171  }
172 
173 #define FLOAT_RATES_NORM(_v) (sqrtf((_v).p*(_v).p + (_v).q*(_v).q + (_v).r*(_v).r))
174 
175 #define FLOAT_RATES_ADD_SCALED_VECT(_ro, _v, _s) { \
176  _ro.p += _v.x * _s; \
177  _ro.q += _v.y * _s; \
178  _ro.r += _v.z * _s; \
179  }
180 
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 #define FLOAT_RATES_SCALE(_ro,_s) { \
190  _ro.p *= _s; \
191  _ro.q *= _s; \
192  _ro.r *= _s; \
193  }
194 
195 #define FLOAT_RATES_INTEGRATE_FI(_ra, _racc, _dt) { \
196  (_ra).p += (_racc).p * (_dt); \
197  (_ra).q += (_racc).q * (_dt); \
198  (_ra).r += (_racc).r * (_dt); \
199  }
200 
201 #define FLOAT_RATES_OF_EULER_DOT(_ra, _e, _ed) { \
202  _ra.p = _ed.phi - sinf(_e.theta) *_ed.psi; \
203  _ra.q = cosf(_e.phi)*_ed.theta + sinf(_e.phi)*cosf(_e.theta)*_ed.psi; \
204  _ra.r = -sinf(_e.phi)*_ed.theta + cosf(_e.phi)*cosf(_e.theta)*_ed.psi; \
205  }
206 
207 
208 
209 /*
210  * 3x3 matrices
211  */
212 #define FLOAT_MAT33_ZERO(_m) { \
213  MAT33_ELMT(_m, 0, 0) = 0.; \
214  MAT33_ELMT(_m, 0, 1) = 0.; \
215  MAT33_ELMT(_m, 0, 2) = 0.; \
216  MAT33_ELMT(_m, 1, 0) = 0.; \
217  MAT33_ELMT(_m, 1, 1) = 0.; \
218  MAT33_ELMT(_m, 1, 2) = 0.; \
219  MAT33_ELMT(_m, 2, 0) = 0.; \
220  MAT33_ELMT(_m, 2, 1) = 0.; \
221  MAT33_ELMT(_m, 2, 2) = 0.; \
222  }
223 
224 #define FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22) { \
225  MAT33_ELMT(_m, 0, 0) = _d00; \
226  MAT33_ELMT(_m, 0, 1) = 0.; \
227  MAT33_ELMT(_m, 0, 2) = 0.; \
228  MAT33_ELMT(_m, 1, 0) = 0.; \
229  MAT33_ELMT(_m, 1, 1) = _d11; \
230  MAT33_ELMT(_m, 1, 2) = 0.; \
231  MAT33_ELMT(_m, 2, 0) = 0.; \
232  MAT33_ELMT(_m, 2, 1) = 0.; \
233  MAT33_ELMT(_m, 2, 2) = _d22; \
234  }
235 
236 
237 /*
238  * Rotation Matrices
239  */
240 
241 /* initialises a matrix to identity */
242 #define FLOAT_RMAT_ZERO(_rm) FLOAT_MAT33_DIAG(_rm, 1., 1., 1.)
243 
244 /* initialises a rotation matrix from unit vector axis and angle */
245 #define FLOAT_RMAT_OF_AXIS_ANGLE(_rm, _uv, _an) { \
246  \
247  const float ux2 = _uv.x*_uv.x; \
248  const float uy2 = _uv.y*_uv.y; \
249  const float uz2 = _uv.z*_uv.z; \
250  const float uxuy = _uv.x*_uv.y; \
251  const float uyuz = _uv.y*_uv.z; \
252  const float uxuz = _uv.x*_uv.z; \
253  const float can = cosf(_an); \
254  const float san = sinf(_an); \
255  const float one_m_can = (1. - can); \
256  \
257  RMAT_ELMT(_rm, 0, 0) = ux2 + (1.-ux2)*can; \
258  RMAT_ELMT(_rm, 0, 1) = uxuy*one_m_can + _uv.z*san; \
259  RMAT_ELMT(_rm, 0, 2) = uxuz*one_m_can - _uv.y*san; \
260  RMAT_ELMT(_rm, 1, 0) = uxuy*one_m_can - _uv.z*san; \
261  RMAT_ELMT(_rm, 1, 1) = uy2 + (1.-uy2)*can; \
262  RMAT_ELMT(_rm, 1, 2) = uyuz*one_m_can + _uv.x*san; \
263  RMAT_ELMT(_rm, 2, 0) = uxuz*one_m_can + _uv.y*san; \
264  RMAT_ELMT(_rm, 2, 1) = uyuz*one_m_can - _uv.x*san; \
265  RMAT_ELMT(_rm, 2, 2) = uz2 + (1.-uz2)*can; \
266  \
267  }
268 
269 /* multiply _vin by _rmat, store in _vout */
270 #define FLOAT_RMAT_VECT3_MUL(_vout, _rmat, _vin) RMAT_VECT3_MUL(_vout, _rmat, _vin)
271 #define FLOAT_RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin) RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin)
272 
273 #define FLOAT_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va) { \
274  (_vb).p = ( (_m_b2a).m[0]*(_va).p + (_m_b2a).m[3]*(_va).q + (_m_b2a).m[6]*(_va).r); \
275  (_vb).q = ( (_m_b2a).m[1]*(_va).p + (_m_b2a).m[4]*(_va).q + (_m_b2a).m[7]*(_va).r); \
276  (_vb).r = ( (_m_b2a).m[2]*(_va).p + (_m_b2a).m[5]*(_va).q + (_m_b2a).m[8]*(_va).r); \
277  }
278 
279 #define FLOAT_RMAT_RATEMULT(_vb, _m_a2b, _va) { \
280  (_vb).p = ( (_m_a2b).m[0]*(_va).p + (_m_a2b).m[1]*(_va).q + (_m_a2b).m[2]*(_va).r); \
281  (_vb).q = ( (_m_a2b).m[3]*(_va).p + (_m_a2b).m[4]*(_va).q + (_m_a2b).m[5]*(_va).r); \
282  (_vb).r = ( (_m_a2b).m[6]*(_va).p + (_m_a2b).m[7]*(_va).q + (_m_a2b).m[8]*(_va).r); \
283  }
284 
285 /* _m_a2c = _m_a2b comp _m_b2c , aka _m_a2c = _m_b2c * _m_a2b */
286 #define FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c) { \
287  _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]); \
288  _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]); \
289  _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]); \
290  _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]); \
291  _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]); \
292  _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]); \
293  _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]); \
294  _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]); \
295  _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]); \
296  }
297 
298 
299 /* _m_a2b = _m_a2c comp_inv _m_b2c , aka _m_a2b = inv(_m_b2c) * _m_a2c */
300 #define FLOAT_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c) { \
301  _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]); \
302  _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]); \
303  _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]); \
304  _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]); \
305  _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]); \
306  _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]); \
307  _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]); \
308  _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]); \
309  _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]); \
310  }
311 
312 
313 /* _m_b2a = inv(_m_a2b) = transp(_m_a2b) */
314 #define FLOAT_RMAT_INV(_m_b2a, _m_a2b) { \
315  RMAT_ELMT(_m_b2a, 0, 0) = RMAT_ELMT(_m_a2b, 0, 0); \
316  RMAT_ELMT(_m_b2a, 0, 1) = RMAT_ELMT(_m_a2b, 1, 0); \
317  RMAT_ELMT(_m_b2a, 0, 2) = RMAT_ELMT(_m_a2b, 2, 0); \
318  RMAT_ELMT(_m_b2a, 1, 0) = RMAT_ELMT(_m_a2b, 0, 1); \
319  RMAT_ELMT(_m_b2a, 1, 1) = RMAT_ELMT(_m_a2b, 1, 1); \
320  RMAT_ELMT(_m_b2a, 1, 2) = RMAT_ELMT(_m_a2b, 2, 1); \
321  RMAT_ELMT(_m_b2a, 2, 0) = RMAT_ELMT(_m_a2b, 0, 2); \
322  RMAT_ELMT(_m_b2a, 2, 1) = RMAT_ELMT(_m_a2b, 1, 2); \
323  RMAT_ELMT(_m_b2a, 2, 2) = RMAT_ELMT(_m_a2b, 2, 2); \
324  }
325 
326 #define FLOAT_RMAT_NORM(_m) ( \
327  sqrtf(SQUARE((_m).m[0])+ SQUARE((_m).m[1])+ SQUARE((_m).m[2])+ \
328  SQUARE((_m).m[3])+ SQUARE((_m).m[4])+ SQUARE((_m).m[5])+ \
329  SQUARE((_m).m[6])+ SQUARE((_m).m[7])+ SQUARE((_m).m[8])) \
330  )
331 
332 #define FLOAT_RMAT_OF_EULERS(_rm, _e) FLOAT_RMAT_OF_EULERS_321(_rm, _e)
333 
334 /* C n->b rotation matrix */
335 #define FLOAT_RMAT_OF_EULERS_321(_rm, _e) { \
336  \
337  const float sphi = sinf((_e).phi); \
338  const float cphi = cosf((_e).phi); \
339  const float stheta = sinf((_e).theta); \
340  const float ctheta = cosf((_e).theta); \
341  const float spsi = sinf((_e).psi); \
342  const float cpsi = cosf((_e).psi); \
343  \
344  RMAT_ELMT(_rm, 0, 0) = ctheta*cpsi; \
345  RMAT_ELMT(_rm, 0, 1) = ctheta*spsi; \
346  RMAT_ELMT(_rm, 0, 2) = -stheta; \
347  RMAT_ELMT(_rm, 1, 0) = sphi*stheta*cpsi - cphi*spsi; \
348  RMAT_ELMT(_rm, 1, 1) = sphi*stheta*spsi + cphi*cpsi; \
349  RMAT_ELMT(_rm, 1, 2) = sphi*ctheta; \
350  RMAT_ELMT(_rm, 2, 0) = cphi*stheta*cpsi + sphi*spsi; \
351  RMAT_ELMT(_rm, 2, 1) = cphi*stheta*spsi - sphi*cpsi; \
352  RMAT_ELMT(_rm, 2, 2) = cphi*ctheta; \
353  \
354  }
355 
356 
357 #define FLOAT_RMAT_OF_EULERS_312(_rm, _e) { \
358  \
359  const float sphi = sinf((_e).phi); \
360  const float cphi = cosf((_e).phi); \
361  const float stheta = sinf((_e).theta); \
362  const float ctheta = cosf((_e).theta); \
363  const float spsi = sinf((_e).psi); \
364  const float cpsi = cosf((_e).psi); \
365  \
366  RMAT_ELMT(_rm, 0, 0) = ctheta*cpsi - sphi * stheta * spsi; \
367  RMAT_ELMT(_rm, 0, 1) = ctheta*spsi + sphi * stheta * cpsi; \
368  RMAT_ELMT(_rm, 0, 2) = -cphi * stheta; \
369  RMAT_ELMT(_rm, 1, 0) = -cphi * spsi; \
370  RMAT_ELMT(_rm, 1, 1) = cphi * cpsi; \
371  RMAT_ELMT(_rm, 1, 2) = sphi; \
372  RMAT_ELMT(_rm, 2, 0) = stheta*cpsi + sphi*ctheta*spsi; \
373  RMAT_ELMT(_rm, 2, 1) = stheta*spsi - sphi*ctheta*cpsi; \
374  RMAT_ELMT(_rm, 2, 2) = cphi*ctheta; \
375  \
376  }
377 
378 
379 /* C n->b rotation matrix */
380 #ifdef ALGEBRA_FLOAT_USE_SLOW_FUNCTIONS
381 #define FLOAT_RMAT_OF_QUAT(_rm, _q) { \
382  const float qx2 = (_q).qx*(_q).qx; \
383  const float qy2 = (_q).qy*(_q).qy; \
384  const float qz2 = (_q).qz*(_q).qz; \
385  const float qiqx = (_q).qi*(_q).qx; \
386  const float qiqy = (_q).qi*(_q).qy; \
387  const float qiqz = (_q).qi*(_q).qz; \
388  const float qxqy = (_q).qx*(_q).qy; \
389  const float qxqz = (_q).qx*(_q).qz; \
390  const float qyqz = (_q).qy*(_q).qz; \
391  /* dcm00 = 1.0 - 2.*( qy2 + qz2 ); */ \
392  RMAT_ELMT(_rm, 0, 0) = 1. - 2.*( qy2 + qz2 ); \
393  /* dcm01 = 2.*( qxqy + qiqz ); */ \
394  RMAT_ELMT(_rm, 0, 1) = 2.*( qxqy + qiqz ); \
395  /* dcm02 = 2.*( qxqz - qiqy ); */ \
396  RMAT_ELMT(_rm, 0, 2) = 2.*( qxqz - qiqy ); \
397  /* dcm10 = 2.*( qxqy - qiqz ); */ \
398  RMAT_ELMT(_rm, 1, 0) = 2.*( qxqy - qiqz ); \
399  /* dcm11 = 1.0 - 2.*(qx2+qz2); */ \
400  RMAT_ELMT(_rm, 1, 1) = 1.0 - 2.*(qx2+qz2); \
401  /* dcm12 = 2.*( qyqz + qiqx ); */ \
402  RMAT_ELMT(_rm, 1, 2) = 2.*( qyqz + qiqx ); \
403  /* dcm20 = 2.*( qxqz + qiqy ); */ \
404  RMAT_ELMT(_rm, 2, 0) = 2.*( qxqz + qiqy ); \
405  /* dcm21 = 2.*( qyqz - qiqx ); */ \
406  RMAT_ELMT(_rm, 2, 1) = 2.*( qyqz - qiqx ); \
407  /* dcm22 = 1.0 - 2.*( qx2 + qy2 ); */ \
408  RMAT_ELMT(_rm, 2, 2) = 1.0 - 2.*( qx2 + qy2 ); \
409  }
410 #else
411 #define FLOAT_RMAT_OF_QUAT(_rm, _q) { \
412  const float _a = M_SQRT2*(_q).qi; \
413  const float _b = M_SQRT2*(_q).qx; \
414  const float _c = M_SQRT2*(_q).qy; \
415  const float _d = M_SQRT2*(_q).qz; \
416  const float a2_1 = _a*_a-1; \
417  const float ab = _a*_b; \
418  const float ac = _a*_c; \
419  const float ad = _a*_d; \
420  const float bc = _b*_c; \
421  const float bd = _b*_d; \
422  const float cd = _c*_d; \
423  RMAT_ELMT(_rm, 0, 0) = a2_1+_b*_b; \
424  RMAT_ELMT(_rm, 0, 1) = bc+ad; \
425  RMAT_ELMT(_rm, 0, 2) = bd-ac; \
426  RMAT_ELMT(_rm, 1, 0) = bc-ad; \
427  RMAT_ELMT(_rm, 1, 1) = a2_1+_c*_c; \
428  RMAT_ELMT(_rm, 1, 2) = cd+ab; \
429  RMAT_ELMT(_rm, 2, 0) = bd+ac; \
430  RMAT_ELMT(_rm, 2, 1) = cd-ab; \
431  RMAT_ELMT(_rm, 2, 2) = a2_1+_d*_d; \
432  }
433 #endif
434 
435 /* in place first order integration of a rotation matrix */
436 #define FLOAT_RMAT_INTEGRATE_FI(_rm, _omega, _dt ) { \
437  struct FloatRMat exp_omega_dt = { \
438  { 1. , dt*omega.r, -dt*omega.q, \
439  -dt*omega.r, 1. , dt*omega.p, \
440  dt*omega.q, -dt*omega.p, 1. }}; \
441  struct FloatRMat R_tdt; \
442  FLOAT_RMAT_COMP(R_tdt, _rm, exp_omega_dt); \
443  memcpy(&(_rm), &R_tdt, sizeof(R_tdt)); \
444  }
445 
446 
447 static inline float renorm_factor(float n) {
448  if (n < 1.5625f && n > 0.64f)
449  return .5 * (3-n);
450  else if (n < 100.0f && n > 0.01f)
451  return 1. / sqrtf(n);
452  else
453  return 0.;
454 }
455 
456 static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
457 
458  const struct FloatVect3 r0 = {RMAT_ELMT(*rm, 0,0),
459  RMAT_ELMT(*rm, 0,1),
460  RMAT_ELMT(*rm, 0,2)};
461  const struct FloatVect3 r1 = {RMAT_ELMT(*rm, 1,0),
462  RMAT_ELMT(*rm, 1,1),
463  RMAT_ELMT(*rm, 1,2)};
464  float _err = -0.5*FLOAT_VECT3_DOT_PRODUCT(r0, r1);
465  struct FloatVect3 r0_t;
466  VECT3_SUM_SCALED(r0_t, r0, r1, _err);
467  struct FloatVect3 r1_t;
468  VECT3_SUM_SCALED(r1_t, r1, r0, _err);
469  struct FloatVect3 r2_t;
470  FLOAT_VECT3_CROSS_PRODUCT(r2_t, r0_t, r1_t);
471  float s = renorm_factor(FLOAT_VECT3_NORM2(r0_t));
472  MAT33_ROW_VECT3_SMUL(*rm, 0, r0_t, s);
473  s = renorm_factor(FLOAT_VECT3_NORM2(r1_t));
474  MAT33_ROW_VECT3_SMUL(*rm, 1, r1_t, s);
475  s = renorm_factor(FLOAT_VECT3_NORM2(r2_t));
476  MAT33_ROW_VECT3_SMUL(*rm, 2, r2_t, s);
477 
478  return _err;
479 
480 }
481 
482 
483 /*
484  * Quaternions
485  */
486 
487 #define FLOAT_QUAT_ZERO(_q) QUAT_ASSIGN(_q, 1., 0., 0., 0.)
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 #define FLOAT_QUAT_EXPLEMENTARY(b,a) QUAT_EXPLEMENTARY(b,a)
496 
497 #define FLOAT_QUAT_COPY(_qo, _qi) QUAT_COPY(_qo, _qi)
498 
499 #define FLOAT_QUAT_NORM(_q) (sqrtf(SQUARE((_q).qi) + SQUARE((_q).qx)+ \
500  SQUARE((_q).qy) + SQUARE((_q).qz)))
501 
502 #define FLOAT_QUAT_NORMALIZE(_q) { \
503  float norm = FLOAT_QUAT_NORM(_q); \
504  if (norm > FLT_MIN) { \
505  (_q).qi = (_q).qi / norm; \
506  (_q).qx = (_q).qx / norm; \
507  (_q).qy = (_q).qy / norm; \
508  (_q).qz = (_q).qz / norm; \
509  } \
510  }
511 
512 #define FLOAT_QUAT_INVERT(_qo, _qi) QUAT_INVERT(_qo, _qi)
513 
514 #define FLOAT_QUAT_WRAP_SHORTEST(_q) { \
515  if ((_q).qi < 0.) \
516  QUAT_EXPLEMENTARY(_q,_q); \
517  }
518 
519 /* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
520 #define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \
521  FLOAT_QUAT_COMP(_a2c, _a2b, _b2c); \
522  FLOAT_QUAT_WRAP_SHORTEST(_a2c); \
523  FLOAT_QUAT_NORMALIZE(_a2c); \
524  }
525 
526 /* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
527 #define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c) { \
528  (_a2c).qi = (_a2b).qi*(_b2c).qi - (_a2b).qx*(_b2c).qx - (_a2b).qy*(_b2c).qy - (_a2b).qz*(_b2c).qz; \
529  (_a2c).qx = (_a2b).qi*(_b2c).qx + (_a2b).qx*(_b2c).qi + (_a2b).qy*(_b2c).qz - (_a2b).qz*(_b2c).qy; \
530  (_a2c).qy = (_a2b).qi*(_b2c).qy - (_a2b).qx*(_b2c).qz + (_a2b).qy*(_b2c).qi + (_a2b).qz*(_b2c).qx; \
531  (_a2c).qz = (_a2b).qi*(_b2c).qz + (_a2b).qx*(_b2c).qy - (_a2b).qy*(_b2c).qx + (_a2b).qz*(_b2c).qi; \
532  }
533 
534 #define FLOAT_QUAT_MULT(_a2c, _a2b, _b2c) FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
535 
536 /* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
537 #define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) { \
538  FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c); \
539  FLOAT_QUAT_WRAP_SHORTEST(_a2b); \
540  FLOAT_QUAT_NORMALIZE(_a2b); \
541  }
542 
543 /* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
544 #define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c) { \
545  (_a2b).qi = (_a2c).qi*(_b2c).qi + (_a2c).qx*(_b2c).qx + (_a2c).qy*(_b2c).qy + (_a2c).qz*(_b2c).qz; \
546  (_a2b).qx = -(_a2c).qi*(_b2c).qx + (_a2c).qx*(_b2c).qi - (_a2c).qy*(_b2c).qz + (_a2c).qz*(_b2c).qy; \
547  (_a2b).qy = -(_a2c).qi*(_b2c).qy + (_a2c).qx*(_b2c).qz + (_a2c).qy*(_b2c).qi - (_a2c).qz*(_b2c).qx; \
548  (_a2b).qz = -(_a2c).qi*(_b2c).qz - (_a2c).qx*(_b2c).qy + (_a2c).qy*(_b2c).qx + (_a2c).qz*(_b2c).qi; \
549  }
550 
551 /* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
552 #define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) { \
553  FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c); \
554  FLOAT_QUAT_WRAP_SHORTEST(_b2c); \
555  FLOAT_QUAT_NORMALIZE(_b2c); \
556  }
557 
558 /* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
559 #define FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c) { \
560  (_b2c).qi = (_a2b).qi*(_a2c).qi + (_a2b).qx*(_a2c).qx + (_a2b).qy*(_a2c).qy + (_a2b).qz*(_a2c).qz; \
561  (_b2c).qx = (_a2b).qi*(_a2c).qx - (_a2b).qx*(_a2c).qi - (_a2b).qy*(_a2c).qz + (_a2b).qz*(_a2c).qy; \
562  (_b2c).qy = (_a2b).qi*(_a2c).qy + (_a2b).qx*(_a2c).qz - (_a2b).qy*(_a2c).qi - (_a2b).qz*(_a2c).qx; \
563  (_b2c).qz = (_a2b).qi*(_a2c).qz - (_a2b).qx*(_a2c).qy + (_a2b).qy*(_a2c).qx - (_a2b).qz*(_a2c).qi; \
564  }
565 
566 #define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt) { \
567  const float v_norm = sqrt((w).p*(w).p + (w).q*(w).q + (w).r*(w).r); \
568  const float c2 = cos(dt*v_norm/2.0); \
569  const float s2 = sin(dt*v_norm/2.0); \
570  if (v_norm < 1e-8) { \
571  (q_out).qi = 1; \
572  (q_out).qx = 0; \
573  (q_out).qy = 0; \
574  (q_out).qz = 0; \
575  } else { \
576  (q_out).qi = c2; \
577  (q_out).qx = (w).p/v_norm * s2; \
578  (q_out).qy = (w).q/v_norm * s2; \
579  (q_out).qz = (w).r/v_norm * s2; \
580  } \
581  }
582 
583 /* in place quaternion integration with constante rotational velocity */
584 #define FLOAT_QUAT_INTEGRATE(_q, _omega, _dt) { \
585  const float no = FLOAT_RATES_NORM(_omega); \
586  if (no > FLT_MIN) { \
587  const float a = 0.5*no*_dt; \
588  const float ca = cosf(a); \
589  const float sa_ov_no = sinf(a)/no; \
590  const float dp = sa_ov_no*_omega.p; \
591  const float dq = sa_ov_no*_omega.q; \
592  const float dr = sa_ov_no*_omega.r; \
593  const float qi = _q.qi; \
594  const float qx = _q.qx; \
595  const float qy = _q.qy; \
596  const float qz = _q.qz; \
597  _q.qi = ca*qi - dp*qx - dq*qy - dr*qz; \
598  _q.qx = dp*qi + ca*qx + dr*qy - dq*qz; \
599  _q.qy = dq*qi - dr*qx + ca*qy + dp*qz; \
600  _q.qz = dr*qi + dq*qx - dp*qy + ca*qz; \
601  } \
602  }
603 
604 #ifdef ALGEBRA_FLOAT_USE_SLOW_FUNCTIONS
605 #define FLOAT_QUAT_VMULT(v_out, q, v_in) { \
606  const float qi2 = q.qi*q.qi; \
607  const float qiqx = q.qi*q.qx; \
608  const float qiqy = q.qi*q.qy; \
609  const float qiqz = q.qi*q.qz; \
610  const float qx2 = q.qx*q.qx; \
611  const float qxqy = q.qx*q.qy; \
612  const float qxqz = q.qx*q.qz; \
613  const float qy2 = q.qy*q.qy; \
614  const float qyqz = q.qy*q.qz; \
615  const float qz2 = q.qz*q.qz; \
616  const float m00 = qi2 + qx2 - qy2 - qz2; \
617  const float m01 = 2 * ( qxqy + qiqz ); \
618  const float m02 = 2 * ( qxqz - qiqy ); \
619  const float m10 = 2 * ( qxqy - qiqz ); \
620  const float m11 = qi2 - qx2 + qy2 - qz2; \
621  const float m12 = 2 * ( qyqz + qiqx ); \
622  const float m20 = 2 * ( qxqz + qiqy ); \
623  const float m21 = 2 * ( qyqz - qiqx ); \
624  const float m22 = qi2 - qx2 - qy2 + qz2; \
625  v_out.x = m00 * v_in.x + m01 * v_in.y + m02 * v_in.z; \
626  v_out.y = m10 * v_in.x + m11 * v_in.y + m12 * v_in.z; \
627  v_out.z = m20 * v_in.x + m21 * v_in.y + m22 * v_in.z; \
628  }
629 #else
630 #define FLOAT_QUAT_VMULT(v_out, q, v_in) { \
631  const float qi2_M1_2 = q.qi*q.qi - 0.5; \
632  const float qiqx = q.qi*q.qx; \
633  const float qiqy = q.qi*q.qy; \
634  const float qiqz = q.qi*q.qz; \
635  float m01 = q.qx*q.qy; /* aka qxqy */ \
636  float m02 = q.qx*q.qz; /* aka qxqz */ \
637  float m12 = q.qy*q.qz; /* aka qyqz */ \
638 \
639  const float m00 = qi2_M1_2 + q.qx*q.qx; \
640  const float m10 = m01 - qiqz; \
641  const float m20 = m02 + qiqy; \
642  const float m21 = m12 - qiqx; \
643  m01 += qiqz; \
644  m02 -= qiqy; \
645  m12 += qiqx; \
646  const float m11 = qi2_M1_2 + q.qy*q.qy; \
647  const float m22 = qi2_M1_2 + q.qz*q.qz; \
648  v_out.x = 2*(m00 * v_in.x + m01 * v_in.y + m02 * v_in.z); \
649  v_out.y = 2*(m10 * v_in.x + m11 * v_in.y + m12 * v_in.z); \
650  v_out.z = 2*(m20 * v_in.x + m21 * v_in.y + m22 * v_in.z); \
651  }
652 #endif
653 
654 /* _qd = -0.5*omega(_r) * _q */
655 #define FLOAT_QUAT_DERIVATIVE(_qd, _r, _q) { \
656  (_qd).qi = -0.5*( (_r).p*(_q).qx + (_r).q*(_q).qy + (_r).r*(_q).qz); \
657  (_qd).qx = -0.5*(-(_r).p*(_q).qi - (_r).r*(_q).qy + (_r).q*(_q).qz); \
658  (_qd).qy = -0.5*(-(_r).q*(_q).qi + (_r).r*(_q).qx - (_r).p*(_q).qz); \
659  (_qd).qz = -0.5*(-(_r).r*(_q).qi - (_r).q*(_q).qx + (_r).p*(_q).qy); \
660  }
661 
662 /* _qd = -0.5*omega(_r) * _q */
663 #define FLOAT_QUAT_DERIVATIVE_LAGRANGE(_qd, _r, _q) { \
664  const float K_LAGRANGE = 1.; \
665  const float c = K_LAGRANGE * ( 1 - FLOAT_QUAT_NORM(_q)) / -0.5; \
666  (_qd).qi = -0.5*( c*(_q).qi + (_r).p*(_q).qx + (_r).q*(_q).qy + (_r).r*(_q).qz); \
667  (_qd).qx = -0.5*(-(_r).p*(_q).qi + c*(_q).qx - (_r).r*(_q).qy + (_r).q*(_q).qz); \
668  (_qd).qy = -0.5*(-(_r).q*(_q).qi + (_r).r*(_q).qx + c*(_q).qy - (_r).p*(_q).qz); \
669  (_qd).qz = -0.5*(-(_r).r*(_q).qi - (_r).q*(_q).qx + (_r).p*(_q).qy + c*(_q).qz); \
670  }
671 
672 #define FLOAT_QUAT_OF_EULERS(_q, _e) { \
673  \
674  const float phi2 = (_e).phi/2.0; \
675  const float theta2 = (_e).theta/2.0; \
676  const float psi2 = (_e).psi/2.0; \
677  \
678  const float s_phi2 = sinf( phi2 ); \
679  const float c_phi2 = cosf( phi2 ); \
680  const float s_theta2 = sinf( theta2 ); \
681  const float c_theta2 = cosf( theta2 ); \
682  const float s_psi2 = sinf( psi2 ); \
683  const float c_psi2 = cosf( psi2 ); \
684  \
685  (_q).qi = c_phi2 * c_theta2 * c_psi2 + s_phi2 * s_theta2 * s_psi2; \
686  (_q).qx = -c_phi2 * s_theta2 * s_psi2 + s_phi2 * c_theta2 * c_psi2; \
687  (_q).qy = c_phi2 * s_theta2 * c_psi2 + s_phi2 * c_theta2 * s_psi2; \
688  (_q).qz = c_phi2 * c_theta2 * s_psi2 - s_phi2 * s_theta2 * c_psi2; \
689  \
690  }
691 
692 #define FLOAT_QUAT_OF_AXIS_ANGLE(_q, _uv, _an) { \
693  const float san = sinf(_an/2.); \
694  _q.qi = cosf(_an/2.); \
695  _q.qx = san * _uv.x; \
696  _q.qy = san * _uv.y; \
697  _q.qz = san * _uv.z; \
698  }
699 
700 #define FLOAT_QUAT_OF_RMAT(_q, _r) { \
701  const float tr = RMAT_TRACE(_r); \
702  if (tr > 0) { \
703  const float two_qi = sqrtf(1.+tr); \
704  const float four_qi = 2. * two_qi; \
705  _q.qi = 0.5 * two_qi; \
706  _q.qx = (RMAT_ELMT(_r, 1, 2)-RMAT_ELMT(_r, 2, 1))/four_qi; \
707  _q.qy = (RMAT_ELMT(_r, 2, 0)-RMAT_ELMT(_r, 0, 2))/four_qi; \
708  _q.qz = (RMAT_ELMT(_r, 0, 1)-RMAT_ELMT(_r, 1, 0))/four_qi; \
709  /*printf("tr > 0\n");*/ \
710  } \
711  else { \
712  if (RMAT_ELMT(_r, 0, 0) > RMAT_ELMT(_r, 1, 1) && \
713  RMAT_ELMT(_r, 0, 0) > RMAT_ELMT(_r, 2, 2)) { \
714  const float two_qx = sqrtf(RMAT_ELMT(_r, 0, 0) -RMAT_ELMT(_r, 1, 1) \
715  -RMAT_ELMT(_r, 2, 2) + 1); \
716  const float four_qx = 2. * two_qx; \
717  _q.qi = (RMAT_ELMT(_r, 1, 2)-RMAT_ELMT(_r, 2, 1))/four_qx; \
718  _q.qx = 0.5 * two_qx; \
719  _q.qy = (RMAT_ELMT(_r, 0, 1)+RMAT_ELMT(_r, 1, 0))/four_qx; \
720  _q.qz = (RMAT_ELMT(_r, 2, 0)+RMAT_ELMT(_r, 0, 2))/four_qx; \
721  /*printf("m00 largest\n");*/ \
722  } \
723  else if (RMAT_ELMT(_r, 1, 1) > RMAT_ELMT(_r, 2, 2)) { \
724  const float two_qy = \
725  sqrtf(RMAT_ELMT(_r, 1, 1) - RMAT_ELMT(_r, 0, 0) - RMAT_ELMT(_r, 2, 2) + 1); \
726  const float four_qy = 2. * two_qy; \
727  _q.qi = (RMAT_ELMT(_r, 2, 0) - RMAT_ELMT(_r, 0, 2))/four_qy; \
728  _q.qx = (RMAT_ELMT(_r, 0, 1) + RMAT_ELMT(_r, 1, 0))/four_qy; \
729  _q.qy = 0.5 * two_qy; \
730  _q.qz = (RMAT_ELMT(_r, 1, 2) + RMAT_ELMT(_r, 2, 1))/four_qy; \
731  /*printf("m11 largest\n");*/ \
732  } \
733  else { \
734  const float two_qz = \
735  sqrtf(RMAT_ELMT(_r, 2, 2) - RMAT_ELMT(_r, 0, 0) - RMAT_ELMT(_r, 1, 1) + 1); \
736  const float four_qz = 2. * two_qz; \
737  _q.qi = (RMAT_ELMT(_r, 0, 1)- RMAT_ELMT(_r, 1, 0))/four_qz; \
738  _q.qx = (RMAT_ELMT(_r, 2, 0)+ RMAT_ELMT(_r, 0, 2))/four_qz; \
739  _q.qy = (RMAT_ELMT(_r, 1, 2)+ RMAT_ELMT(_r, 2, 1))/four_qz; \
740  _q.qz = 0.5 * two_qz; \
741  /*printf("m22 largest\n");*/ \
742  } \
743  } \
744  }
745 
746 
747 /*
748  * Euler angles
749  */
750 
751 #define FLOAT_EULERS_ZERO(_e) EULERS_ASSIGN(_e, 0., 0., 0.);
752 
753 #define FLOAT_EULERS_NORM(_e) (sqrtf(SQUARE((_e).phi)+SQUARE((_e).theta)+SQUARE((_e).psi))) ;
754 
755 #define FLOAT_EULERS_OF_RMAT(_e, _rm) { \
756  \
757  const float dcm00 = (_rm).m[0]; \
758  const float dcm01 = (_rm).m[1]; \
759  const float dcm02 = (_rm).m[2]; \
760  const float dcm12 = (_rm).m[5]; \
761  const float dcm22 = (_rm).m[8]; \
762  (_e).phi = atan2f( dcm12, dcm22 ); \
763  (_e).theta = -asinf( dcm02 ); \
764  (_e).psi = atan2f( dcm01, dcm00 ); \
765  \
766  }
767 
768 #define FLOAT_EULERS_OF_QUAT(_e, _q) { \
769  \
770  const float qx2 = (_q).qx*(_q).qx; \
771  const float qy2 = (_q).qy*(_q).qy; \
772  const float qz2 = (_q).qz*(_q).qz; \
773  const float qiqx = (_q).qi*(_q).qx; \
774  const float qiqy = (_q).qi*(_q).qy; \
775  const float qiqz = (_q).qi*(_q).qz; \
776  const float qxqy = (_q).qx*(_q).qy; \
777  const float qxqz = (_q).qx*(_q).qz; \
778  const float qyqz = (_q).qy*(_q).qz; \
779  const float dcm00 = 1.0 - 2.*( qy2 + qz2 ); \
780  const float dcm01 = 2.*( qxqy + qiqz ); \
781  const float dcm02 = 2.*( qxqz - qiqy ); \
782  const float dcm12 = 2.*( qyqz + qiqx ); \
783  const float dcm22 = 1.0 - 2.*( qx2 + qy2 ); \
784  \
785  (_e).phi = atan2f( dcm12, dcm22 ); \
786  (_e).theta = -asinf( dcm02 ); \
787  (_e).psi = atan2f( dcm01, dcm00 ); \
788  \
789  }
790 
791 
792 
793 
794 
795 #endif /* PPRZ_ALGEBRA_FLOAT_H */
rotation matrix
#define FLOAT_VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
#define VECT3_SUM_SCALED(_c, _a, _b, _s)
Definition: pprz_algebra.h:148
float m[3 *3]
angular rates
float psi
in radians
float m[3 *3]
#define FLOAT_VECT3_DOT_PRODUCT(_v1, _v2)
float theta
in radians
euler angles
float p
in rad/s^2
Roation quaternion.
#define MAT33_ROW_VECT3_SMUL(_mat, _row, _vin, _s)
Definition: pprz_algebra.h:452
float phi
in radians
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:520
float r
in rad/s^2
static float renorm_factor(float n)
static float float_rmat_reorthogonalize(struct FloatRMat *rm)
float q
in rad/s^2
#define FLOAT_VECT3_NORM2(_v)