Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
pprz_algebra_float.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2008-2014 The Paparazzi Team
3 *
4 * This file is part of paparazzi.
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, see
18 * <http://www.gnu.org/licenses/>.
19 */
20
27#include "pprz_algebra_float.h"
28
30void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv, float dt)
31{
32 vec->x += dv->x * dt;
33 vec->y += dv->y * dt;
34 vec->z += dv->z * dt;
35}
36
38void float_rates_integrate_fi(struct FloatRates *r, struct FloatRates *dr, float dt)
39{
40 r->p += dr->p * dt;
41 r->q += dr->q * dt;
42 r->r += dr->r * dt;
43}
44
46{
47 r->p = edot->phi - sinf(e->theta) * edot->psi;
48 r->q = cosf(e->phi) * edot->theta + sinf(e->phi) * cosf(e->theta) * edot->psi;
49 r->r = -sinf(e->phi) * edot->theta + cosf(e->phi) * cosf(e->theta) * edot->psi;
50}
51
52
53
54
56{
57 RMAT_ELMT(*m_b2a, 0, 0) = RMAT_ELMT(*m_a2b, 0, 0);
58 RMAT_ELMT(*m_b2a, 0, 1) = RMAT_ELMT(*m_a2b, 1, 0);
59 RMAT_ELMT(*m_b2a, 0, 2) = RMAT_ELMT(*m_a2b, 2, 0);
60 RMAT_ELMT(*m_b2a, 1, 0) = RMAT_ELMT(*m_a2b, 0, 1);
61 RMAT_ELMT(*m_b2a, 1, 1) = RMAT_ELMT(*m_a2b, 1, 1);
62 RMAT_ELMT(*m_b2a, 1, 2) = RMAT_ELMT(*m_a2b, 2, 1);
63 RMAT_ELMT(*m_b2a, 2, 0) = RMAT_ELMT(*m_a2b, 0, 2);
64 RMAT_ELMT(*m_b2a, 2, 1) = RMAT_ELMT(*m_a2b, 1, 2);
65 RMAT_ELMT(*m_b2a, 2, 2) = RMAT_ELMT(*m_a2b, 2, 2);
66}
67
69{
70 return sqrtf(SQUARE(rm->m[0]) + SQUARE(rm->m[1]) + SQUARE(rm->m[2]) +
71 SQUARE(rm->m[3]) + SQUARE(rm->m[4]) + SQUARE(rm->m[5]) +
72 SQUARE(rm->m[6]) + SQUARE(rm->m[7]) + SQUARE(rm->m[8]));
73}
74
79{
80 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];
81 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];
82 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];
83 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];
84 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];
85 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];
86 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];
87 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];
88 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];
89}
90
95{
96 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];
97 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];
98 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];
99 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];
100 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];
101 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];
102 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];
103 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];
104 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];
105}
106
110void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
111{
112 vb->x = m_a2b->m[0] * va->x + m_a2b->m[1] * va->y + m_a2b->m[2] * va->z;
113 vb->y = m_a2b->m[3] * va->x + m_a2b->m[4] * va->y + m_a2b->m[5] * va->z;
114 vb->z = m_a2b->m[6] * va->x + m_a2b->m[7] * va->y + m_a2b->m[8] * va->z;
115}
116
121{
122 vb->x = m_b2a->m[0] * va->x + m_b2a->m[3] * va->y + m_b2a->m[6] * va->z;
123 vb->y = m_b2a->m[1] * va->x + m_b2a->m[4] * va->y + m_b2a->m[7] * va->z;
124 vb->z = m_b2a->m[2] * va->x + m_b2a->m[5] * va->y + m_b2a->m[8] * va->z;
125}
126
131{
132 rb->phi = m_a2b->m[0] * ra->phi + m_a2b->m[1] * ra->theta + m_a2b->m[2] * ra->psi;
133 rb->theta = m_a2b->m[3] * ra->phi + m_a2b->m[4] * ra->theta + m_a2b->m[5] * ra->psi;
134 rb->psi = m_a2b->m[6] * ra->phi + m_a2b->m[7] * ra->theta + m_a2b->m[8] * ra->psi;
135}
136
141{
142 rb->phi = m_b2a->m[0] * ra->phi + m_b2a->m[3] * ra->theta + m_b2a->m[6] * ra->psi;
143 rb->theta = m_b2a->m[1] * ra->phi + m_b2a->m[4] * ra->theta + m_b2a->m[7] * ra->psi;
144 rb->psi = m_b2a->m[2] * ra->phi + m_b2a->m[5] * ra->theta + m_b2a->m[8] * ra->psi;
145}
146
151{
152 rb->p = m_a2b->m[0] * ra->p + m_a2b->m[1] * ra->q + m_a2b->m[2] * ra->r;
153 rb->q = m_a2b->m[3] * ra->p + m_a2b->m[4] * ra->q + m_a2b->m[5] * ra->r;
154 rb->r = m_a2b->m[6] * ra->p + m_a2b->m[7] * ra->q + m_a2b->m[8] * ra->r;
155}
156
161{
162 rb->p = m_b2a->m[0] * ra->p + m_b2a->m[3] * ra->q + m_b2a->m[6] * ra->r;
163 rb->q = m_b2a->m[1] * ra->p + m_b2a->m[4] * ra->q + m_b2a->m[7] * ra->r;
164 rb->r = m_b2a->m[2] * ra->p + m_b2a->m[5] * ra->q + m_b2a->m[8] * ra->r;
165}
166
167
169void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle)
170{
171 const float ux2 = uv->x * uv->x;
172 const float uy2 = uv->y * uv->y;
173 const float uz2 = uv->z * uv->z;
174 const float uxuy = uv->x * uv->y;
175 const float uyuz = uv->y * uv->z;
176 const float uxuz = uv->x * uv->z;
177 const float can = cosf(angle);
178 const float san = sinf(angle);
179 const float one_m_can = (1. - can);
180
181 RMAT_ELMT(*rm, 0, 0) = ux2 + (1. - ux2) * can;
182 RMAT_ELMT(*rm, 0, 1) = uxuy * one_m_can + uv->z * san;
183 RMAT_ELMT(*rm, 0, 2) = uxuz * one_m_can - uv->y * san;
184 RMAT_ELMT(*rm, 1, 0) = RMAT_ELMT(*rm, 0, 1);
185 RMAT_ELMT(*rm, 1, 1) = uy2 + (1. - uy2) * can;
186 RMAT_ELMT(*rm, 1, 2) = uyuz * one_m_can + uv->x * san;
187 RMAT_ELMT(*rm, 2, 0) = RMAT_ELMT(*rm, 0, 2);
188 RMAT_ELMT(*rm, 2, 1) = RMAT_ELMT(*rm, 1, 2);
189 RMAT_ELMT(*rm, 2, 2) = uz2 + (1. - uz2) * can;
190}
191
192
193/* C n->b rotation matrix */
195{
196 const float sphi = sinf(e->phi);
197 const float cphi = cosf(e->phi);
198 const float stheta = sinf(e->theta);
199 const float ctheta = cosf(e->theta);
200 const float spsi = sinf(e->psi);
201 const float cpsi = cosf(e->psi);
202
203 RMAT_ELMT(*rm, 0, 0) = ctheta * cpsi;
204 RMAT_ELMT(*rm, 0, 1) = ctheta * spsi;
205 RMAT_ELMT(*rm, 0, 2) = -stheta;
206 RMAT_ELMT(*rm, 1, 0) = sphi * stheta * cpsi - cphi * spsi;
207 RMAT_ELMT(*rm, 1, 1) = sphi * stheta * spsi + cphi * cpsi;
208 RMAT_ELMT(*rm, 1, 2) = sphi * ctheta;
209 RMAT_ELMT(*rm, 2, 0) = cphi * stheta * cpsi + sphi * spsi;
210 RMAT_ELMT(*rm, 2, 1) = cphi * stheta * spsi - sphi * cpsi;
211 RMAT_ELMT(*rm, 2, 2) = cphi * ctheta;
212}
213
215{
216 const float sphi = sinf(e->phi);
217 const float cphi = cosf(e->phi);
218 const float stheta = sinf(e->theta);
219 const float ctheta = cosf(e->theta);
220 const float spsi = sinf(e->psi);
221 const float cpsi = cosf(e->psi);
222
223 RMAT_ELMT(*rm, 0, 0) = ctheta * cpsi - sphi * stheta * spsi;
224 RMAT_ELMT(*rm, 0, 1) = ctheta * spsi + sphi * stheta * cpsi;
225 RMAT_ELMT(*rm, 0, 2) = -cphi * stheta;
226 RMAT_ELMT(*rm, 1, 0) = -cphi * spsi;
227 RMAT_ELMT(*rm, 1, 1) = cphi * cpsi;
228 RMAT_ELMT(*rm, 1, 2) = sphi;
229 RMAT_ELMT(*rm, 2, 0) = stheta * cpsi + sphi * ctheta * spsi;
230 RMAT_ELMT(*rm, 2, 1) = stheta * spsi - sphi * ctheta * cpsi;
231 RMAT_ELMT(*rm, 2, 2) = cphi * ctheta;
232}
233
234
235/* C n->b rotation matrix */
236void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
237{
238 const float _a = M_SQRT2 * q->qi;
239 const float _b = M_SQRT2 * q->qx;
240 const float _c = M_SQRT2 * q->qy;
241 const float _d = M_SQRT2 * q->qz;
242 const float a2_1 = _a * _a - 1;
243 const float ab = _a * _b;
244 const float ac = _a * _c;
245 const float ad = _a * _d;
246 const float bc = _b * _c;
247 const float bd = _b * _d;
248 const float cd = _c * _d;
249 RMAT_ELMT(*rm, 0, 0) = a2_1 + _b * _b;
250 RMAT_ELMT(*rm, 0, 1) = bc + ad;
251 RMAT_ELMT(*rm, 0, 2) = bd - ac;
252 RMAT_ELMT(*rm, 1, 0) = bc - ad;
253 RMAT_ELMT(*rm, 1, 1) = a2_1 + _c * _c;
254 RMAT_ELMT(*rm, 1, 2) = cd + ab;
255 RMAT_ELMT(*rm, 2, 0) = bd + ac;
256 RMAT_ELMT(*rm, 2, 1) = cd - ab;
257 RMAT_ELMT(*rm, 2, 2) = a2_1 + _d * _d;
258}
259
261void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt)
262{
263 struct FloatRMat exp_omega_dt = {
264 {
265 1. , dt *omega->r, -dt *omega->q,
266 -dt *omega->r, 1. , dt *omega->p,
267 dt *omega->q, -dt *omega->p, 1.
268 }
269 };
270 struct FloatRMat R_tdt;
272 memcpy(rm, &R_tdt, sizeof(R_tdt));
273}
274
275static inline float renorm_factor(float n)
276{
277 if (n < 1.5625f && n > 0.64f) {
278 return .5 * (3 - n);
279 } else if (n < 100.0f && n > 0.01f) {
280 return 1. / sqrtf(n);
281 } else {
282 return 0.;
283 }
284}
285
287{
288 const struct FloatVect3 r0 = {RMAT_ELMT(*rm, 0, 0),
289 RMAT_ELMT(*rm, 0, 1),
290 RMAT_ELMT(*rm, 0, 2)
291 };
292 const struct FloatVect3 r1 = {RMAT_ELMT(*rm, 1, 0),
293 RMAT_ELMT(*rm, 1, 1),
294 RMAT_ELMT(*rm, 1, 2)
295 };
296 float _err = -0.5 * VECT3_DOT_PRODUCT(r0, r1);
297 struct FloatVect3 r0_t;
299 struct FloatVect3 r1_t;
301 struct FloatVect3 r2_t;
309
310 return _err;
311}
312
313
314/*
315 *
316 * Quaternion functions.
317 *
318 */
319
320void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
321{
322 a2c->qi = a2b->qi * b2c->qi - a2b->qx * b2c->qx - a2b->qy * b2c->qy - a2b->qz * b2c->qz;
323 a2c->qx = a2b->qi * b2c->qx + a2b->qx * b2c->qi + a2b->qy * b2c->qz - a2b->qz * b2c->qy;
324 a2c->qy = a2b->qi * b2c->qy - a2b->qx * b2c->qz + a2b->qy * b2c->qi + a2b->qz * b2c->qx;
325 a2c->qz = a2b->qi * b2c->qz + a2b->qx * b2c->qy - a2b->qy * b2c->qx + a2b->qz * b2c->qi;
326}
327
329{
330 a2b->qi = a2c->qi * b2c->qi + a2c->qx * b2c->qx + a2c->qy * b2c->qy + a2c->qz * b2c->qz;
331 a2b->qx = -a2c->qi * b2c->qx + a2c->qx * b2c->qi - a2c->qy * b2c->qz + a2c->qz * b2c->qy;
332 a2b->qy = -a2c->qi * b2c->qy + a2c->qx * b2c->qz + a2c->qy * b2c->qi - a2c->qz * b2c->qx;
333 a2b->qz = -a2c->qi * b2c->qz - a2c->qx * b2c->qy + a2c->qy * b2c->qx + a2c->qz * b2c->qi;
334}
335
337{
338 b2c->qi = a2b->qi * a2c->qi + a2b->qx * a2c->qx + a2b->qy * a2c->qy + a2b->qz * a2c->qz;
339 b2c->qx = a2b->qi * a2c->qx - a2b->qx * a2c->qi - a2b->qy * a2c->qz + a2b->qz * a2c->qy;
340 b2c->qy = a2b->qi * a2c->qy + a2b->qx * a2c->qz - a2b->qy * a2c->qi - a2b->qz * a2c->qx;
341 b2c->qz = a2b->qi * a2c->qz - a2b->qx * a2c->qy + a2b->qy * a2c->qx - a2b->qz * a2c->qi;
342}
343
350
357
364
365void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt)
366{
367 const float v_norm = sqrtf(w->p * w->p + w->q * w->q + w->r * w->r);
368 const float c2 = cos(dt * v_norm / 2.0);
369 const float s2 = sin(dt * v_norm / 2.0);
370 if (v_norm < 1e-8) {
371 q_out->qi = 1;
372 q_out->qx = 0;
373 q_out->qy = 0;
374 q_out->qz = 0;
375 } else {
376 q_out->qi = c2;
377 q_out->qx = w->p / v_norm * s2;
378 q_out->qy = w->q / v_norm * s2;
379 q_out->qz = w->r / v_norm * s2;
380 }
381}
382
384void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, float dt)
385{
386 const float qi = q->qi;
387 const float qx = q->qx;
388 const float qy = q->qy;
389 const float qz = q->qz;
390 const float dp = 0.5 * dt * omega->p;
391 const float dq = 0.5 * dt * omega->q;
392 const float dr = 0.5 * dt * omega->r;
393 q->qi = qi - dp * qx - dq * qy - dr * qz;
394 q->qx = dp * qi + qx + dr * qy - dq * qz;
395 q->qy = dq * qi - dr * qx + qy + dp * qz;
396 q->qz = dr * qi + dq * qx - dp * qy + qz;
397}
398
400void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt)
401{
402 const float no = FLOAT_RATES_NORM(*omega);
403 if (no > FLT_MIN) {
404 const float a = 0.5 * no * dt;
405 const float ca = cosf(a);
406 const float sa_ov_no = sinf(a) / no;
407 const float dp = sa_ov_no * omega->p;
408 const float dq = sa_ov_no * omega->q;
409 const float dr = sa_ov_no * omega->r;
410 const float qi = q->qi;
411 const float qx = q->qx;
412 const float qy = q->qy;
413 const float qz = q->qz;
414 q->qi = ca * qi - dp * qx - dq * qy - dr * qz;
415 q->qx = dp * qi + ca * qx + dr * qy - dq * qz;
416 q->qy = dq * qi - dr * qx + ca * qy + dp * qz;
417 q->qz = dr * qi + dq * qx - dp * qy + ca * qz;
418 }
419}
420
421void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
422{
423 const float qi2_M1_2 = q->qi * q->qi - 0.5;
424 const float qiqx = q->qi * q->qx;
425 const float qiqy = q->qi * q->qy;
426 const float qiqz = q->qi * q->qz;
427 float m01 = q->qx * q->qy; /* aka qxqy */
428 float m02 = q->qx * q->qz; /* aka qxqz */
429 float m12 = q->qy * q->qz; /* aka qyqz */
430
431 const float m00 = qi2_M1_2 + q->qx * q->qx;
432 const float m10 = m01 - qiqz;
433 const float m20 = m02 + qiqy;
434 const float m21 = m12 - qiqx;
435 m01 += qiqz;
436 m02 -= qiqy;
437 m12 += qiqx;
438 const float m11 = qi2_M1_2 + q->qy * q->qy;
439 const float m22 = qi2_M1_2 + q->qz * q->qz;
440 v_out->x = 2 * (m00 * v_in->x + m01 * v_in->y + m02 * v_in->z);
441 v_out->y = 2 * (m10 * v_in->x + m11 * v_in->y + m12 * v_in->z);
442 v_out->z = 2 * (m20 * v_in->x + m21 * v_in->y + m22 * v_in->z);
443}
444
450void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
451{
452 qd->qi = -0.5 * (r->p * q->qx + r->q * q->qy + r->r * q->qz);
453 qd->qx = -0.5 * (-r->p * q->qi - r->r * q->qy + r->q * q->qz);
454 qd->qy = -0.5 * (-r->q * q->qi + r->r * q->qx - r->p * q->qz);
455 qd->qz = -0.5 * (-r->r * q->qi - r->q * q->qx + r->p * q->qy);
456}
457
462{
463 const float K_LAGRANGE = 1.;
464 const float c = K_LAGRANGE * (1 - float_quat_norm(q)) / -0.5;
465 qd->qi = -0.5 * (c * q->qi + r->p * q->qx + r->q * q->qy + r->r * q->qz);
466 qd->qx = -0.5 * (-r->p * q->qi + c * q->qx - r->r * q->qy + r->q * q->qz);
467 qd->qy = -0.5 * (-r->q * q->qi + r->r * q->qx + c * q->qy - r->p * q->qz);
468 qd->qz = -0.5 * (-r->r * q->qi - r->q * q->qx + r->p * q->qy + c * q->qz);
469}
470
478{
479
480 const float phi2 = e->phi / 2.f;
481 const float theta2 = e->theta / 2.f;
482 const float psi2 = e->psi / 2.f;
483
484 const float s_phi2 = sinf(phi2);
485 const float c_phi2 = cosf(phi2);
486 const float s_theta2 = sinf(theta2);
487 const float c_theta2 = cosf(theta2);
488 const float s_psi2 = sinf(psi2);
489 const float c_psi2 = cosf(psi2);
490
492 q->qx = -c_phi2 * s_theta2 * s_psi2 + s_phi2 * c_theta2 * c_psi2;
495}
496
505{
506 const float phi2 = e->phi / 2.f;
507 const float theta2 = e->theta / 2.f;
508 const float psi2 = e->psi / 2.f;
509
510 const float s_phi2 = sinf(phi2);
511 const float c_phi2 = cosf(phi2);
512 const float s_theta2 = sinf(theta2);
513 const float c_theta2 = cosf(theta2);
514 const float s_psi2 = sinf(psi2);
515 const float c_psi2 = cosf(psi2);
516
521}
522
533{
534 const float phi2 = e->phi / 2.f;
535 const float theta2 = e->theta / 2.f;
536 const float psi2 = e->psi / 2.f;
537
538 const float s_phi2 = sinf(phi2);
539 const float c_phi2 = cosf(phi2);
540 const float s_theta2 = sinf(theta2);
541 const float c_theta2 = cosf(theta2);
542 const float s_psi2 = sinf(psi2);
543 const float c_psi2 = cosf(psi2);
544
549}
550
551void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle)
552{
553 const float san = sinf(angle / 2.f);
554 q->qi = cosf(angle / 2.f);
555 q->qx = san * uv->x;
556 q->qy = san * uv->y;
557 q->qz = san * uv->z;
558}
559
560void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov)
561{
562 const float ov_norm = sqrtf(ov->x * ov->x + ov->y * ov->y + ov->z * ov->z);
563 if (ov_norm < 1e-8) {
564 q->qi = 1;
565 q->qx = 0;
566 q->qy = 0;
567 q->qz = 0;
568 } else {
569 const float s2_normalized = sinf(ov_norm / 2.0) / ov_norm;
570 q->qi = cosf(ov_norm / 2.0);
571 q->qx = ov->x * s2_normalized;
572 q->qy = ov->y * s2_normalized;
573 q->qz = ov->z * s2_normalized;
574 }
575}
576
577void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm)
578{
579 const float tr = RMAT_TRACE(*rm);
580 if (tr > 0) {
581 const float two_qi = sqrtf(1. + tr);
582 const float four_qi = 2. * two_qi;
583 q->qi = 0.5 * two_qi;
584 q->qx = (RMAT_ELMT(*rm, 1, 2) - RMAT_ELMT(*rm, 2, 1)) / four_qi;
585 q->qy = (RMAT_ELMT(*rm, 2, 0) - RMAT_ELMT(*rm, 0, 2)) / four_qi;
586 q->qz = (RMAT_ELMT(*rm, 0, 1) - RMAT_ELMT(*rm, 1, 0)) / four_qi;
587 /*printf("tr > 0\n");*/
588 } else {
589 if (RMAT_ELMT(*rm, 0, 0) > RMAT_ELMT(*rm, 1, 1) &&
590 RMAT_ELMT(*rm, 0, 0) > RMAT_ELMT(*rm, 2, 2)) {
591 const float two_qx = sqrtf(RMAT_ELMT(*rm, 0, 0) - RMAT_ELMT(*rm, 1, 1)
592 - RMAT_ELMT(*rm, 2, 2) + 1);
593 const float four_qx = 2. * two_qx;
594 q->qi = (RMAT_ELMT(*rm, 1, 2) - RMAT_ELMT(*rm, 2, 1)) / four_qx;
595 q->qx = 0.5 * two_qx;
596 q->qy = (RMAT_ELMT(*rm, 0, 1) + RMAT_ELMT(*rm, 1, 0)) / four_qx;
597 q->qz = (RMAT_ELMT(*rm, 2, 0) + RMAT_ELMT(*rm, 0, 2)) / four_qx;
598 /*printf("m00 largest\n");*/
599 } else if (RMAT_ELMT(*rm, 1, 1) > RMAT_ELMT(*rm, 2, 2)) {
600 const float two_qy =
601 sqrtf(RMAT_ELMT(*rm, 1, 1) - RMAT_ELMT(*rm, 0, 0) - RMAT_ELMT(*rm, 2, 2) + 1);
602 const float four_qy = 2. * two_qy;
603 q->qi = (RMAT_ELMT(*rm, 2, 0) - RMAT_ELMT(*rm, 0, 2)) / four_qy;
604 q->qx = (RMAT_ELMT(*rm, 0, 1) + RMAT_ELMT(*rm, 1, 0)) / four_qy;
605 q->qy = 0.5 * two_qy;
606 q->qz = (RMAT_ELMT(*rm, 1, 2) + RMAT_ELMT(*rm, 2, 1)) / four_qy;
607 /*printf("m11 largest\n");*/
608 } else {
609 const float two_qz =
610 sqrtf(RMAT_ELMT(*rm, 2, 2) - RMAT_ELMT(*rm, 0, 0) - RMAT_ELMT(*rm, 1, 1) + 1);
611 const float four_qz = 2. * two_qz;
612 q->qi = (RMAT_ELMT(*rm, 0, 1) - RMAT_ELMT(*rm, 1, 0)) / four_qz;
613 q->qx = (RMAT_ELMT(*rm, 2, 0) + RMAT_ELMT(*rm, 0, 2)) / four_qz;
614 q->qy = (RMAT_ELMT(*rm, 1, 2) + RMAT_ELMT(*rm, 2, 1)) / four_qz;
615 q->qz = 0.5 * two_qz;
616 /*printf("m22 largest\n");*/
617 }
618 }
619}
620
634void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, struct FloatQuat *quat)
635{
636 struct FloatVect3 z = {0., 0., 1.};
637 struct FloatVect3 z_rot;
638
639 // Find the z axis of the target quaternion reference frame
640 struct FloatQuat qinv;
641 float_quat_invert(&qinv, quat);
643
644 // The cross product gives the axis around which to rotate to match the Z vectors.
645 struct FloatVect3 axis;
647
648 tilt->qi = 1 + z_rot.z;
649 tilt->qx = axis.x;
650 tilt->qy = axis.y;
651 tilt->qz = axis.z;
652
654
655 // The tilt quaternion completes the rotation.
657}
658
659
660/*
661 *
662 * Euler angle functions.
663 *
664 */
665
667{
668 const float dcm00 = rm->m[0];
669 const float dcm01 = rm->m[1];
670 float dcm02 = rm->m[2];
671 const float dcm12 = rm->m[5];
672 const float dcm22 = rm->m[8];
673
674 // asinf does not exist outside [-1,1]
675 BoundAbs(dcm02, 1.0);
676
677 e->phi = atan2f(dcm12, dcm22);
678 e->theta = -asinf(dcm02);
679 e->psi = atan2f(dcm01, dcm00);
680}
681
689{
690 const float qx2 = q->qx * q->qx;
691 const float qy2 = q->qy * q->qy;
692 const float qz2 = q->qz * q->qz;
693 const float qiqx = q->qi * q->qx;
694 const float qiqy = q->qi * q->qy;
695 const float qiqz = q->qi * q->qz;
696 const float qxqy = q->qx * q->qy;
697 const float qxqz = q->qx * q->qz;
698 const float qyqz = q->qy * q->qz;
699 const float dcm00 = 1.0 - 2.*(qy2 + qz2);
700 const float dcm01 = 2.*(qxqy + qiqz);
701 float dcm02 = 2.*(qxqz - qiqy);
702 const float dcm12 = 2.*(qyqz + qiqx);
703 const float dcm22 = 1.0 - 2.*(qx2 + qy2);
704
705 // asinf does not exist outside [-1,1]
706 BoundAbs(dcm02, 1.0);
707
708 e->phi = atan2f(dcm12, dcm22);
709 e->theta = -asinf(dcm02);
710 e->psi = atan2f(dcm01, dcm00);
711}
712
723{
724 const float qx2 = q->qx * q->qx;
725 const float qy2 = q->qy * q->qy;
726 const float qz2 = q->qz * q->qz;
727 const float qi2 = q->qi * q->qi;
728 const float qiqx = q->qi * q->qx;
729 const float qiqy = q->qi * q->qy;
730 const float qiqz = q->qi * q->qz;
731 const float qxqy = q->qx * q->qy;
732 const float qxqz = q->qx * q->qz;
733 const float qyqz = q->qy * q->qz;
734 const float r11 = 2.f * (qxqz + qiqy);
735 const float r12 = qi2 - qx2 + qy2 + qz2;
736 float r21 = -2.f * (qyqz - qiqx);
737 const float r31 = 2.f * (qxqy + qiqz);
738 const float r32 = qi2 - qx2 + qy2 - qz2;
739
740 // asinf does not exist outside [-1,1]
741 BoundAbs(r21, 1.0);
742
743 e->theta = atan2f(r11, r12);
744 e->phi = asinf(r21);
745 e->psi = atan2f(r31, r32);
746}
747
756{
757 const float qx2 = q->qx * q->qx;
758 const float qy2 = q->qy * q->qy;
759 const float qz2 = q->qz * q->qz;
760 const float qi2 = q->qi * q->qi;
761 const float qiqx = q->qi * q->qx;
762 const float qiqy = q->qi * q->qy;
763 const float qiqz = q->qi * q->qz;
764 const float qxqy = q->qx * q->qy;
765 const float qxqz = q->qx * q->qz;
766 const float qyqz = q->qy * q->qz;
767 const float r11 = -2 * (qxqy - qiqz);
768 const float r12 = qi2 - qx2 + qy2 - qz2;
769 float r21 = 2 * (qyqz + qiqx);
770 const float r31 = -2 * (qxqz - qiqy);
771 const float r32 = qi2 - qx2 - qy2 + qz2;
772
773 // asinf does not exist outside [-1,1]
774 BoundAbs(r21, 1.0);
775
776 e->psi = atan2f(r11, r12);
777 e->phi = asinf(r21);
778 e->theta = atan2f(r31, r32);
779}
780
789bool float_mat_inv_2d(float inv_out[4], float mat_in[4])
790{
791 float det = mat_in[0] * mat_in[3] - mat_in[1] * mat_in[2];
792
793 if (fabsf(det) < 1e-4) { return 1; } //not invertible
794
795 inv_out[0] = mat_in[3] / det;
796 inv_out[1] = -mat_in[1] / det;
797 inv_out[2] = -mat_in[2] / det;
798 inv_out[3] = mat_in[0] / det;
799
800 return 0; //return success
801}
802
811{
812 vect_out->x = mat[0] * vect_in.x + mat[1] * vect_in.y;
813 vect_out->y = mat[2] * vect_in.x + mat[3] * vect_in.y;
814}
815
824bool float_mat_inv_3d(float inv_out[3][3], float mat_in[3][3])
825{
826 const float m00 = mat_in[1][1]*mat_in[2][2] - mat_in[1][2]*mat_in[2][1];
827 const float m10 = mat_in[0][1]*mat_in[2][2] - mat_in[0][2]*mat_in[2][1];
828 const float m20 = mat_in[0][1]*mat_in[1][2] - mat_in[0][2]*mat_in[1][1];
829 const float m01 = mat_in[1][0]*mat_in[2][2] - mat_in[1][2]*mat_in[2][0];
830 const float m11 = mat_in[0][0]*mat_in[2][2] - mat_in[0][2]*mat_in[2][0];
831 const float m21 = mat_in[0][0]*mat_in[1][2] - mat_in[0][2]*mat_in[1][0];
832 const float m02 = mat_in[1][0]*mat_in[2][1] - mat_in[1][1]*mat_in[2][0];
833 const float m12 = mat_in[0][0]*mat_in[2][1] - mat_in[0][1]*mat_in[2][0];
834 const float m22 = mat_in[0][0]*mat_in[1][1] - mat_in[0][1]*mat_in[1][0];
835 const float det = mat_in[0][0]*m00 - mat_in[1][0]*m10 + mat_in[2][0]*m20;
836 if (fabs(det) > FLT_EPSILON) {
837 inv_out[0][0] = m00 / det;
838 inv_out[1][0] = -m01 / det;
839 inv_out[2][0] = m02 / det;
840 inv_out[0][1] = -m10 / det;
841 inv_out[1][1] = m11 / det;
842 inv_out[2][1] = -m12 / det;
843 inv_out[0][2] = m20 / det;
844 inv_out[1][2] = -m21 / det;
845 inv_out[2][2] = m22 / det;
846 return 0;
847 }
848 return 1;
849}
850
858void float_mat3_mult(struct FloatVect3 *vect_out, float mat[3][3], struct FloatVect3 vect_in)
859{
860 vect_out->x = mat[0][0] * vect_in.x + mat[0][1] * vect_in.y + mat[0][2] * vect_in.z;
861 vect_out->y = mat[1][0] * vect_in.x + mat[1][1] * vect_in.y + mat[1][2] * vect_in.z;
862 vect_out->z = mat[2][0] * vect_in.x + mat[2][1] * vect_in.y + mat[2][2] * vect_in.z;
863}
864
865/*
866 * 4x4 Matrix inverse.
867 * obtained from: http://rodolphe-vaillant.fr/?e=7
868 */
869
870static float float_mat_minor_4d(float m[4][4], int r0, int r1, int r2, int c0, int c1, int c2)
871{
872 return m[r0][c0] * (m[r1][c1] * m[r2][c2] - m[r2][c1] * m[r1][c2]) -
873 m[r0][c1] * (m[r1][c0] * m[r2][c2] - m[r2][c0] * m[r1][c2]) +
874 m[r0][c2] * (m[r1][c0] * m[r2][c1] - m[r2][c0] * m[r1][c1]);
875}
876
877
878static void float_mat_adjoint_4d(float adjOut[4][4], float m[4][4])
879{
880 adjOut[0][0] = float_mat_minor_4d(m, 1, 2, 3, 1, 2, 3);
881 adjOut[0][1] = -float_mat_minor_4d(m, 0, 2, 3, 1, 2, 3);
882 adjOut[0][2] = float_mat_minor_4d(m, 0, 1, 3, 1, 2, 3);
883 adjOut[0][3] = -float_mat_minor_4d(m, 0, 1, 2, 1, 2, 3);
884 adjOut[1][0] = -float_mat_minor_4d(m, 1, 2, 3, 0, 2, 3);
885 adjOut[1][1] = float_mat_minor_4d(m, 0, 2, 3, 0, 2, 3);
886 adjOut[1][2] = -float_mat_minor_4d(m, 0, 1, 3, 0, 2, 3);
887 adjOut[1][3] = float_mat_minor_4d(m, 0, 1, 2, 0, 2, 3);
888 adjOut[2][0] = float_mat_minor_4d(m, 1, 2, 3, 0, 1, 3);
889 adjOut[2][1] = -float_mat_minor_4d(m, 0, 2, 3, 0, 1, 3);
890 adjOut[2][2] = float_mat_minor_4d(m, 0, 1, 3, 0, 1, 3);
891 adjOut[2][3] = -float_mat_minor_4d(m, 0, 1, 2, 0, 1, 3);
892 adjOut[3][0] = -float_mat_minor_4d(m, 1, 2, 3, 0, 1, 2);
893 adjOut[3][1] = float_mat_minor_4d(m, 0, 2, 3, 0, 1, 2);
894 adjOut[3][2] = -float_mat_minor_4d(m, 0, 1, 3, 0, 1, 2);
895 adjOut[3][3] = float_mat_minor_4d(m, 0, 1, 2, 0, 1, 2);
896}
897
898static float float_mat_det_4d(float m[4][4])
899{
900 return m[0][0] * float_mat_minor_4d(m, 1, 2, 3, 1, 2, 3) -
901 m[0][1] * float_mat_minor_4d(m, 1, 2, 3, 0, 2, 3) +
902 m[0][2] * float_mat_minor_4d(m, 1, 2, 3, 0, 1, 3) -
903 m[0][3] * float_mat_minor_4d(m, 1, 2, 3, 0, 1, 2);
904}
905
912bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4])
913{
915
916 float det = float_mat_det_4d(mat_in);
917 if (fabsf(det) < 1e-4) { return 1; } //not invertible
918
919 float inv_det = 1.0f / det;
920 int i;
921 for (i = 0; i < 4; ++i) {
922 invOut[0][i] = invOut[0][i] * inv_det;
923 invOut[1][i] = invOut[1][i] * inv_det;
924 invOut[2][i] = invOut[2][i] * inv_det;
925 invOut[3][i] = invOut[3][i] * inv_det;
926 }
927
928 return 0; //success
929}
930
935void float_mat_invert(float **o, float **mat, int n)
936{
937 int i, j, k;
938 float t;
939 float a[n][2 * n];
940
941 // Append an identity matrix on the right of the original matrix
942 for (i = 0; i < n; i++) {
943 for (j = 0; j < 2 * n; j++) {
944 if (j < n) {
945 a[i][j] = mat[i][j];
946 } else if ((j >= n) && (j == i + n)) {
947 a[i][j] = 1.0;
948 } else {
949 a[i][j] = 0.0;
950 }
951 }
952 }
953
954 // Do the inversion
955 for (i = 0; i < n; i++) {
956 t = a[i][i]; // Store diagonal variable (temp)
957
958 for (j = i; j < 2 * n; j++) {
959 a[i][j] = a[i][j] / t; // Divide by the diagonal value
960 }
961
962 for (j = 0; j < n; j++) {
963 if (i != j) {
964 t = a[j][i];
965 for (k = 0; k < 2 * n; k++) {
966 a[j][k] = a[j][k] - t * a[i][k];
967 }
968 }
969 }
970 }
971
972 // Cut out the identity, which has now moved to the left side
973 for (i = 0 ; i < n ; i++) {
974 for (j = n; j < 2 * n; j++) {
975 o[i][j - n] = a[i][j];
976 }
977 }
978}
979
980/*
981 * o[n][n] = e^a[n][n]
982 * Replicates expm(a) in Matlab
983 *
984 * Adapted from the following reference:
985 * Cleve Moler, Charles VanLoan,
986 * Nineteen Dubious Ways to Compute the Exponential of a Matrix,
987 * Twenty-Five Years Later, SIAM Review, Volume 45, Number 1, March 2003, pages 3-49.
988 * https://people.sc.fsu.edu/~jburkardt/c_src/matrix_exponential/matrix_exponential.c
989 */
990void float_mat_exp(float **a, float **o, int n)
991{
992 float a_norm, c, t;
993 const int q = 6;
994 float d[n][n];
995 float x[n][n];
996 float a_copy[n][n];
997 int ee, k, s;
998 int p;
999
1000 MAKE_MATRIX_PTR(_a, a, n);
1001 MAKE_MATRIX_PTR(_o, o, n);
1002 MAKE_MATRIX_PTR(_d, d, n);
1003 MAKE_MATRIX_PTR(_x, x, n);
1005
1006 float_mat_copy(_a_copy, _a, n, n); // Make a copy of a to compute on
1007 a_norm = float_mat_norm_li(_a_copy, n, n); // Compute the infinity norm of the matrix
1008 ee = (int)(float_log_n(a_norm, 2)) + 1;
1009 s = Max(0, ee + 1);
1010 t = 1.0 / powf(2.0, s);
1011 float_mat_scale(_a_copy, t, n, n);
1012 float_mat_copy(_x, _a_copy, n, n); // x = a_copy
1013 c = 0.5;
1014
1015 float_mat_diagonal_scal(_o, 1.0, n); // make identiy
1016 float_mat_sum_scaled(_o, _a_copy, c, n, n);
1017
1018 float_mat_diagonal_scal(_d, 1.0, n);
1019 float_mat_sum_scaled(_d, _a_copy, -c, n, n);
1020
1021 p = 1;
1022 for (k = 2; k <= q; k++) {
1023 c = c * (float)(q - k + 1) / (float)(k * (2 * q - k + 1));
1024 float_mat_mul_copy(_x, _x, _a_copy, n, n, n);
1025
1026 float_mat_sum_scaled(_o, _x, c, n, n);
1027
1028 if (p) {
1029 float_mat_sum_scaled(_d, _x, c, n, n);
1030 } else {
1031 float_mat_sum_scaled(_d, _x, -c, n, n);
1032 }
1033 p = !p;
1034 }
1035
1036 // E -> inverse(D) * E
1037 float temp[n][n];
1038 MAKE_MATRIX_PTR(_temp, temp, n);
1040 float_mat_mul_copy(_o, _temp, _o, n, n, n);
1041
1042 // E -> E^(2*S)
1043 for (k = 1; k <= s; k++) {
1044 float_mat_mul_copy(_o, _o, _o, n, n, n);
1045 }
1046
1047}
1048
1049/* Returns L-oo of matrix a */
1050float float_mat_norm_li(float **a, int m, int n)
1051{
1052 float row_sum;
1053 float value;
1054
1055 value = 0.0;
1056 for (int i = 0; i < m; i++) {
1057 row_sum = 0.0;
1058 for (int j = 0; j < n; j++) {
1059 row_sum = row_sum + fabsf(a[i][j]);
1060 }
1061 value = Max(value, row_sum);
1062 }
1063 return value;
1064}
1065
1066/* Scale a 3D vector to within a 2D bound */
1067void float_vect3_bound_in_2d(struct FloatVect3 *vect3, float bound) {
1068 float norm = FLOAT_VECT2_NORM(*vect3);
1069 if (norm > bound) {
1070 float scale = bound / norm;
1071 vect3->x *= scale;
1072 vect3->y *= scale;
1073 }
1074}
1075
1076/* Scale a 3D vector to within a 3D bound */
1077void float_vect3_bound_in_3d(struct FloatVect3 *vect3, float bound) {
1078 float norm = FLOAT_VECT3_NORM(*vect3);
1079 if(norm>bound) {
1080 float scale = bound/norm;
1081 vect3->x *= scale;
1082 vect3->y *= scale;
1083 vect3->z *= scale;
1084 }
1085}
1086
1087/* Scale a 3D vector to a certain length in 2D */
1088void float_vect3_scale_in_2d(struct FloatVect3 *vect3, float norm_des) {
1089 float norm = FLOAT_VECT2_NORM(*vect3);
1090 if (norm > 0.01f) {
1091 float scale = norm_des / norm;
1092 vect3->x *= scale;
1093 vect3->y *= scale;
1094 }
1095}
1096
1097/* Scale a 2D vector to within a 2D bound */
1098void float_vect2_bound_in_2d(struct FloatVect2 *vect2, float bound) {
1099 float norm = FLOAT_VECT2_NORM(*vect2);
1100 if (norm > bound) {
1101 float scale = bound / norm;
1102 vect2->x *= scale;
1103 vect2->y *= scale;
1104 }
1105}
1106
1107/* Scale a 2D vector to a certain length in 2D */
1108void float_vect2_scale_in_2d(struct FloatVect2 *vect2, float norm_des) {
1109 float norm = FLOAT_VECT2_NORM(*vect2);
1110 if (norm > 0.01f) {
1111 float scale = norm_des / norm;
1112 vect2->x *= scale;
1113 vect2->y *= scale;
1114 }
1115}
static uint16_t c1
static uint16_t c2
static const float scale[]
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
float psi
in radians
void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e)
Rotation matrix from 321 Euler angles (float).
static void float_quat_normalize(struct FloatQuat *q)
void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv, float dt)
in place first order integration of a 3D-vector
void float_rmat_mult(struct FloatEulers *rb, struct FloatRMat *m_a2b, struct FloatEulers *ra)
rotate angle by rotation matrix.
void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle)
Quaternion from unit vector and angle.
float float_rmat_reorthogonalize(struct FloatRMat *rm)
void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions with normalization.
void float_mat3_mult(struct FloatVect3 *vect_out, float mat[3][3], struct FloatVect3 vect_in)
Multiply 3D matrix with vector.
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
void float_eulers_of_quat_yxz(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'YXZ' This function calculates from a quaternion the Euler angles with the order YXZ,...
void float_vect3_scale_in_2d(struct FloatVect3 *vect3, float norm_des)
void float_vect3_bound_in_2d(struct FloatVect3 *vect3, float bound)
void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions.
void float_vect2_scale_in_2d(struct FloatVect2 *vect2, float norm_des)
void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm)
Quaternion from rotation matrix.
void float_quat_of_eulers_zxy(struct FloatQuat *q, struct FloatEulers *e)
quat from euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
void float_rmat_transp_mult(struct FloatEulers *rb, struct FloatRMat *m_b2a, struct FloatEulers *ra)
rotate angle by transposed rotation matrix.
void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
void float_rmat_comp_inv(struct FloatRMat *m_a2b, struct FloatRMat *m_a2c, struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
#define FLOAT_RATES_NORM(_v)
void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
rotate anglular rates by rotation matrix.
void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e)
void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b)
Inverse/transpose of a rotation matrix.
void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
bool float_mat_inv_2d(float inv_out[4], float mat_in[4])
2x2 matrix inverse
void float_vect2_bound_in_2d(struct FloatVect2 *vect2, float bound)
void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt)
in place quaternion integration with constant rotational velocity
void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity.
#define M_SQRT2
void float_vect3_bound_in_3d(struct FloatVect3 *vect3, float bound)
static void float_mat_sum_scaled(float **a, float **b, float k, int m, int n)
a += k*b, where k is a scalar value
void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions with normalization.
static void float_mat_mul_copy(float **o, float **a, float **b, int m, int n, int l)
o = a * b
static void float_mat_copy(float **a, float **b, int m, int n)
a = b
void float_quat_of_eulers_yxz(struct FloatQuat *q, struct FloatEulers *e)
quat from euler rotation 'YXZ' This function calculates a quaternion from Euler angles with the order...
void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt)
in place first order integration of a rotation matrix
void float_rates_of_euler_dot(struct FloatRates *r, struct FloatEulers *e, struct FloatEulers *edot)
float float_mat_norm_li(float **a, int m, int n)
static void float_quat_wrap_shortest(struct FloatQuat *q)
#define FLOAT_VECT3_NORM(_v)
void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov)
Quaternion from orientation vector.
bool float_mat_inv_3d(float inv_out[3][3], float mat_in[3][3])
3x3 matrix inverse
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
quat of euler roation 'ZYX'
void float_rates_integrate_fi(struct FloatRates *r, struct FloatRates *dr, float dt)
in place first order integration of angular rates
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
float float_rmat_norm(struct FloatRMat *rm)
Norm of a rotation matrix.
void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm)
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
void float_mat_invert(float **o, float **mat, int n)
Calculate inverse of any n x n matrix (passed as C array) o = mat^-1 Algorithm verified with Matlab.
void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, struct FloatQuat *quat)
Tilt twist decomposition of a quaternion (z axis)
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
static void float_quat_invert(struct FloatQuat *qo, struct FloatQuat *qi)
bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4])
4x4 Matrix inverse
void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, float dt)
in place first order quaternion integration with constant rotational velocity
void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in)
Multiply 2D matrix with vector.
#define FLOAT_VECT2_NORM(_v)
void float_mat_exp(float **a, float **o, int n)
static float float_log_n(float v, float n)
void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle)
initialises a rotation matrix from unit vector axis and angle
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
static void float_mat_diagonal_scal(float **o, float v, int n)
Make an n x n identity matrix (for matrix passed as array)
void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt)
Delta rotation quaternion with constant angular rates.
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
static float float_quat_norm(struct FloatQuat *q)
static void float_mat_scale(float **a, float k, int m, int n)
a *= k, where k is a scalar value
void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity.
euler angles
Roation quaternion.
rotation matrix
angular rates
#define RMAT_TRACE(_rm)
#define SQUARE(_a)
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
#define VECT3_NORM2(_v)
#define RMAT_ELMT(_rm, _row, _col)
#define MAT33_ROW_VECT3_SMUL(_mat, _row, _vin, _s)
#define VECT3_SUM_SCALED(_c, _a, _b, _s)
#define VECT3_DOT_PRODUCT(_v1, _v2)
static float p[2][2]
static uint32_t s
uint16_t foo
Definition main_demo5.c:58
static float renorm_factor(float n)
static void float_mat_adjoint_4d(float adjOut[4][4], float m[4][4])
static float float_mat_det_4d(float m[4][4])
static float float_mat_minor_4d(float m[4][4], int r0, int r1, int r2, int c0, int c1, int c2)
Paparazzi floating point algebra.