pprz_algebra_float.h File Reference

Paparazzi floating point algebra. More...

#include "pprz_algebra.h"
#include "message_pragmas.h"
#include <math.h>
#include <float.h>

Include dependency graph for pprz_algebra_float.h:

Go to the source code of this file.

Data Structures
struct	FloatVect2
struct	FloatVect3
struct	FloatQuat
	Roation quaternion. More...
struct	FloatMat33
struct	FloatRMat
	rotation matrix More...
struct	FloatEulers
	euler angles More...
struct	FloatRates
	angular rates More...

Macros
#define	M_SQRT2   1.41421356237309504880
#define	FLOAT_ANGLE_NORMALIZE(_a)
#define	FLOAT_VECT2_ZERO(_v)   VECT2_ASSIGN(_v, 0., 0.)
#define	FLOAT_VECT2_NORM(_v)   sqrtf(VECT2_NORM2(_v))
#define	FLOAT_VECT2_NORMALIZE(_v)   float_vect2_normalize(&(_v))
#define	FLOAT_VECT3_ZERO(_v)   VECT3_ASSIGN(_v, 0., 0., 0.)
#define	FLOAT_VECT3_NORM(_v)   sqrtf(VECT3_NORM2(_v))
#define	FLOAT_VECT3_NORMALIZE(_v)   float_vect3_normalize(&(_v))
#define	FLOAT_RATES_ZERO(_r)
#define	FLOAT_RATES_NORM(_v)   (sqrtf((_v).p*(_v).p + (_v).q*(_v).q + (_v).r*(_v).r))
#define	FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2)
#define	FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt)   WARNING("FLOAT_VECT3_INTEGRATE_FI macro is deprecated, use the lower case function instead") float_vect3_integrate_fi(&(_vo), &(_dv), _dt)
#define	FLOAT_RATES_INTEGRATE_FI(_ra, _racc, _dt)   WARNING("FLOAT_RATES_INTEGRATE_FI macro is deprecated, use the lower case function instead") float_rates_integrate_fi(&(_ra), &(_racc), _dt)
#define	FLOAT_RATES_OF_EULER_DOT(_ra, _e, _ed)   WARNING("FLOAT_RATES_OF_EULER_DOT macro is deprecated, use the lower case function instead") float_rates_of_euler_dot(&(_ra), &(_e), &(_ed))
#define	FLOAT_MAT33_ZERO(_m)
#define	FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22)
#define	float_rmat_of_eulers   float_rmat_of_eulers_321
#define	FLOAT_RMAT_INV(_m_b2a, _m_a2b)   WARNING("FLOAT_RMAT_INV macro is deprecated, use the lower case function instead") float_rmat_inv(&(_m_b2a), &(_m_a2b))
#define	FLOAT_RMAT_NORM(_m)   WARNING("FLOAT_RMAT_NORM macro is deprecated, use the lower case function instead") float_rmat_norm(&(_m))
#define	FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c)   WARNING("FLOAT_RMAT_COMP macro is deprecated, use the lower case function instead") float_rmat_comp(&(_m_a2c), &(_m_a2b), &(_m_b2c))
#define	FLOAT_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c)   WARNING("FLOAT_RMAT_COMP_INV macro is deprecated, use the lower case function instead") float_rmat_comp_inv(&(_m_a2b), &(_m_a2c), &(_m_b2c))
#define	FLOAT_RMAT_VMULT(_vb, _m_a2b, _va)   WARNING("FLOAT_RMAT_VMULT macro is deprecated, use the lower case function instead") float_rmat_vmult(&(_vb), &(_m_a2b), &(_va))
#define	FLOAT_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)   WARNING("FLOAT_RMAT_TRANSP_VMULT macro is deprecated, use the lower case function instead") float_rmat_transp_vmult(&(_vb), &(_m_b2a), &(_va))
#define	FLOAT_RMAT_RATEMULT(_rb, _m_a2b, _ra)   WARNING("FLOAT_RMAT_RATEMULT macro is deprecated, use the lower case function instead") float_rmat_ratemult(&(_rb), &(_m_a2b), &(_ra))
#define	FLOAT_RMAT_TRANSP_RATEMULT(_rb, _m_b2a, _ra)   WARNING("FLOAT_RMAT_TRANSP_RATEMULT macro is deprecated, use the lower case function instead") float_rmat_ratemult(&(_rb), &(_m_b2a), &(_ra))
#define	FLOAT_RMAT_OF_AXIS_ANGLE(_rm, _uv, _an)   WARNING("FLOAT_RMAT_OF_AXIS_ANGLE macro is deprecated, use the lower case function instead") float_rmat_of_axis_angle(&(_rm), &(_uv), _an)
#define	FLOAT_RMAT_OF_EULERS(_rm, _e)   WARNING("FLOAT_RMAT_OF_EULERS macro is deprecated, use the lower case function instead") float_rmat_of_eulers_321(&(_rm), &(_e))
#define	FLOAT_RMAT_OF_EULERS_321(_rm, _e)   WARNING("FLOAT_RMAT_OF_EULERS_321 macro is deprecated, use the lower case function instead") float_rmat_of_eulers_321(&(_rm), &(_e))
#define	FLOAT_RMAT_OF_EULERS_312(_rm, _e)   WARNING("FLOAT_RMAT_OF_EULERS_312 macro is deprecated, use the lower case function instead") float_rmat_of_eulers_312(&(_rm), &(_e))
#define	FLOAT_RMAT_OF_QUAT(_rm, _q)   WARNING("FLOAT_RMAT_OF_QUAT macro is deprecated, use the lower case function instead") float_rmat_of_quat(&(_rm), &(_q))
#define	FLOAT_RMAT_INTEGRATE_FI(_rm, _omega, _dt)   WARNING("FLOAT_RMAT_INTEGRATE_FI macro is deprecated, use the lower case function instead") float_rmat_integrate_fi(&(_rm), &(_omega), &(_dt))
#define	FLOAT_QUAT_NORM2(_q)   (SQUARE((_q).qi) + SQUARE((_q).qx) + SQUARE((_q).qy) + SQUARE((_q).qz))
#define	FLOAT_QUAT_EXTRACT(_vo, _qi)   QUAT_EXTRACT_Q(_vo, _qi)
#define	FLOAT_QUAT_ZERO(_q)   WARNING("FLOAT_QUAT_ZERO macro is deprecated, use the lower case function instead") float_quat_identity(&(_q))
#define	FLOAT_QUAT_INVERT(_qo, _qi)   WARNING("FLOAT_QUAT_INVERT macro is deprecated, use the lower case function instead") float_quat_invert(&(_qo), &(_qi))
#define	FLOAT_QUAT_WRAP_SHORTEST(_q)   WARNING("FLOAT_QUAT_WRAP_SHORTEST macro is deprecated, use the lower case function instead") float_quat_wrap_shortest(&(_q))
#define	FLOAT_QUAT_NORM(_q)   WARNING("FLOAT_QUAT_NORM macro is deprecated, use the lower case function instead") float_quat_norm(&(_q))
#define	FLOAT_QUAT_NORMALIZE(_q)   WARNING("FLOAT_QUAT_NORMALIZE macro is deprecated, use the lower case function instead") float_quat_normalize(&(_q))
#define	FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)   WARNING("FLOAT_QUAT_COMP macro is deprecated, use the lower case function instead") float_quat_comp(&(_a2c), &(_a2b), &(_b2c))
#define	FLOAT_QUAT_MULT(_a2c, _a2b, _b2c)   WARNING("FLOAT_QUAT_MULT macro is deprecated, use the lower case function instead") float_quat_comp(&(_a2c), &(_a2b), &(_b2c))
#define	FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c)   WARNING("FLOAT_QUAT_INV_COMP macro is deprecated, use the lower case function instead") float_quat_inv_comp(&(_b2c), &(_a2b), &(_a2c))
#define	FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c)   WARNING("FLOAT_QUAT_COMP_INV macro is deprecated, use the lower case function instead") float_quat_comp_inv(&(_a2b), &(_a2c), &(_b2c))
#define	FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)   WARNING("FLOAT_QUAT_COMP_NORM_SHORTEST macro is deprecated, use the lower case function instead") float_quat_comp_norm_shortest(&(_a2c), &(_a2b), &(_b2c))
#define	FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c)   WARNING("FLOAT_QUAT_COMP_INV_NORM_SHORTEST macro is deprecated, use the lower case function instead") float_quat_comp_inv_norm_shortest(&(_a2b), &(_a2c), &(_b2c))
#define	FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c)   WARNING("FLOAT_QUAT_INV_COMP_NORM_SHORTEST macro is deprecated, use the lower case function instead") float_quat_inv_comp_norm_shortest(&(_b2c), &(_a2b), &(_a2c))
#define	FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt)   WARNING("FLOAT_QUAT_DIFFERENTIAL macro is deprecated, use the lower case function instead") float_quat_differential(&(q_out), &(w), dt)
#define	FLOAT_QUAT_INTEGRATE(_q, _omega, _dt)   WARNING("FLOAT_QUAT_INTEGRATE macro is deprecated, use the lower case function instead") float_quat_integrate(&(_q), &(_omega), _dt)
#define	FLOAT_QUAT_VMULT(v_out, q, v_in)   WARNING("FLOAT_QUAT_VMULT macro is deprecated, use the lower case function instead") float_quat_vmult(&(v_out), &(q), &(v_in))
#define	FLOAT_QUAT_DERIVATIVE(_qd, _r, _q)   WARNING("FLOAT_QUAT_DERIVATIVE macro is deprecated, use the lower case function instead") float_quat_derivative(&(_qd), &(_r), &(_q))
#define	FLOAT_QUAT_DERIVATIVE_LAGRANGE(_qd, _r, _q)   WARNING("FLOAT_QUAT_DERIVATIVE_LAGRANGE macro is deprecated, use the lower case function instead") float_quat_derivative_lagrange(&(_qd), &(_r), &(_q))
#define	FLOAT_QUAT_OF_EULERS(_q, _e)   WARNING("FLOAT_QUAT_OF_EULERS macro is deprecated, use the lower case function instead") float_quat_of_eulers(&(_q), &(_e))
#define	FLOAT_QUAT_OF_AXIS_ANGLE(_q, _uv, _an)   WARNING("FLOAT_QUAT_OF_AXIS_ANGLE macro is deprecated, use the lower case function instead") float_quat_of_axis_angle(&(_q), &(_uv), _an)
#define	FLOAT_QUAT_OF_ORIENTATION_VECT(_q, _ov)   WARNING("FLOAT_QUAT_OF_ORIENTATION_VECT macro is deprecated, use the lower case function instead") float_quat_of_orientation_vect(&(_q), &(_ov))
#define	FLOAT_QUAT_OF_RMAT(_q, _r)   WARNING("FLOAT_QUAT_OF_RMAT macro is deprecated, use the lower case function instead") float_quat_of_rmat(&(_q), &(_r))
#define	FLOAT_EULERS_ZERO(_e)   EULERS_ASSIGN(_e, 0., 0., 0.);
#define	FLOAT_EULERS_OF_RMAT(_e, _rm)   WARNING("FLOAT_EULERS_OF_RMAT macro is deprecated, use the lower case function instead") float_eulers_of_rmat(&(_e), &(_rm))
#define	FLOAT_EULERS_OF_QUAT(_e, _q)   WARNING("FLOAT_EULERS_OF_QUAT macro is deprecated, use the lower case function instead") float_eulers_of_quat(&(_e), &(_q))
#define	FLOAT_EULERS_NORM(_e)   WARNING("FLOAT_EULERS_NORM macro is deprecated, use the lower case function instead") float_eulers_norm(&(_e))
#define	MAKE_MATRIX_PTR(_ptr, _mat, _rows)
	Make a pointer to a matrix of _rows lines. More...

