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