pprz_algebra_float.h File Reference

Paparazzi floating point algebra. More...

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

Include dependency graph for pprz_algebra_float.h:

This graph shows which files directly or indirectly include this file:

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)   float_vect3_integrate_fi(&(_vo), &(_dv), _dt)
#define	FLOAT_RATES_INTEGRATE_FI(_ra, _racc, _dt)   float_rates_integrate_fi(&(_ra), &(_racc), _dt)
#define	FLOAT_RATES_OF_EULER_DOT(_ra, _e, _ed)   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)   float_rmat_inv(&(_m_b2a), &(_m_a2b))
#define	FLOAT_RMAT_NORM(_m)   float_rmat_norm(&(_m))
#define	FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c)   float_rmat_comp(&(_m_a2c), &(_m_a2b), &(_m_b2c))
#define	FLOAT_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c)   float_rmat_comp_inv(&(_m_a2b), &(_m_a2c), &(_m_b2c))
#define	FLOAT_RMAT_VMULT(_vb, _m_a2b, _va)   float_rmat_vmult(&(_vb), &(_m_a2b), &(_va))
#define	FLOAT_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)   float_rmat_transp_vmult(&(_vb), &(_m_b2a), &(_va))
#define	FLOAT_RMAT_RATEMULT(_rb, _m_a2b, _ra)   float_rmat_ratemult(&(_rb), &(_m_a2b), &(_ra))
#define	FLOAT_RMAT_TRANSP_RATEMULT(_rb, _m_b2a, _ra)   float_rmat_ratemult(&(_rb), &(_m_b2a), &(_ra))
#define	FLOAT_RMAT_OF_AXIS_ANGLE(_rm, _uv, _an)   float_rmat_of_axis_angle(&(_rm), &(_uv), _an)
#define	FLOAT_RMAT_OF_EULERS(_rm, _e)   float_rmat_of_eulers_321(&(_rm), &(_e))
#define	FLOAT_RMAT_OF_EULERS_321(_rm, _e)   float_rmat_of_eulers_321(&(_rm), &(_e))
#define	FLOAT_RMAT_OF_EULERS_312(_rm, _e)   float_rmat_of_eulers_312(&(_rm), &(_e))
#define	FLOAT_RMAT_OF_QUAT(_rm, _q)   float_rmat_of_quat(&(_rm), &(_q))
#define	FLOAT_RMAT_INTEGRATE_FI(_rm, _omega, _dt)   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)   float_quat_identity(&(_q))
#define	FLOAT_QUAT_INVERT(_qo, _qi)   float_quat_invert(&(_qo), &(_qi))
#define	FLOAT_QUAT_WRAP_SHORTEST(_q)   float_quat_wrap_shortest(&(_q))
#define	FLOAT_QUAT_NORM(_q)   float_quat_norm(&(_q))
#define	FLOAT_QUAT_NORMALIZE(_q)   float_quat_normalize(&(_q))
#define	FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)   float_quat_comp(&(_a2c), &(_a2b), &(_b2c))
#define	FLOAT_QUAT_MULT(_a2c, _a2b, _b2c)   float_quat_comp(&(_a2c), &(_a2b), &(_b2c))
#define	FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c)   float_quat_inv_comp(&(_b2c), &(_a2b), &(_a2c))
#define	FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c)   float_quat_comp_inv(&(_a2b), &(_a2c), &(_b2c))
#define	FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)   float_quat_comp_norm_shortest(&(_a2c), &(_a2b), &(_b2c))
#define	FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c)   float_quat_comp_inv_norm_shortest(&(_a2b), &(_a2c), &(_b2c))
#define	FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c)   float_quat_inv_comp_norm_shortest(&(_b2c), &(_a2b), &(_a2c))
#define	FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt)   float_quat_differential(&(q_out), &(w), dt)
#define	FLOAT_QUAT_INTEGRATE(_q, _omega, _dt)   float_quat_integrate(&(_q), &(_omega), _dt)
#define	FLOAT_QUAT_VMULT(v_out, q, v_in)   float_quat_vmult(&(v_out), &(q), &(v_in))
#define	FLOAT_QUAT_DERIVATIVE(_qd, _r, _q)   float_quat_derivative(&(_qd), &(_r), &(_q))
#define	FLOAT_QUAT_DERIVATIVE_LAGRANGE(_qd, _r, _q)   float_quat_derivative_lagrange(&(_qd), &(_r), &(_q))
#define	FLOAT_QUAT_OF_EULERS(_q, _e)   float_quat_of_eulers(&(_q), &(_e))
#define	FLOAT_QUAT_OF_AXIS_ANGLE(_q, _uv, _an)   float_quat_of_axis_angle(&(_q), &(_uv), _an)
#define	FLOAT_QUAT_OF_ORIENTATION_VECT(_q, _ov)   float_quat_of_orientation_vect(&(_q), &(_ov))
#define	FLOAT_QUAT_OF_RMAT(_q, _r)   float_quat_of_rmat(&(_q), &(_r))
#define	FLOAT_EULERS_ZERO(_e)   EULERS_ASSIGN(_e, 0., 0., 0.);
#define	FLOAT_EULERS_OF_RMAT(_e, _rm)   float_eulers_of_rmat(&(_e), &(_rm))
#define	FLOAT_EULERS_OF_QUAT(_e, _q)   float_eulers_of_quat(&(_e), &(_q))
#define	FLOAT_EULERS_NORM(_e)   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.