Functions
static float	float_vect2_norm2 (struct FloatVect2 *v)
static float	float_vect2_norm (struct FloatVect2 *v)
static void	float_vect2_normalize (struct FloatVect2 *v)
	normalize 2D vector in place More...
static float	float_vect3_norm2 (struct FloatVect3 *v)
static float	float_vect3_norm (struct FloatVect3 *v)
static void	float_vect3_normalize (struct FloatVect3 *v)
	normalize 3D vector in place More...
void	float_vect3_integrate_fi (struct FloatVect3 *vec, struct FloatVect3 *dv, float dt)
	in place first order integration of a 3D-vector More...
void	float_rates_integrate_fi (struct FloatRates *r, struct FloatRates *dr, float dt)
	in place first order integration of angular rates More...
void	float_rates_of_euler_dot (struct FloatRates *r, struct FloatEulers *e, struct FloatEulers *edot)
static void	float_rmat_identity (struct FloatRMat *rm)
	initialises a rotation matrix to identity More...
void	float_rmat_inv (struct FloatRMat *m_b2a, struct FloatRMat *m_a2b)
	Inverse/transpose of a rotation matrix. More...
void	float_rmat_comp (struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c)
	Composition (multiplication) of two rotation matrices. More...
void	float_rmat_comp_inv (struct FloatRMat *m_a2b, struct FloatRMat *m_a2c, struct FloatRMat *m_b2c)
	Composition (multiplication) of two rotation matrices. More...
