31 const double sphi =
sin(e->
phi);
32 const double cphi =
cos(e->
phi);
35 const double spsi =
sin(e->
psi);
36 const double cpsi =
cos(e->
psi);
41 RMAT_ELMT(*
rm, 1, 0) = sphi * stheta * cpsi - cphi * spsi;
42 RMAT_ELMT(*
rm, 1, 1) = sphi * stheta * spsi + cphi * cpsi;
44 RMAT_ELMT(*
rm, 2, 0) = cphi * stheta * cpsi + sphi * spsi;
45 RMAT_ELMT(*
rm, 2, 1) = cphi * stheta * spsi - sphi * cpsi;
51 const double phi2 = e->
phi / 2.0;
53 const double psi2 = e->
psi / 2.0;
70 const double qx2 = q->
qx * q->
qx;
71 const double qy2 = q->
qy * q->
qy;
72 const double qz2 = q->
qz * q->
qz;
180 const double ab =
_a *
_b;
181 const double ac =
_a *
_c;
182 const double ad =
_a *
_d;
183 const double bc =
_b *
_c;
184 const double bd =
_b *
_d;
185 const double cd =
_c *
_d;
void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q)
void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in)
void double_rmat_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_a2b, struct DoubleVect3 *va)
rotate 3D vector by rotation matrix.
void double_rmat_inv(struct DoubleRMat *m_b2a, struct DoubleRMat *m_a2b)
Inverse/transpose of a rotation matrix.
void double_quat_comp(struct DoubleQuat *a2c, struct DoubleQuat *a2b, struct DoubleQuat *b2c)
Composition (multiplication) of two quaternions.
void double_rmat_of_eulers_321(struct DoubleRMat *rm, struct DoubleEulers *e)
Rotation matrix from 321 Euler angles (double).
void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e)
void double_rmat_transp_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_b2a, struct DoubleVect3 *va)
rotate 3D vector by transposed rotation matrix.
void double_rmat_comp(struct DoubleRMat *m_a2c, struct DoubleRMat *m_a2b, struct DoubleRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
void double_rmat_of_quat(struct DoubleRMat *rm, struct DoubleQuat *q)
#define RMAT_ELMT(_rm, _row, _col)
Paparazzi double precision floating point algebra.