171 const float ux2 =
uv->x *
uv->x;
172 const float uy2 =
uv->y *
uv->y;
173 const float uz2 =
uv->z *
uv->z;
196 const float sphi =
sinf(e->
phi);
197 const float cphi =
cosf(e->
phi);
200 const float spsi =
sinf(e->
psi);
201 const float cpsi =
cosf(e->
psi);
206 RMAT_ELMT(*
rm, 1, 0) = sphi * stheta * cpsi - cphi * spsi;
207 RMAT_ELMT(*
rm, 1, 1) = sphi * stheta * spsi + cphi * cpsi;
209 RMAT_ELMT(*
rm, 2, 0) = cphi * stheta * cpsi + sphi * spsi;
210 RMAT_ELMT(*
rm, 2, 1) = cphi * stheta * spsi - sphi * cpsi;
216 const float sphi =
sinf(e->
phi);
217 const float cphi =
cosf(e->
phi);
220 const float spsi =
sinf(e->
psi);
221 const float cpsi =
cosf(e->
psi);
223 RMAT_ELMT(*
rm, 0, 0) = ctheta * cpsi - sphi * stheta * spsi;
224 RMAT_ELMT(*
rm, 0, 1) = ctheta * spsi + sphi * stheta * cpsi;
229 RMAT_ELMT(*
rm, 2, 0) = stheta * cpsi + sphi * ctheta * spsi;
230 RMAT_ELMT(*
rm, 2, 1) = stheta * spsi - sphi * ctheta * cpsi;
265 1. , dt *omega->
r, -dt *omega->
q,
266 -dt *omega->
r, 1. , dt *omega->
p,
267 dt *omega->
q, -dt *omega->
p, 1.
280 return 1. /
sqrtf(n);
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;
404 const float a = 0.5 *
no * dt;
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;
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);
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);
480 const float phi2 = e->
phi / 2.f;
482 const float psi2 = e->
psi / 2.f;
506 const float phi2 = e->
phi / 2.f;
508 const float psi2 = e->
psi / 2.f;
534 const float phi2 = e->
phi / 2.f;
536 const float psi2 = e->
psi / 2.f;
553 const float san =
sinf(angle / 2.f);
554 q->
qi =
cosf(angle / 2.f);
690 const float qx2 = q->
qx * q->
qx;
691 const float qy2 = q->
qy * q->
qy;
692 const float qz2 = q->
qz * q->
qz;
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;
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;
793 if (
fabsf(
det) < 1e-4) {
return 1; }
917 if (
fabsf(
det) < 1e-4) {
return 1; }
921 for (i = 0; i < 4; ++i) {
942 for (i = 0; i < n; i++) {
943 for (
j = 0;
j < 2 * n;
j++) {
946 }
else if ((
j >= n) && (
j == i + n)) {
955 for (i = 0; i < n; i++) {
958 for (
j = i;
j < 2 * n;
j++) {
959 a[i][
j] = a[i][
j] /
t;
962 for (
j = 0;
j < n;
j++) {
965 for (k = 0; k < 2 * n; k++) {
966 a[
j][k] = a[
j][k] -
t * a[i][k];
973 for (i = 0 ; i < n ; i++) {
974 for (
j = n;
j < 2 * n;
j++) {
975 o[i][
j - n] = a[i][
j];
1022 for (k = 2; k <= q; k++) {
1023 c = c * (
float)(q - k + 1) / (
float)(k * (2 * q - k + 1));
1043 for (k = 1; k <=
s; k++) {
1056 for (
int i = 0; i < m; i++) {
1058 for (
int j = 0;
j < n;
j++) {
static const float scale[]
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.
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.
#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 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.