float	float_rmat_norm (struct FloatRMat *rm)
	Norm of a rotation matrix. More...
void	float_rmat_vmult (struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
	rotate 3D vector by rotation matrix. More...
void	float_rmat_transp_vmult (struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
	rotate 3D vector by transposed rotation matrix. More...
void	float_rmat_ratemult (struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
	rotate anglular rates by rotation matrix. More...
void	float_rmat_transp_ratemult (struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
	rotate anglular rates by transposed rotation matrix. More...
void	float_rmat_of_axis_angle (struct FloatRMat *rm, struct FloatVect3 *uv, float angle)
	initialises a rotation matrix from unit vector axis and angle More...
void	float_rmat_of_eulers_321 (struct FloatRMat *rm, struct FloatEulers *e)
	Rotation matrix from 321 Euler angles (float). More...
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_integrate_fi (struct FloatRMat *rm, struct FloatRates *omega, float dt)
	in place first order integration of a rotation matrix More...
float	float_rmat_reorthogonalize (struct FloatRMat *rm)
static void	float_quat_identity (struct FloatQuat *q)
	initialises a quaternion to identity More...
static float	float_quat_norm (struct FloatQuat *q)
static void	float_quat_normalize (struct FloatQuat *q)
static void	float_quat_invert (struct FloatQuat *qo, struct FloatQuat *qi)
static void	float_quat_wrap_shortest (struct FloatQuat *q)
void	float_quat_comp (struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
	Composition (multiplication) of two quaternions. More...
void	float_quat_comp_inv (struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
	Composition (multiplication) of two quaternions. More...
void	float_quat_inv_comp (struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
	Composition (multiplication) of two quaternions. More...
void	float_quat_comp_norm_shortest (struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
	Composition (multiplication) of two quaternions with normalization. More...
void	float_quat_comp_inv_norm_shortest (struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
	Composition (multiplication) of two quaternions with normalization. More...
void	float_quat_inv_comp_norm_shortest (struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
	Composition (multiplication) of two quaternions with normalization. More...
void	float_quat_derivative (struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
	Quaternion derivative from rotational velocity. More...
void	float_quat_derivative_lagrange (struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
	Quaternion derivative from rotational velocity with Lagrange multiplier. More...
void	float_quat_differential (struct FloatQuat *q_out, struct FloatRates *w, float dt)
	Delta rotation quaternion with constant angular rates. More...
void	float_quat_integrate_fi (struct FloatQuat *q, struct FloatRates *omega, float dt)
	in place first order quaternion integration with constant rotational velocity More...
void	float_quat_integrate (struct FloatQuat *q, struct FloatRates *omega, float dt)
	in place quaternion integration with constant rotational velocity More...
void	float_quat_vmult (struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
	rotate 3D vector by quaternion. More...
void	float_quat_of_eulers (struct FloatQuat *q, struct FloatEulers *e)
	Quaternion from Euler angles. More...
void	float_quat_of_axis_angle (struct FloatQuat *q, const struct FloatVect3 *uv, float angle)
	Quaternion from unit vector and angle. More...
void	float_quat_of_orientation_vect (struct FloatQuat *q, const struct FloatVect3 *ov)
	Quaternion from orientation vector. More...
void	float_quat_of_rmat (struct FloatQuat *q, struct FloatRMat *rm)
	Quaternion from rotation matrix. More...
static float	float_eulers_norm (struct FloatEulers *e)
void	float_eulers_of_rmat (struct FloatEulers *e, struct FloatRMat *rm)
void	float_eulers_of_quat (struct FloatEulers *e, struct FloatQuat *q)
static void	float_vect_zero (float *a, const int n)
	a = 0 More...
static void	float_vect_copy (float *a, const float *b, const int n)
	a = b More...
static void	float_vect_sum (float *o, const float *a, const float *b, const int n)
	o = a + b More...
static void	float_vect_diff (float *o, const float *a, const float *b, const int n)
	o = a - b More...
static void	float_vect_mul (float *o, const float *a, const float *b, const int n)
	o = a * b (element wise) More...
static void	float_vect_add (float *a, const float *b, const int n)
	a += b More...
static void	float_vect_sub (float *a, const float *b, const int n)
	a -= b More...
static void	float_vect_smul (float *o, const float *a, const float s, const int n)
	o = a * s More...
static void	float_vect_sdiv (float *o, const float *a, const float s, const int n)
	o = a / s More...
static float	float_vect_norm (const float *a, const int n)
	||a|| More...
static void	float_mat_zero (float **a, int m, int n)
	a = 0 More...
static void	float_mat_copy (float **a, float **b, int m, int n)
	a = b More...
static void	float_mat_sum (float **o, float **a, float **b, int m, int n)
	o = a + b More...
static void	float_mat_diff (float **o, float **a, float **b, int m, int n)
	o = a - b More...
static void	float_mat_transpose (float **a, int n)
	transpose square matrix More...
static void	float_mat_mul (float **o, float **a, float **b, int m, int n, int l)
	o = a * b More...
static void	float_mat_minor (float **o, float **a, int m, int n, int d)
	matrix minor More...
static void	float_mat_vmul (float **o, float *v, int n)
	o = I - v v^T More...
static void	float_mat_col (float *o, float **a, int m, int c)
	o = c-th column of matrix a[m x n] More...

Detailed Description

Paparazzi floating point algebra.

Definition in file pprz_algebra_float.h.