mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-23 13:24:03 +08:00
[math] some dox updates
This commit is contained in:
@@ -51,9 +51,7 @@ void float_rates_of_euler_dot(struct FloatRates* r, struct FloatEulers* e, struc
|
||||
|
||||
|
||||
|
||||
/** Inverse/transpose of a rotation matrix.
|
||||
* _m_b2a = inv(_m_a2b) = transp(_m_a2b)
|
||||
*/
|
||||
|
||||
void float_rmat_inv(struct FloatRMat* m_b2a, struct FloatRMat* m_a2b)
|
||||
{
|
||||
RMAT_ELMT(*m_b2a, 0, 0) = RMAT_ELMT(*m_a2b, 0, 0);
|
||||
@@ -75,7 +73,7 @@ float float_rmat_norm(struct FloatRMat* rm)
|
||||
}
|
||||
|
||||
/** Composition (multiplication) of two rotation matrices.
|
||||
* _m_a2c = _m_a2b comp _m_b2c , aka _m_a2c = _m_b2c * _m_a2b
|
||||
* m_a2c = m_a2b comp m_b2c , aka m_a2c = m_b2c * m_a2b
|
||||
*/
|
||||
void float_rmat_comp(struct FloatRMat* m_a2c, struct FloatRMat* m_a2b, struct FloatRMat* m_b2c)
|
||||
{
|
||||
@@ -91,7 +89,7 @@ void float_rmat_comp(struct FloatRMat* m_a2c, struct FloatRMat* m_a2b, struct Fl
|
||||
}
|
||||
|
||||
/** Composition (multiplication) of two rotation matrices.
|
||||
* _m_a2b = _m_a2c comp_inv _m_b2c , aka _m_a2b = inv(_m_b2c) * _m_a2c
|
||||
* m_a2b = m_a2c comp_inv m_b2c , aka m_a2b = inv(_m_b2c) * m_a2c
|
||||
*/
|
||||
void float_rmat_comp_inv(struct FloatRMat* m_a2b, struct FloatRMat* m_a2c, struct FloatRMat* m_b2c)
|
||||
{
|
||||
@@ -307,7 +305,6 @@ void float_quat_comp(struct FloatQuat* a2c, struct FloatQuat* a2b, struct FloatQ
|
||||
a2c->qz = a2b->qi * b2c->qz + a2b->qx * b2c->qy - a2b->qy * b2c->qx + a2b->qz * b2c->qi;
|
||||
}
|
||||
|
||||
/* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
|
||||
void float_quat_comp_inv(struct FloatQuat* a2b, struct FloatQuat* a2c, struct FloatQuat* b2c)
|
||||
{
|
||||
a2b->qi = a2c->qi * b2c->qi + a2c->qx * b2c->qx + a2c->qy * b2c->qy + a2c->qz * b2c->qz;
|
||||
@@ -316,7 +313,6 @@ void float_quat_comp_inv(struct FloatQuat* a2b, struct FloatQuat* a2c, struct Fl
|
||||
a2b->qz = -a2c->qi * b2c->qz - a2c->qx * b2c->qy + a2c->qy * b2c->qx + a2c->qz * b2c->qi;
|
||||
}
|
||||
|
||||
/* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
|
||||
void float_quat_inv_comp(struct FloatQuat* b2c, struct FloatQuat* a2b, struct FloatQuat* a2c)
|
||||
{
|
||||
b2c->qi = a2b->qi * a2c->qi + a2b->qx * a2c->qx + a2b->qy * a2c->qy + a2b->qz * a2c->qz;
|
||||
@@ -325,7 +321,6 @@ void float_quat_inv_comp(struct FloatQuat* b2c, struct FloatQuat* a2b, struct Fl
|
||||
b2c->qz = a2b->qi * a2c->qz - a2b->qx * a2c->qy + a2b->qy * a2c->qx - a2b->qz * a2c->qi;
|
||||
}
|
||||
|
||||
/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
|
||||
void float_quat_comp_norm_shortest(struct FloatQuat* a2c, struct FloatQuat* a2b, struct FloatQuat* b2c)
|
||||
{
|
||||
float_quat_comp(a2c, a2b, b2c);
|
||||
@@ -333,7 +328,6 @@ void float_quat_comp_norm_shortest(struct FloatQuat* a2c, struct FloatQuat* a2b,
|
||||
float_quat_normalize(a2c);
|
||||
}
|
||||
|
||||
/* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
|
||||
void float_quat_comp_inv_norm_shortest(struct FloatQuat* a2b, struct FloatQuat* a2c, struct FloatQuat* b2c)
|
||||
{
|
||||
float_quat_comp_inv(a2b, a2c, b2c);
|
||||
@@ -341,7 +335,6 @@ void float_quat_comp_inv_norm_shortest(struct FloatQuat* a2b, struct FloatQuat*
|
||||
float_quat_normalize(a2b);
|
||||
}
|
||||
|
||||
/* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
|
||||
void float_quat_inv_comp_norm_shortest(struct FloatQuat* b2c, struct FloatQuat* a2b, struct FloatQuat* a2c)
|
||||
{
|
||||
float_quat_inv_comp(b2c, a2b, a2c);
|
||||
|
||||
@@ -238,20 +238,57 @@ static inline void float_rmat_identity(struct FloatRMat* rm)
|
||||
FLOAT_MAT33_DIAG(*rm, 1., 1., 1.);
|
||||
}
|
||||
|
||||
/** Inverse/transpose of a rotation matrix.
|
||||
* m_b2a = inv(_m_a2b) = transp(_m_a2b)
|
||||
*/
|
||||
extern void float_rmat_inv(struct FloatRMat* m_b2a, struct FloatRMat* m_a2b);
|
||||
|
||||
/** Composition (multiplication) of two rotation matrices.
|
||||
* m_a2c = m_a2b comp m_b2c , aka m_a2c = m_b2c * m_a2b
|
||||
*/
|
||||
extern void float_rmat_comp(struct FloatRMat* m_a2c, struct FloatRMat* m_a2b,
|
||||
struct FloatRMat* m_b2c);
|
||||
|
||||
/** Composition (multiplication) of two rotation matrices.
|
||||
* m_a2b = m_a2c comp_inv m_b2c , aka m_a2b = inv(_m_b2c) * m_a2c
|
||||
*/
|
||||
extern void float_rmat_comp_inv(struct FloatRMat* m_a2b, struct FloatRMat* m_a2c,
|
||||
struct FloatRMat* m_b2c);
|
||||
|
||||
/// Norm of a rotation matrix.
|
||||
extern float float_rmat_norm(struct FloatRMat* rm);
|
||||
|
||||
/** rotate 3D vector by rotation matrix.
|
||||
* vb = m_a2b * va
|
||||
*/
|
||||
extern void float_rmat_vmult(struct FloatVect3* vb, struct FloatRMat* m_a2b,
|
||||
struct FloatVect3* va);
|
||||
|
||||
/** rotate 3D vector by transposed rotation matrix.
|
||||
* vb = m_b2a^T * va
|
||||
*/
|
||||
extern void float_rmat_transp_vmult(struct FloatVect3* vb, struct FloatRMat* m_b2a,
|
||||
struct FloatVect3* va);
|
||||
|
||||
/** rotate anglular rates by rotation matrix.
|
||||
* rb = m_a2b * ra
|
||||
*/
|
||||
extern void float_rmat_ratemult(struct FloatRates* rb, struct FloatRMat* m_a2b,
|
||||
struct FloatRates* ra);
|
||||
|
||||
/** rotate anglular rates by transposed rotation matrix.
|
||||
* rb = m_b2a^T * ra
|
||||
*/
|
||||
extern void float_rmat_transp_ratemult(struct FloatRates* rb, struct FloatRMat* m_b2a,
|
||||
struct FloatRates* ra);
|
||||
|
||||
/** initialises a rotation matrix from unit vector axis and angle */
|
||||
extern void float_rmat_of_axis_angle(struct FloatRMat* rm, struct FloatVect3* uv, float angle);
|
||||
/* C n->b rotation matrix */
|
||||
|
||||
extern void float_rmat_of_eulers_321(struct FloatRMat* rm, struct FloatEulers* e);
|
||||
extern void float_rmat_of_eulers_312(struct FloatRMat* rm, struct FloatEulers* e);
|
||||
#define float_rmat_of_eulers float_rmat_of_eulers_321
|
||||
/* C n->b rotation matrix */
|
||||
|
||||
extern void float_rmat_of_quat(struct FloatRMat* rm, struct FloatQuat* q);
|
||||
/** in place first order integration of a rotation matrix */
|
||||
extern void float_rmat_integrate_fi(struct FloatRMat* rm, struct FloatRates* omega, float dt);
|
||||
@@ -322,17 +359,6 @@ static inline void float_quat_wrap_shortest(struct FloatQuat* q)
|
||||
|
||||
#define FLOAT_QUAT_EXTRACT(_vo, _qi) QUAT_EXTRACT_Q(_vo, _qi)
|
||||
|
||||
extern void float_rmat_vmult(struct FloatVect3* vb, struct FloatRMat* m_a2b,
|
||||
struct FloatVect3* va);
|
||||
|
||||
extern void float_rmat_transp_vmult(struct FloatVect3* vb, struct FloatRMat* m_b2a,
|
||||
struct FloatVect3* va);
|
||||
|
||||
extern void float_rmat_ratemult(struct FloatRates* rb, struct FloatRMat* m_a2b,
|
||||
struct FloatRates* ra);
|
||||
extern void float_rmat_transp_ratemult(struct FloatRates* rb, struct FloatRMat* m_b2a,
|
||||
struct FloatRates* ra);
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
@@ -429,24 +455,52 @@ extern void float_rmat_transp_ratemult(struct FloatRates* rb, struct FloatRMat*
|
||||
}
|
||||
|
||||
|
||||
/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
|
||||
/** Composition (multiplication) of two quaternions.
|
||||
* a2c = a2b comp b2c , aka a2c = a2b * b2c
|
||||
*/
|
||||
extern void float_quat_comp(struct FloatQuat* a2c, struct FloatQuat* a2b, struct FloatQuat* b2c);
|
||||
|
||||
/* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
|
||||
/** Composition (multiplication) of two quaternions.
|
||||
* a2b = a2c comp_inv b2c , aka a2b = a2c * inv(b2c)
|
||||
*/
|
||||
extern void float_quat_comp_inv(struct FloatQuat* a2b, struct FloatQuat* a2c, struct FloatQuat* b2c);
|
||||
|
||||
/* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
|
||||
/** Composition (multiplication) of two quaternions.
|
||||
* b2c = a2b inv_comp a2c , aka b2c = inv(_a2b) * a2c
|
||||
*/
|
||||
extern void float_quat_inv_comp(struct FloatQuat* b2c, struct FloatQuat* a2b, struct FloatQuat* a2c);
|
||||
|
||||
/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
|
||||
/** Composition (multiplication) of two quaternions with normalization.
|
||||
* a2c = a2b comp b2c , aka a2c = a2b * b2c
|
||||
*/
|
||||
extern void float_quat_comp_norm_shortest(struct FloatQuat* a2c, struct FloatQuat* a2b, struct FloatQuat* b2c);
|
||||
|
||||
/* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
|
||||
/** Composition (multiplication) of two quaternions with normalization.
|
||||
* a2b = a2c comp_inv b2c , aka a2b = a2c * inv(b2c)
|
||||
*/
|
||||
extern void float_quat_comp_inv_norm_shortest(struct FloatQuat* a2b, struct FloatQuat* a2c, struct FloatQuat* b2c);
|
||||
|
||||
/* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
|
||||
/** Composition (multiplication) of two quaternions with normalization.
|
||||
* b2c = a2b inv_comp a2c , aka b2c = inv(_a2b) * a2c
|
||||
*/
|
||||
extern void float_quat_inv_comp_norm_shortest(struct FloatQuat* b2c, struct FloatQuat* a2b, struct FloatQuat* a2c);
|
||||
|
||||
/** Quaternion derivative from rotational velocity.
|
||||
* qd = -0.5*omega(r) * q
|
||||
* or equally:
|
||||
* qd = 0.5 * q * omega(r)
|
||||
*/
|
||||
extern void float_quat_derivative(struct FloatQuat* qd, struct FloatRates* r, struct FloatQuat* q);
|
||||
|
||||
/** Quaternion derivative from rotational velocity with Lagrange multiplier.
|
||||
* qd = -0.5*omega(r) * q
|
||||
* or equally:
|
||||
* qd = 0.5 * q * omega(r)
|
||||
*/
|
||||
extern void float_quat_derivative_lagrange(struct FloatQuat* qd, struct FloatRates* r, struct FloatQuat* q);
|
||||
|
||||
/** Delta rotation quaternion with constant angular rates.
|
||||
*/
|
||||
extern void float_quat_differential(struct FloatQuat* q_out, struct FloatRates* w, float dt);
|
||||
|
||||
/** in place first order quaternion integration with constant rotational velocity */
|
||||
@@ -455,21 +509,23 @@ extern void float_quat_integrate_fi(struct FloatQuat* q, struct FloatRates* omeg
|
||||
/** in place quaternion integration with constant rotational velocity */
|
||||
extern void float_quat_integrate(struct FloatQuat* q, struct FloatRates* omega, float dt);
|
||||
|
||||
/** rotate 3D vector by quaternion.
|
||||
* vb = q_a2b * va * q_a2b^-1
|
||||
*/
|
||||
extern void float_quat_vmult(struct FloatVect3* v_out, struct FloatQuat* q, struct FloatVect3* v_in);
|
||||
|
||||
/** Quaternion derivative from rotational velocity.
|
||||
* qd = -0.5*omega(r) * q
|
||||
*/
|
||||
extern void float_quat_derivative(struct FloatQuat* qd, struct FloatRates* r, struct FloatQuat* q);
|
||||
|
||||
/** Quaternion derivative from rotational velocity.
|
||||
* qd = -0.5*omega(r) * q
|
||||
*/
|
||||
extern void float_quat_derivative_lagrange(struct FloatQuat* qd, struct FloatRates* r, struct FloatQuat* q);
|
||||
|
||||
/// Quaternion from Euler angles.
|
||||
extern void float_quat_of_eulers(struct FloatQuat* q, struct FloatEulers* e);
|
||||
|
||||
/// Quaternion from unit vector and angle.
|
||||
extern void float_quat_of_axis_angle(struct FloatQuat* q, const struct FloatVect3* uv, float angle);
|
||||
|
||||
/** Quaternion from orientation vector.
|
||||
* Length/norm of the vector is the angle.
|
||||
*/
|
||||
extern void float_quat_of_orientation_vect(struct FloatQuat* q, const struct FloatVect3* ov);
|
||||
|
||||
/// Quaternion from rotation matrix.
|
||||
extern void float_quat_of_rmat(struct FloatQuat* q, struct FloatRMat* rm);
|
||||
|
||||
|
||||
|
||||
@@ -250,7 +250,6 @@ void int32_rmat_of_eulers_312(struct Int32RMat* rm, struct Int32Eulers* e)
|
||||
*
|
||||
*/
|
||||
|
||||
/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
|
||||
void int32_quat_comp(struct Int32Quat* a2c, struct Int32Quat* a2b, struct Int32Quat* b2c)
|
||||
{
|
||||
a2c->qi = (a2b->qi * b2c->qi - a2b->qx * b2c->qx - a2b->qy * b2c->qy - a2b->qz * b2c->qz) >> INT32_QUAT_FRAC;
|
||||
@@ -259,7 +258,6 @@ void int32_quat_comp(struct Int32Quat* a2c, struct Int32Quat* a2b, struct Int32Q
|
||||
a2c->qz = (a2b->qi * b2c->qz + a2b->qx * b2c->qy - a2b->qy * b2c->qx + a2b->qz * b2c->qi) >> INT32_QUAT_FRAC;
|
||||
}
|
||||
|
||||
/* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
|
||||
void int32_quat_comp_inv(struct Int32Quat* a2b, struct Int32Quat* a2c, struct Int32Quat* b2c)
|
||||
{
|
||||
a2b->qi = (a2c->qi * b2c->qi + a2c->qx * b2c->qx + a2c->qy * b2c->qy + a2c->qz * b2c->qz) >> INT32_QUAT_FRAC;
|
||||
@@ -268,7 +266,6 @@ void int32_quat_comp_inv(struct Int32Quat* a2b, struct Int32Quat* a2c, struct In
|
||||
a2b->qz = (-a2c->qi * b2c->qz - a2c->qx * b2c->qy + a2c->qy * b2c->qx + a2c->qz * b2c->qi) >> INT32_QUAT_FRAC;
|
||||
}
|
||||
|
||||
/* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
|
||||
void int32_quat_inv_comp(struct Int32Quat* b2c, struct Int32Quat* a2b, struct Int32Quat* a2c)
|
||||
{
|
||||
b2c->qi = (a2b->qi * a2c->qi + a2b->qx * a2c->qx + a2b->qy * a2c->qy + a2b->qz * a2c->qz) >> INT32_QUAT_FRAC;
|
||||
@@ -277,7 +274,6 @@ void int32_quat_inv_comp(struct Int32Quat* b2c, struct Int32Quat* a2b, struct In
|
||||
b2c->qz = (a2b->qi * a2c->qz - a2b->qx * a2c->qy + a2b->qy * a2c->qx - a2b->qz * a2c->qi) >> INT32_QUAT_FRAC;
|
||||
}
|
||||
|
||||
/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
|
||||
void int32_quat_comp_norm_shortest(struct Int32Quat* a2c, struct Int32Quat* a2b, struct Int32Quat* b2c)
|
||||
{
|
||||
int32_quat_comp(a2c, a2b, b2c);
|
||||
@@ -285,7 +281,6 @@ void int32_quat_comp_norm_shortest(struct Int32Quat* a2c, struct Int32Quat* a2b,
|
||||
int32_quat_normalize(a2c);
|
||||
}
|
||||
|
||||
/* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
|
||||
void int32_quat_comp_inv_norm_shortest(struct Int32Quat* a2b, struct Int32Quat* a2c, struct Int32Quat* b2c)
|
||||
{
|
||||
int32_quat_comp_inv(a2b, a2c, b2c);
|
||||
@@ -293,7 +288,6 @@ void int32_quat_comp_inv_norm_shortest(struct Int32Quat* a2b, struct Int32Quat*
|
||||
int32_quat_normalize(a2b);
|
||||
}
|
||||
|
||||
/* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
|
||||
void int32_quat_inv_comp_norm_shortest(struct Int32Quat* b2c, struct Int32Quat* a2b, struct Int32Quat* a2c)
|
||||
{
|
||||
int32_quat_inv_comp(b2c, a2b, a2c);
|
||||
@@ -303,7 +297,9 @@ void int32_quat_inv_comp_norm_shortest(struct Int32Quat* b2c, struct Int32Quat*
|
||||
|
||||
/** Quaternion derivative from rotational velocity.
|
||||
* qd = -0.5*omega(r) * q
|
||||
* mult with 0.5 is done by shifting one more bit to the right
|
||||
* or equally:
|
||||
* qd = 0.5 * q * omega(r)
|
||||
* Multiplication with 0.5 is done by shifting one more bit to the right.
|
||||
*/
|
||||
void int32_quat_derivative(struct Int32Quat* qd, const struct Int32Rates* r, struct Int32Quat* q)
|
||||
{
|
||||
|
||||
@@ -234,16 +234,19 @@ extern int32_t int32_sqrt(int32_t in);
|
||||
/* macros also usable if _v is not a Int32Vect2, but a different struct with x,y members */
|
||||
#define INT32_VECT2_NORM(_v) int32_sqrt(VECT2_NORM2(_v))
|
||||
|
||||
/** return squared norm of 2D vector */
|
||||
static inline uint32_t int32_vect2_norm2(struct Int32Vect2* v)
|
||||
{
|
||||
return v->x * v->x + v->y * v->y;
|
||||
}
|
||||
|
||||
/** return norm of 2D vector */
|
||||
static inline uint32_t int32_vect2_norm(struct Int32Vect2* v)
|
||||
{
|
||||
return int32_sqrt(int32_vect2_norm2(v));
|
||||
}
|
||||
|
||||
/** normalize 2D vector inplace */
|
||||
static inline void int32_vect2_normalize(struct Int32Vect2* v, uint8_t frac)
|
||||
{
|
||||
const uint32_t f = BFP_OF_REAL((1.), frac);
|
||||
@@ -340,27 +343,52 @@ static inline void int32_rmat_identity(struct Int32RMat* rm)
|
||||
INT32_MAT33_DIAG(*rm, TRIG_BFP_OF_REAL(1.), TRIG_BFP_OF_REAL(1.), TRIG_BFP_OF_REAL(1.));
|
||||
}
|
||||
|
||||
/** Composition (multiplication) of two rotation matrices.
|
||||
* m_a2c = m_a2b comp m_b2c , aka m_a2c = m_b2c * m_a2b
|
||||
*/
|
||||
extern void int32_rmat_comp(struct Int32RMat* m_a2c, struct Int32RMat* m_a2b,
|
||||
struct Int32RMat* m_b2c);
|
||||
|
||||
/** Composition (multiplication) of two rotation matrices.
|
||||
* m_a2b = m_a2c comp_inv m_b2c , aka m_a2b = inv(_m_b2c) * m_a2c
|
||||
*/
|
||||
extern void int32_rmat_comp_inv(struct Int32RMat* m_a2b, struct Int32RMat* m_a2c,
|
||||
struct Int32RMat* m_b2c);
|
||||
|
||||
/** rotate 3D vector by rotation matrix.
|
||||
* vb = m_a2b * va
|
||||
*/
|
||||
extern void int32_rmat_vmult(struct Int32Vect3* vb, struct Int32RMat* m_a2b,
|
||||
struct Int32Vect3* va);
|
||||
|
||||
/** rotate 3D vector by transposed rotation matrix.
|
||||
* vb = m_b2a^T * va
|
||||
*/
|
||||
extern void int32_rmat_transp_vmult(struct Int32Vect3* vb, struct Int32RMat* m_b2a,
|
||||
struct Int32Vect3* va);
|
||||
|
||||
/** rotate anglular rates by rotation matrix.
|
||||
* rb = m_a2b * ra
|
||||
*/
|
||||
extern void int32_rmat_ratemult(struct Int32Rates* rb, struct Int32RMat* m_a2b,
|
||||
struct Int32Rates* ra);
|
||||
|
||||
/** rotate anglular rates by transposed rotation matrix.
|
||||
* rb = m_b2a^T * ra
|
||||
*/
|
||||
extern void int32_rmat_transp_ratemult(struct Int32Rates* rb, struct Int32RMat* m_b2a,
|
||||
struct Int32Rates* ra);
|
||||
|
||||
/// Convert unit quaternion to rotation matrix.
|
||||
extern void int32_rmat_of_quat(struct Int32RMat* rm, struct Int32Quat* q);
|
||||
|
||||
/// Rotation matrix from 321 Euler angles.
|
||||
extern void int32_rmat_of_eulers_321(struct Int32RMat* rm, struct Int32Eulers* e);
|
||||
|
||||
/// Rotation matrix from 312 Euler angles.
|
||||
extern void int32_rmat_of_eulers_312(struct Int32RMat* rm, struct Int32Eulers* e);
|
||||
|
||||
/// Rotation matrix from Euler angles.
|
||||
#define int32_rmat_of_eulers int32_rmat_of_eulers_321
|
||||
|
||||
/* defines for backwards compatibility */
|
||||
@@ -392,6 +420,8 @@ static inline void int32_quat_identity(struct Int32Quat* q)
|
||||
q->qz = 0;
|
||||
}
|
||||
|
||||
/** Norm of a quaternion.
|
||||
*/
|
||||
static inline int32_t int32_quat_norm(struct Int32Quat* q)
|
||||
{
|
||||
int32_t n2 = q->qi * q->qi + q->qx * q->qx + q->qy * q->qy + q->qz * q->qz;
|
||||
@@ -405,6 +435,7 @@ static inline void int32_quat_wrap_shortest(struct Int32Quat* q)
|
||||
}
|
||||
}
|
||||
|
||||
/** normalize a quaternion inplace */
|
||||
static inline void int32_quat_normalize(struct Int32Quat* q)
|
||||
{
|
||||
int32_t n = int32_quat_norm(q);
|
||||
@@ -416,36 +447,58 @@ static inline void int32_quat_normalize(struct Int32Quat* q)
|
||||
}
|
||||
}
|
||||
|
||||
/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
|
||||
/** Composition (multiplication) of two quaternions.
|
||||
* a2c = a2b comp b2c , aka a2c = a2b * b2c
|
||||
*/
|
||||
extern void int32_quat_comp(struct Int32Quat* a2c, struct Int32Quat* a2b, struct Int32Quat* b2c);
|
||||
|
||||
/* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
|
||||
/** Composition (multiplication) of two quaternions.
|
||||
* a2b = a2c comp_inv b2c , aka a2b = a2c * inv(b2c)
|
||||
*/
|
||||
extern void int32_quat_comp_inv(struct Int32Quat* a2b, struct Int32Quat* a2c, struct Int32Quat* b2c);
|
||||
|
||||
/* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
|
||||
/** Composition (multiplication) of two quaternions.
|
||||
* b2c = a2b inv_comp a2c , aka b2c = inv(_a2b) * a2c
|
||||
*/
|
||||
extern void int32_quat_inv_comp(struct Int32Quat* b2c, struct Int32Quat* a2b, struct Int32Quat* a2c);
|
||||
|
||||
/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
|
||||
/** Composition (multiplication) of two quaternions with normalization.
|
||||
* a2c = a2b comp b2c , aka a2c = a2b * b2c
|
||||
*/
|
||||
extern void int32_quat_comp_norm_shortest(struct Int32Quat* a2c, struct Int32Quat* a2b, struct Int32Quat* b2c);
|
||||
|
||||
/* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
|
||||
/** Composition (multiplication) of two quaternions with normalization.
|
||||
* a2b = a2c comp_inv b2c , aka a2b = a2c * inv(b2c)
|
||||
*/
|
||||
extern void int32_quat_comp_inv_norm_shortest(struct Int32Quat* a2b, struct Int32Quat* a2c, struct Int32Quat* b2c);
|
||||
|
||||
/* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
|
||||
/** Composition (multiplication) of two quaternions with normalization.
|
||||
* b2c = a2b inv_comp a2c , aka b2c = inv(_a2b) * a2c
|
||||
*/
|
||||
extern void int32_quat_inv_comp_norm_shortest(struct Int32Quat* b2c, struct Int32Quat* a2b, struct Int32Quat* a2c);
|
||||
|
||||
/** Quaternion derivative from rotational velocity.
|
||||
* qd = -0.5*omega(r) * q
|
||||
* mult with 0.5 is done by shifting one more bit to the right
|
||||
* or equally:
|
||||
* qd = 0.5 * q * omega(r)
|
||||
*/
|
||||
extern void int32_quat_derivative(struct Int32Quat* qd, const struct Int32Rates* r, struct Int32Quat* q);
|
||||
|
||||
/** in place quaternion first order integration with constant rotational velocity. */
|
||||
extern void int32_quat_integrate_fi(struct Int32Quat* q, struct Int64Quat* hr, struct Int32Rates* omega, int freq);
|
||||
|
||||
/** rotate 3D vector by quaternion.
|
||||
* vb = q_a2b * va * q_a2b^-1
|
||||
*/
|
||||
extern void int32_quat_vmult(struct Int32Vect3* v_out, struct Int32Quat* q, struct Int32Vect3* v_in);
|
||||
|
||||
/// Quaternion from Euler angles.
|
||||
extern void int32_quat_of_eulers(struct Int32Quat* q, struct Int32Eulers* e);
|
||||
|
||||
/// Quaternion from unit vector and angle.
|
||||
extern void int32_quat_of_axis_angle(struct Int32Quat* q, struct Int32Vect3* uv, int32_t angle);
|
||||
|
||||
/// Quaternion from rotation matrix.
|
||||
extern void int32_quat_of_rmat(struct Int32Quat* q, struct Int32RMat* r);
|
||||
|
||||
/* defines for backwards compatibility */
|
||||
|
||||
Reference in New Issue
Block a user