[math] some dox updates

This commit is contained in:
Felix Ruess
2014-09-12 17:58:53 +02:00
parent 8d6d84376d
commit a3e860ccb7
4 changed files with 151 additions and 53 deletions
+3 -10
View File
@@ -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);
+85 -29
View File
@@ -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);
+3 -7
View File
@@ -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)
{
+60 -7
View File
@@ -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 */