88 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];
89 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];
90 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];
91 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];
92 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];
93 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];
94 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];
95 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];
96 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];
104 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];
105 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];
106 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];
107 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];
108 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];
109 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];
110 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];
111 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];
112 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];
120 vb->
x = m_a2b->
m[0] * va->
x + m_a2b->
m[1] * va->
y + m_a2b->
m[2] * va->
z;
121 vb->
y = m_a2b->
m[3] * va->
x + m_a2b->
m[4] * va->
y + m_a2b->
m[5] * va->
z;
122 vb->
z = m_a2b->
m[6] * va->
x + m_a2b->
m[7] * va->
y + m_a2b->
m[8] * va->
z;
130 vb->
x = m_b2a->
m[0] * va->
x + m_b2a->
m[3] * va->
y + m_b2a->
m[6] * va->
z;
131 vb->
y = m_b2a->
m[1] * va->
x + m_b2a->
m[4] * va->
y + m_b2a->
m[7] * va->
z;
132 vb->
z = m_b2a->
m[2] * va->
x + m_b2a->
m[5] * va->
y + m_b2a->
m[8] * va->
z;
140 rb->
phi = m_a2b->
m[0] * ra->
phi + m_a2b->
m[1] * ra->
theta + m_a2b->
m[2] * ra->
psi;
142 rb->
psi = m_a2b->
m[6] * ra->
phi + m_a2b->
m[7] * ra->
theta + m_a2b->
m[8] * ra->
psi;
150 rb->
phi = m_b2a->
m[0] * ra->
phi + m_b2a->
m[3] * ra->
theta + m_b2a->
m[6] * ra->
psi;
152 rb->
psi = m_b2a->
m[2] * ra->
phi + m_b2a->
m[5] * ra->
theta + m_b2a->
m[8] * ra->
psi;
160 rb->
p = m_a2b->
m[0] * ra->
p + m_a2b->
m[1] * ra->
q + m_a2b->
m[2] * ra->
r;
161 rb->
q = m_a2b->
m[3] * ra->
p + m_a2b->
m[4] * ra->
q + m_a2b->
m[5] * ra->
r;
162 rb->
r = m_a2b->
m[6] * ra->
p + m_a2b->
m[7] * ra->
q + m_a2b->
m[8] * ra->
r;
170 rb->
p = m_b2a->
m[0] * ra->
p + m_b2a->
m[3] * ra->
q + m_b2a->
m[6] * ra->
r;
171 rb->
q = m_b2a->
m[1] * ra->
p + m_b2a->
m[4] * ra->
q + m_b2a->
m[7] * ra->
r;
172 rb->
r = m_b2a->
m[2] * ra->
p + m_b2a->
m[5] * ra->
q + m_b2a->
m[8] * ra->
r;
179 const float ux2 = uv->
x * uv->
x;
180 const float uy2 = uv->
y * uv->
y;
181 const float uz2 = uv->
z * uv->
z;
182 const float uxuy = uv->
x * uv->
y;
183 const float uyuz = uv->
y * uv->
z;
184 const float uxuz = uv->
x * uv->
z;
185 const float can = cosf(angle);
186 const float san = sinf(angle);
187 const float one_m_can = (1. - can);
189 RMAT_ELMT(*rm, 0, 0) = ux2 + (1. - ux2) * can;
190 RMAT_ELMT(*rm, 0, 1) = uxuy * one_m_can + uv->
z * san;
191 RMAT_ELMT(*rm, 0, 2) = uxuz * one_m_can - uv->
y * san;
193 RMAT_ELMT(*rm, 1, 1) = uy2 + (1. - uy2) * can;
194 RMAT_ELMT(*rm, 1, 2) = uyuz * one_m_can + uv->
x * san;
197 RMAT_ELMT(*rm, 2, 2) = uz2 + (1. - uz2) * can;
204 const float sphi = sinf(e->
phi);
205 const float cphi = cosf(e->
phi);
206 const float stheta = sinf(e->
theta);
207 const float ctheta = cosf(e->
theta);
208 const float spsi = sinf(e->
psi);
209 const float cpsi = cosf(e->
psi);
214 RMAT_ELMT(*rm, 1, 0) = sphi * stheta * cpsi - cphi * spsi;
215 RMAT_ELMT(*rm, 1, 1) = sphi * stheta * spsi + cphi * cpsi;
217 RMAT_ELMT(*rm, 2, 0) = cphi * stheta * cpsi + sphi * spsi;
218 RMAT_ELMT(*rm, 2, 1) = cphi * stheta * spsi - sphi * cpsi;
224 const float sphi = sinf(e->
phi);
225 const float cphi = cosf(e->
phi);
226 const float stheta = sinf(e->
theta);
227 const float ctheta = cosf(e->
theta);
228 const float spsi = sinf(e->
psi);
229 const float cpsi = cosf(e->
psi);
231 RMAT_ELMT(*rm, 0, 0) = ctheta * cpsi - sphi * stheta * spsi;
232 RMAT_ELMT(*rm, 0, 1) = ctheta * spsi + sphi * stheta * cpsi;
237 RMAT_ELMT(*rm, 2, 0) = stheta * cpsi + sphi * ctheta * spsi;
238 RMAT_ELMT(*rm, 2, 1) = stheta * spsi - sphi * ctheta * cpsi;
250 const float a2_1 = _a * _a - 1;
251 const float ab = _a * _b;
252 const float ac = _a * _c;
253 const float ad = _a * _d;
254 const float bc = _b * _c;
255 const float bd = _b * _d;
256 const float cd = _c * _d;
273 1., dt *omega->
r, -dt *omega->
q,
274 -dt *omega->
r, 1., dt *omega->
p,
275 dt *omega->
q, -dt *omega->
p, 1.
280 memcpy(rm, &R_tdt,
sizeof(R_tdt));
285 if (n < 1.5625f && n > 0.64f) {
287 }
else if (n < 100.0f && n > 0.01f) {
288 return 1. / sqrtf(n);
330 a2c->
qi = a2b->
qi * b2c->
qi - a2b->
qx * b2c->
qx - a2b->
qy * b2c->
qy - a2b->
qz * b2c->
qz;
331 a2c->
qx = a2b->
qi * b2c->
qx + a2b->
qx * b2c->
qi + a2b->
qy * b2c->
qz - a2b->
qz * b2c->
qy;
332 a2c->
qy = a2b->
qi * b2c->
qy - a2b->
qx * b2c->
qz + a2b->
qy * b2c->
qi + a2b->
qz * b2c->
qx;
333 a2c->
qz = a2b->
qi * b2c->
qz + a2b->
qx * b2c->
qy - a2b->
qy * b2c->
qx + a2b->
qz * b2c->
qi;
338 a2b->
qi = a2c->
qi * b2c->
qi + a2c->
qx * b2c->
qx + a2c->
qy * b2c->
qy + a2c->
qz * b2c->
qz;
339 a2b->
qx = -a2c->
qi * b2c->
qx + a2c->
qx * b2c->
qi - a2c->
qy * b2c->
qz + a2c->
qz * b2c->
qy;
340 a2b->
qy = -a2c->
qi * b2c->
qy + a2c->
qx * b2c->
qz + a2c->
qy * b2c->
qi - a2c->
qz * b2c->
qx;
341 a2b->
qz = -a2c->
qi * b2c->
qz - a2c->
qx * b2c->
qy + a2c->
qy * b2c->
qx + a2c->
qz * b2c->
qi;
346 b2c->
qi = a2b->
qi * a2c->
qi + a2b->
qx * a2c->
qx + a2b->
qy * a2c->
qy + a2b->
qz * a2c->
qz;
347 b2c->
qx = a2b->
qi * a2c->
qx - a2b->
qx * a2c->
qi - a2b->
qy * a2c->
qz + a2b->
qz * a2c->
qy;
348 b2c->
qy = a2b->
qi * a2c->
qy + a2b->
qx * a2c->
qz - a2b->
qy * a2c->
qi - a2b->
qz * a2c->
qx;
349 b2c->
qz = a2b->
qi * a2c->
qz - a2b->
qx * a2c->
qy + a2b->
qy * a2c->
qx - a2b->
qz * a2c->
qi;
375 const float v_norm = sqrtf(w->
p * w->
p + w->
q * w->
q + w->
r * w->
r);
376 const float c2 = cos(dt * v_norm / 2.0);
377 const float s2 = sin(dt * v_norm / 2.0);
385 q_out->
qx = w->
p / v_norm * s2;
386 q_out->
qy = w->
q / v_norm * s2;
387 q_out->
qz = w->
r / v_norm * s2;
394 const float qi = q->
qi;
395 const float qx = q->
qx;
396 const float qy = q->
qy;
397 const float qz = q->
qz;
398 const float dp = 0.5 * dt * omega->
p;
399 const float dq = 0.5 * dt * omega->
q;
400 const float dr = 0.5 * dt * omega->
r;
401 q->
qi = qi - dp * qx - dq * qy - dr * qz;
402 q->
qx = dp * qi + qx + dr * qy - dq * qz;
403 q->
qy = dq * qi - dr * qx + qy + dp * qz;
404 q->
qz = dr * qi + dq * qx - dp * qy + qz;
412 const float a = 0.5 * no * dt;
413 const float ca = cosf(a);
414 const float sa_ov_no = sinf(a) / no;
415 const float dp = sa_ov_no * omega->
p;
416 const float dq = sa_ov_no * omega->
q;
417 const float dr = sa_ov_no * omega->
r;
418 const float qi = q->
qi;
419 const float qx = q->
qx;
420 const float qy = q->
qy;
421 const float qz = q->
qz;
422 q->
qi = ca * qi - dp * qx - dq * qy - dr * qz;
423 q->
qx = dp * qi + ca * qx + dr * qy - dq * qz;
424 q->
qy = dq * qi - dr * qx + ca * qy + dp * qz;
425 q->
qz = dr * qi + dq * qx - dp * qy + ca * qz;
431 const float qi2_M1_2 = q->
qi * q->
qi - 0.5;
432 const float qiqx = q->
qi * q->
qx;
433 const float qiqy = q->
qi * q->
qy;
434 const float qiqz = q->
qi * q->
qz;
435 float m01 = q->
qx * q->
qy;
436 float m02 = q->
qx * q->
qz;
437 float m12 = q->
qy * q->
qz;
439 const float m00 = qi2_M1_2 + q->
qx * q->
qx;
440 const float m10 = m01 - qiqz;
441 const float m20 = m02 + qiqy;
442 const float m21 = m12 - qiqx;
446 const float m11 = qi2_M1_2 + q->
qy * q->
qy;
447 const float m22 = qi2_M1_2 + q->
qz * q->
qz;
448 v_out->
x = 2 * (m00 * v_in->
x + m01 * v_in->
y + m02 * v_in->
z);
449 v_out->
y = 2 * (m10 * v_in->
x + m11 * v_in->
y + m12 * v_in->
z);
450 v_out->
z = 2 * (m20 * v_in->
x + m21 * v_in->
y + m22 * v_in->
z);
460 qd->
qi = -0.5 * (r->
p * q->
qx + r->
q * q->
qy + r->
r * q->
qz);
461 qd->
qx = -0.5 * (-r->
p * q->
qi - r->
r * q->
qy + r->
q * q->
qz);
462 qd->
qy = -0.5 * (-r->
q * q->
qi + r->
r * q->
qx - r->
p * q->
qz);
463 qd->
qz = -0.5 * (-r->
r * q->
qi - r->
q * q->
qx + r->
p * q->
qy);
471 const float K_LAGRANGE = 1.;
473 qd->
qi = -0.5 * (c * q->
qi + r->
p * q->
qx + r->
q * q->
qy + r->
r * q->
qz);
474 qd->
qx = -0.5 * (-r->
p * q->
qi + c * q->
qx - r->
r * q->
qy + r->
q * q->
qz);
475 qd->
qy = -0.5 * (-r->
q * q->
qi + r->
r * q->
qx + c * q->
qy - r->
p * q->
qz);
476 qd->
qz = -0.5 * (-r->
r * q->
qi - r->
q * q->
qx + r->
p * q->
qy + c * q->
qz);
488 const float phi2 = e->
phi / 2.f;
489 const float theta2 = e->
theta / 2.f;
490 const float psi2 = e->
psi / 2.f;
492 const float s_phi2 = sinf(phi2);
493 const float c_phi2 = cosf(phi2);
494 const float s_theta2 = sinf(theta2);
495 const float c_theta2 = cosf(theta2);
496 const float s_psi2 = sinf(psi2);
497 const float c_psi2 = cosf(psi2);
499 q->
qi = c_phi2 * c_theta2 * c_psi2 + s_phi2 * s_theta2 * s_psi2;
500 q->
qx = -c_phi2 * s_theta2 * s_psi2 + s_phi2 * c_theta2 * c_psi2;
501 q->
qy = c_phi2 * s_theta2 * c_psi2 + s_phi2 * c_theta2 * s_psi2;
502 q->
qz = c_phi2 * c_theta2 * s_psi2 - s_phi2 * s_theta2 * c_psi2;
514 const float phi2 = e->
phi / 2.f;
515 const float theta2 = e->
theta / 2.f;
516 const float psi2 = e->
psi / 2.f;
518 const float s_phi2 = sinf(phi2);
519 const float c_phi2 = cosf(phi2);
520 const float s_theta2 = sinf(theta2);
521 const float c_theta2 = cosf(theta2);
522 const float s_psi2 = sinf(psi2);
523 const float c_psi2 = cosf(psi2);
525 q->
qi = c_phi2 * c_theta2 * c_psi2 - s_phi2 * s_theta2 * s_psi2;
526 q->
qx = s_phi2 * c_theta2 * c_psi2 - c_phi2 * s_theta2 * s_psi2;
527 q->
qy = c_phi2 * s_theta2 * c_psi2 + s_phi2 * c_theta2 * s_psi2;
528 q->
qz = s_phi2 * s_theta2 * c_psi2 + c_phi2 * c_theta2 * s_psi2;
542 const float phi2 = e->
phi / 2.f;
543 const float theta2 = e->
theta / 2.f;
544 const float psi2 = e->
psi / 2.f;
546 const float s_phi2 = sinf(phi2);
547 const float c_phi2 = cosf(phi2);
548 const float s_theta2 = sinf(theta2);
549 const float c_theta2 = cosf(theta2);
550 const float s_psi2 = sinf(psi2);
551 const float c_psi2 = cosf(psi2);
553 q->
qi = c_theta2 * c_phi2 * c_psi2 + s_theta2 * s_phi2 * s_psi2;
554 q->
qx = c_theta2 * s_phi2 * c_psi2 + s_theta2 * c_phi2 * s_psi2;
555 q->
qy = s_theta2 * c_phi2 * c_psi2 - c_theta2 * s_phi2 * s_psi2;
556 q->
qz = c_theta2 * c_phi2 * s_psi2 - s_theta2 * s_phi2 * c_psi2;
561 const float san = sinf(angle / 2.f);
562 q->
qi = cosf(angle / 2.f);
570 const float ov_norm = sqrtf(ov->
x * ov->
x + ov->
y * ov->
y + ov->
z * ov->
z);
571 if (ov_norm < 1e-8) {
577 const float s2_normalized = sinf(ov_norm / 2.0) / ov_norm;
578 q->
qi = cosf(ov_norm / 2.0);
579 q->
qx = ov->
x * s2_normalized;
580 q->
qy = ov->
y * s2_normalized;
581 q->
qz = ov->
z * s2_normalized;
589 const float two_qi = sqrtf(1. + tr);
590 const float four_qi = 2. * two_qi;
591 q->
qi = 0.5 * two_qi;
601 const float four_qx = 2. * two_qx;
603 q->
qx = 0.5 * two_qx;
610 const float four_qy = 2. * two_qy;
613 q->
qy = 0.5 * two_qy;
619 const float four_qz = 2. * two_qz;
623 q->
qz = 0.5 * two_qz;
656 tilt->
qi = 1 + z_rot.
z;
676 const float dcm00 = rm->
m[0];
677 const float dcm01 = rm->
m[1];
678 float dcm02 = rm->
m[2];
679 const float dcm12 = rm->
m[5];
680 const float dcm22 = rm->
m[8];
683 BoundAbs(dcm02, 1.0);
685 e->
phi = atan2f(dcm12, dcm22);
686 e->
theta = -asinf(dcm02);
687 e->
psi = atan2f(dcm01, dcm00);
698 const float qx2 = q->
qx * q->
qx;
699 const float qy2 = q->
qy * q->
qy;
700 const float qz2 = q->
qz * q->
qz;
701 const float qiqx = q->
qi * q->
qx;
702 const float qiqy = q->
qi * q->
qy;
703 const float qiqz = q->
qi * q->
qz;
704 const float qxqy = q->
qx * q->
qy;
705 const float qxqz = q->
qx * q->
qz;
706 const float qyqz = q->
qy * q->
qz;
707 const float dcm00 = 1.0 - 2.*(qy2 + qz2);
708 const float dcm01 = 2.*(qxqy + qiqz);
709 float dcm02 = 2.*(qxqz - qiqy);
710 const float dcm12 = 2.*(qyqz + qiqx);
711 const float dcm22 = 1.0 - 2.*(qx2 + qy2);
714 BoundAbs(dcm02, 1.0);
716 e->
phi = atan2f(dcm12, dcm22);
717 e->
theta = -asinf(dcm02);
718 e->
psi = atan2f(dcm01, dcm00);
732 const float qx2 = q->
qx * q->
qx;
733 const float qy2 = q->
qy * q->
qy;
734 const float qz2 = q->
qz * q->
qz;
735 const float qi2 = q->
qi * q->
qi;
736 const float qiqx = q->
qi * q->
qx;
737 const float qiqy = q->
qi * q->
qy;
738 const float qiqz = q->
qi * q->
qz;
739 const float qxqy = q->
qx * q->
qy;
740 const float qxqz = q->
qx * q->
qz;
741 const float qyqz = q->
qy * q->
qz;
742 const float r11 = 2.f * (qxqz + qiqy);
743 const float r12 = qi2 - qx2 + qy2 + qz2;
744 float r21 = -2.f * (qyqz - qiqx);
745 const float r31 = 2.f * (qxqy + qiqz);
746 const float r32 = qi2 - qx2 + qy2 - qz2;
751 e->
theta = atan2f(r11, r12);
753 e->
psi = atan2f(r31, r32);
765 const float qx2 = q->
qx * q->
qx;
766 const float qy2 = q->
qy * q->
qy;
767 const float qz2 = q->
qz * q->
qz;
768 const float qi2 = q->
qi * q->
qi;
769 const float qiqx = q->
qi * q->
qx;
770 const float qiqy = q->
qi * q->
qy;
771 const float qiqz = q->
qi * q->
qz;
772 const float qxqy = q->
qx * q->
qy;
773 const float qxqz = q->
qx * q->
qz;
774 const float qyqz = q->
qy * q->
qz;
775 const float r11 = -2 * (qxqy - qiqz);
776 const float r12 = qi2 - qx2 + qy2 - qz2;
777 float r21 = 2 * (qyqz + qiqx);
778 const float r31 = -2 * (qxqz - qiqy);
779 const float r32 = qi2 - qx2 - qy2 + qz2;
784 e->
psi = atan2f(r11, r12);
786 e->
theta = atan2f(r31, r32);
799 float det = mat_in[0] * mat_in[3] - mat_in[1] * mat_in[2];
801 if (fabsf(det) < 1e-4) {
return 1; }
803 inv_out[0] = mat_in[3] / det;
804 inv_out[1] = -mat_in[1] / det;
805 inv_out[2] = -mat_in[2] / det;
806 inv_out[3] = mat_in[0] / det;
820 vect_out->
x = mat[0] * vect_in.
x + mat[1] * vect_in.
y;
821 vect_out->
y = mat[2] * vect_in.
x + mat[3] * vect_in.
y;
834 const float m00 = mat_in[1][1] * mat_in[2][2] - mat_in[1][2] * mat_in[2][1];
835 const float m10 = mat_in[0][1] * mat_in[2][2] - mat_in[0][2] * mat_in[2][1];
836 const float m20 = mat_in[0][1] * mat_in[1][2] - mat_in[0][2] * mat_in[1][1];
837 const float m01 = mat_in[1][0] * mat_in[2][2] - mat_in[1][2] * mat_in[2][0];
838 const float m11 = mat_in[0][0] * mat_in[2][2] - mat_in[0][2] * mat_in[2][0];
839 const float m21 = mat_in[0][0] * mat_in[1][2] - mat_in[0][2] * mat_in[1][0];
840 const float m02 = mat_in[1][0] * mat_in[2][1] - mat_in[1][1] * mat_in[2][0];
841 const float m12 = mat_in[0][0] * mat_in[2][1] - mat_in[0][1] * mat_in[2][0];
842 const float m22 = mat_in[0][0] * mat_in[1][1] - mat_in[0][1] * mat_in[1][0];
843 const float det = mat_in[0][0] * m00 - mat_in[1][0] * m10 + mat_in[2][0] * m20;
844 if (fabs(det) > FLT_EPSILON) {
845 inv_out[0][0] = m00 / det;
846 inv_out[1][0] = -m01 / det;
847 inv_out[2][0] = m02 / det;
848 inv_out[0][1] = -m10 / det;
849 inv_out[1][1] = m11 / det;
850 inv_out[2][1] = -m12 / det;
851 inv_out[0][2] = m20 / det;
852 inv_out[1][2] = -m21 / det;
853 inv_out[2][2] = m22 / det;
868 vect_out->
x = mat[0][0] * vect_in.
x + mat[0][1] * vect_in.
y + mat[0][2] * vect_in.
z;
869 vect_out->
y = mat[1][0] * vect_in.
x + mat[1][1] * vect_in.
y + mat[1][2] * vect_in.
z;
870 vect_out->
z = mat[2][0] * vect_in.
x + mat[2][1] * vect_in.
y + mat[2][2] * vect_in.
z;
880 return m[r0][c0] * (m[r1][
c1] * m[r2][
c2] - m[r2][
c1] * m[r1][
c2]) -
881 m[r0][
c1] * (m[r1][c0] * m[r2][
c2] - m[r2][c0] * m[r1][
c2]) +
882 m[r0][
c2] * (m[r1][c0] * m[r2][
c1] - m[r2][c0] * m[r1][
c1]);
925 if (fabsf(det) < 1e-4) {
return 1; }
927 float inv_det = 1.0f / det;
929 for (i = 0; i < 4; ++i) {
930 invOut[0][i] = invOut[0][i] * inv_det;
931 invOut[1][i] = invOut[1][i] * inv_det;
932 invOut[2][i] = invOut[2][i] * inv_det;
933 invOut[3][i] = invOut[3][i] * inv_det;
950 for (i = 0; i < n; i++) {
951 for (j = 0; j < 2 * n; j++) {
954 }
else if ((j >= n) && (j == i + n)) {
963 for (i = 0; i < n; i++) {
966 for (j = i; j < 2 * n; j++) {
967 a[i][j] = a[i][j] / t;
970 for (j = 0; j < n; j++) {
973 for (k = 0; k < 2 * n; k++) {
974 a[j][k] = a[j][k] - t * a[i][k];
981 for (i = 0 ; i < n ; i++) {
982 for (j = n; j < 2 * n; j++) {
983 o[i][j - n] = a[i][j];
1018 t = 1.0 / powf(2.0,
s);
1030 for (k = 2; k <= q; k++) {
1031 c = c * (float)(q - k + 1) / (float)(k * (2 * q - k + 1));
1051 for (k = 1; k <=
s; k++) {
1063 for (
int i = 0; i < m; i++) {
1065 for (
int j = 0; j < n; j++) {
1066 row_sum = row_sum + fabsf(a[i][j]);
1068 value = Max(value, row_sum);
1078 float scale = bound / norm;
1089 float scale = bound / norm;
1101 float scale = norm_des / norm;
1112 float scale = bound / norm;
1123 float scale = norm_des / norm;
static void float_quat_normalize(struct FloatQuat *q)
void float_vect2_scale_in_2d(struct FloatVect2 *vect2, const float norm_des)
void float_rates_of_euler_dot(struct FloatRates *r, const struct FloatEulers *e, const struct FloatEulers *edot)
void float_vect3_bound_in_3d(struct FloatVect3 *vect3, const float bound)
float float_rmat_reorthogonalize(struct FloatRMat *rm)
void float_eulers_of_quat_zxy(struct FloatEulers *e, const struct FloatQuat *q)
euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
void float_vect3_scale_in_2d(struct FloatVect3 *vect3, const float norm_des)
void float_mat3_mult(struct FloatVect3 *vect_out, const float mat[3][3], const struct FloatVect3 vect_in)
Multiply 3D matrix with vector.
float float_rmat_norm(const struct FloatRMat *rm)
Norm of a rotation matrix.
void float_rmat_transp_ratemult(struct FloatRates *rb, const struct FloatRMat *m_b2a, const struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
void float_quat_derivative_lagrange(struct FloatQuat *qd, const struct FloatRates *r, const struct FloatQuat *q)
Quaternion derivative from rotational velocity.
void float_mat_invert(float **o, float **mat, const int n)
Calculate inverse of any n x n matrix (passed as C array) o = mat^-1 Algorithm verified with Matlab.
#define FLOAT_RATES_NORM(_v)
void float_rmat_of_eulers_312(struct FloatRMat *rm, const struct FloatEulers *e)
void float_quat_inv_comp(struct FloatQuat *b2c, const struct FloatQuat *a2b, const struct FloatQuat *a2c)
Composition (multiplication) of two quaternions.
static void float_mat_scale(float **a, float k, const int m, const int n)
a *= k, where k is a scalar value
void float_vect3_integrate_fi(struct FloatVect3 *vec, const struct FloatVect3 *dv, const float dt)
in place first order integration of a 3D-vector
void float_quat_of_rmat(struct FloatQuat *q, const struct FloatRMat *rm)
Quaternion from rotation matrix.
void float_mat2_mult(struct FloatVect2 *vect_out, const float mat[4], const struct FloatVect2 vect_in)
Multiply 2D matrix with vector.
void float_eulers_of_quat_yxz(struct FloatEulers *e, const struct FloatQuat *q)
euler rotation 'YXZ' This function calculates from a quaternion the Euler angles with the order YXZ,...
void float_rmat_transp_mult(struct FloatEulers *rb, const struct FloatRMat *m_b2a, const struct FloatEulers *ra)
rotate angle by transposed rotation matrix.
void float_rmat_integrate_fi(struct FloatRMat *rm, const struct FloatRates *omega, float dt)
in place first order integration of a rotation matrix
void float_quat_differential(struct FloatQuat *q_out, const struct FloatRates *w, const float dt)
Delta rotation quaternion with constant angular rates.
void float_quat_comp_norm_shortest(struct FloatQuat *a2c, const struct FloatQuat *a2b, const struct FloatQuat *b2c)
Composition (multiplication) of two quaternions with normalization.
void float_eulers_of_rmat(struct FloatEulers *e, const struct FloatRMat *rm)
void float_quat_of_eulers_yxz(struct FloatQuat *q, const struct FloatEulers *e)
quat from euler rotation 'YXZ' This function calculates a quaternion from Euler angles with the order...
void float_rates_vect3_integrate_fi(struct FloatRates *r, const struct FloatVect3 *dr, const float dt)
in place first order integration of angular rates
bool float_mat_inv_3d(float inv_out[3][3], const float mat_in[3][3])
3x3 matrix inverse
void float_rmat_of_eulers_321(struct FloatRMat *rm, const struct FloatEulers *e)
Rotation matrix from 321 Euler angles (float).
static float float_log_n(const float v, const float n)
void float_quat_of_eulers_zxy(struct FloatQuat *q, const struct FloatEulers *e)
quat from euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
void float_quat_integrate_fi(struct FloatQuat *q, const struct FloatRates *omega, const float dt)
in place first order quaternion integration with constant rotational velocity
static void float_mat_copy(float **a, float **b, const int m, const int n)
a = b
static void float_mat_diagonal_scal(float **o, float v, const int n)
Make an n x n identity matrix (for matrix passed as array).
void float_quat_comp(struct FloatQuat *a2c, const struct FloatQuat *a2b, const struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
static void float_quat_invert(struct FloatQuat *qo, const struct FloatQuat *qi)
float float_mat_norm_li(float **a, const int m, const int n)
void float_rmat_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_a2b, const struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
bool float_mat_inv_2d(float inv_out[4], const float mat_in[4])
2x2 matrix inverse
void float_quat_integrate(struct FloatQuat *q, const struct FloatRates *omega, const float dt)
in place quaternion integration with constant rotational velocity
static void float_quat_wrap_shortest(struct FloatQuat *q)
void float_rmat_ratemult(struct FloatRates *rb, const struct FloatRMat *m_a2b, const struct FloatRates *ra)
rotate anglular rates by rotation matrix.
#define FLOAT_VECT3_NORM(_v)
void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov)
Quaternion from orientation vector.
void float_rmat_comp_inv(struct FloatRMat *m_a2b, const struct FloatRMat *m_a2c, const struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
void float_rates_integrate_fi(struct FloatRates *r, const struct FloatRates *dr, const float dt)
in place first order integration of angular rates
void float_rmat_mult(struct FloatEulers *rb, const struct FloatRMat *m_a2b, const struct FloatEulers *ra)
rotate angle by rotation matrix.
static void float_mat_sum_scaled(float **a, float **b, const float k, const int m, const int n)
a += k*b, where k is a scalar value
void float_vect3_bound_in_2d(struct FloatVect3 *vect3, const float bound)
bool float_mat_inv_4d(float invOut[4][4], const float mat_in[4][4])
4x4 Matrix inverse
void float_quat_of_eulers(struct FloatQuat *q, const struct FloatEulers *e)
quat of euler roation 'ZYX'
void float_eulers_of_quat(struct FloatEulers *e, const struct FloatQuat *q)
euler rotation 'ZYX'
static float float_quat_norm(const struct FloatQuat *q)
void float_rmat_of_quat(struct FloatRMat *rm, const struct FloatQuat *q)
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, const struct FloatQuat *quat)
Tilt twist decomposition of a quaternion (z axis).
void float_vect2_bound_in_2d(struct FloatVect2 *vect2, const float bound)
void float_rmat_of_axis_angle(struct FloatRMat *rm, const struct FloatVect3 *uv, const float angle)
initialises a rotation matrix from unit vector axis and angle
void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, const struct FloatQuat *a2c, const struct FloatQuat *b2c)
Composition (multiplication) of two quaternions with normalization.
void float_mat_exp(float **a, float **o, const int n)
void float_rmat_transp_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_b2a, const struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
void float_rmat_inv(struct FloatRMat *m_b2a, const struct FloatRMat *m_a2b)
Inverse/transpose of a rotation matrix.
void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, const float angle)
Quaternion from unit vector and angle.
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, const struct FloatQuat *a2b, const struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
void float_quat_comp_inv(struct FloatQuat *a2b, const struct FloatQuat *a2c, const struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
#define FLOAT_VECT2_NORM(_v)
static void float_mat_mul_copy(float **o, float **a, float **b, const int m, const int n, const int l)
o = a * b
void float_rmat_comp(struct FloatRMat *m_a2c, const struct FloatRMat *m_a2b, const struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
void float_quat_derivative(struct FloatQuat *qd, const struct FloatRates *r, const struct FloatQuat *q)
Quaternion derivative from rotational velocity.
void float_quat_vmult(struct FloatVect3 *v_out, const struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
#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 renorm_factor(float n)
static float float_mat_det_4d(const float m[4][4])
static float float_mat_minor_4d(const float m[4][4], int r0, int r1, int r2, int c0, int c1, int c2)
static void float_mat_adjoint_4d(float adjOut[4][4], const float m[4][4])
Paparazzi floating point algebra.
static const float scale[]