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:

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.

Functions
static float	float_log_n (const float v, const float n)

static float	float_vect2_norm2 (const struct FloatVect2 *v)

static float	float_vect2_norm (const struct FloatVect2 *v)

static void	float_vect2_normalize (struct FloatVect2 *v)
	normalize 2D vector in place

static float	float_vect3_norm2 (const struct FloatVect3 *v)

static float	float_vect3_norm (const struct FloatVect3 *v)

static void	float_vect3_normalize (struct FloatVect3 *v)
	normalize 3D vector in place

void	float_vect3_integrate_fi (struct FloatVect3 vec, const struct FloatVect3 dv, const float dt)
	in place first order integration of a 3D-vector

void	float_rates_integrate_fi (struct FloatRates r, const struct FloatRates dr, const float dt)
	in place first order integration of angular rates

void	float_rates_vect3_integrate_fi (struct FloatRates r, const struct FloatVect3 dr, const float dt)
	in place first order integration of angular rates

void	float_rates_of_euler_dot (struct FloatRates r, const struct FloatEulers e, const struct FloatEulers *edot)

static void	float_rmat_identity (struct FloatRMat *rm)
	initialises a rotation matrix to identity

void	float_rmat_inv (struct FloatRMat m_b2a, const struct FloatRMat m_a2b)
	Inverse/transpose of a rotation matrix.

void	float_rmat_comp (struct FloatRMat m_a2c, const struct FloatRMat m_a2b, const struct FloatRMat *m_b2c)
	Composition (multiplication) of two rotation matrices.

void	float_rmat_comp_inv (struct FloatRMat m_a2b, const struct FloatRMat m_a2c, const struct FloatRMat *m_b2c)
	Composition (multiplication) of two rotation matrices.

float	float_rmat_norm (const struct FloatRMat *rm)
	Norm of a rotation matrix.

void	float_rmat_vmult (struct FloatVect3 vb, const struct FloatRMat m_a2b, const struct FloatVect3 *va)
	rotate 3D vector by rotation matrix.

void	float_rmat_transp_vmult (struct FloatVect3 vb, const struct FloatRMat m_b2a, const struct FloatVect3 *va)
	rotate 3D vector by transposed rotation matrix.

void	float_rmat_mult (struct FloatEulers rb, const struct FloatRMat m_a2b, const struct FloatEulers *ra)
	rotate angle by rotation matrix.

void	float_rmat_transp_mult (struct FloatEulers rb, const struct FloatRMat m_b2a, const struct FloatEulers *ra)
	rotate angle by transposed rotation matrix.

void	float_rmat_ratemult (struct FloatRates rb, const struct FloatRMat m_a2b, const struct FloatRates *ra)
	rotate anglular rates by rotation matrix.

void	float_rmat_transp_ratemult (struct FloatRates rb, const struct FloatRMat m_b2a, const struct FloatRates *ra)
	rotate anglular rates by transposed rotation matrix.

void	float_rmat_of_axis_angle (struct FloatRMat rm, const struct FloatVect3 uv, const float angle)
	initialises a rotation matrix from unit vector axis and angle

void	float_rmat_of_eulers_321 (struct FloatRMat rm, const struct FloatEulers e)
	Rotation matrix from 321 Euler angles (float).

void	float_rmat_of_eulers_312 (struct FloatRMat rm, const struct FloatEulers e)

void	float_rmat_of_quat (struct FloatRMat rm, const struct FloatQuat q)

void	float_rmat_integrate_fi (struct FloatRMat rm, const struct FloatRates omega, const float dt)
	in place first order integration of a rotation matrix

float	float_rmat_reorthogonalize (struct FloatRMat *rm)

static void	float_quat_identity (struct FloatQuat *q)
	initialises a quaternion to identity

static float	float_quat_norm (const struct FloatQuat *q)

static void	float_quat_normalize (struct FloatQuat *q)

static void	float_quat_invert (struct FloatQuat qo, const struct FloatQuat qi)

static void	float_quat_wrap_shortest (struct FloatQuat *q)

void	float_quat_comp (struct FloatQuat a2c, const struct FloatQuat a2b, const struct FloatQuat *b2c)
	Composition (multiplication) of two quaternions.

void	float_quat_comp_inv (struct FloatQuat a2b, const struct FloatQuat a2c, const struct FloatQuat *b2c)
	Composition (multiplication) of two quaternions.

void	float_quat_inv_comp (struct FloatQuat b2c, const struct FloatQuat a2b, const struct FloatQuat *a2c)
	Composition (multiplication) of two quaternions.

void	float_quat_comp_norm_shortest (struct FloatQuat a2c, const struct FloatQuat a2b, const struct FloatQuat *b2c)
	Composition (multiplication) of two quaternions with normalization.

void	float_quat_comp_inv_norm_shortest (struct FloatQuat a2b, const struct FloatQuat a2c, const struct FloatQuat *b2c)
	Composition (multiplication) of two quaternions with normalization.

void	float_quat_inv_comp_norm_shortest (struct FloatQuat b2c, const struct FloatQuat a2b, const struct FloatQuat *a2c)
	Composition (multiplication) of two quaternions with normalization.

void	float_quat_derivative (struct FloatQuat qd, const struct FloatRates r, const struct FloatQuat *q)
	Quaternion derivative from rotational velocity.

void	float_quat_derivative_lagrange (struct FloatQuat qd, const struct FloatRates r, const struct FloatQuat *q)
	Quaternion derivative from rotational velocity with Lagrange multiplier.

void	float_quat_differential (struct FloatQuat q_out, const struct FloatRates w, const float dt)
	Delta rotation quaternion with constant angular rates.

void	float_quat_integrate_fi (struct FloatQuat q, const struct FloatRates omega, const float dt)
	in place first order quaternion integration with constant rotational velocity

void	float_quat_integrate (struct FloatQuat q, const struct FloatRates omega, const float dt)
	in place quaternion integration with constant rotational velocity

void	float_quat_vmult (struct FloatVect3 v_out, const struct FloatQuat q, const struct FloatVect3 *v_in)
	rotate 3D vector by quaternion.

void	float_quat_of_eulers (struct FloatQuat q, const struct FloatEulers e)
	Quaternion from Euler angles.

void	float_quat_of_eulers_zxy (struct FloatQuat q, const struct FloatEulers e)
	quat from euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch

void	float_quat_of_eulers_yxz (struct FloatQuat q, const struct FloatEulers e)
	quat from euler rotation 'YXZ' This function calculates a quaternion from Euler angles with the order YXZ, so pitch, roll, yaw, instead of the conventional ZYX order.

void	float_quat_of_axis_angle (struct FloatQuat q, const struct FloatVect3 uv, const float angle)
	Quaternion from unit vector and angle.

void	float_quat_of_orientation_vect (struct FloatQuat q, const struct FloatVect3 ov)
	Quaternion from orientation vector.

void	float_quat_of_rmat (struct FloatQuat q, const struct FloatRMat rm)
	Quaternion from rotation matrix.

void	float_quat_tilt_twist (struct FloatQuat tilt, struct FloatQuat twist, const struct FloatQuat *quat)
	Tilt twist decomposition of quaternion.

static float	float_eulers_norm (const struct FloatEulers *e)

void	float_eulers_of_rmat (struct FloatEulers e, const struct FloatRMat rm)

void	float_eulers_of_quat (struct FloatEulers e, const struct FloatQuat q)
	euler rotation 'ZYX'

void	float_eulers_of_quat_zxy (struct FloatEulers e, const struct FloatQuat q)
	euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch

void	float_eulers_of_quat_yxz (struct FloatEulers e, const struct FloatQuat q)
	euler rotation 'YXZ' This function calculates from a quaternion the Euler angles with the order YXZ, so pitch, roll, yaw, instead of the conventional ZYX order.

static void	float_vect_zero (float *a, const int n)
	a = 0

static void	float_vect_copy (float a, const float b, const int n)
	a = b

static void	float_vect_sum (float o, const float a, const float *b, const int n)
	o = a + b

static void	float_vect_diff (float o, const float a, const float *b, const int n)
	o = a - b

static void	float_vect_mul (float o, const float a, const float *b, const int n)
	o = a * b (element wise)

static void	float_vect_add (float a, const float b, const int n)
	a += b

static void	float_vect_sub (float a, const float b, const int n)
	a -= b

static void	float_vect_smul (float o, const float a, const float s, const int n)
	o = a * s

static void	float_vect_sdiv (float o, const float a, const float s, const int n)
	o = a / s

static float	float_vect_norm (const float *a, const int n)
	\|\|a\|\|

static void	float_vect_scale (float *a, const float s, const int n)
	a *= s

static float	float_vect_dot_product (const float a, const float b, const int n)
	a.b

void	float_mat_invert (float o, float mat, const int n)
	Calculate inverse of any n x n matrix (passed as C array) o = mat^-1 Algorithm verified with Matlab.

void	float_mat_exp (float a, float o, const int n)

float	float_mat_norm_li (float **o, const int m, const int n)

static void	float_mat_zero (float **a, const int m, const int n)
	a = 0

static void	float_mat_copy (float a, float b, const int m, const int n)
	a = b

static void	float_mat_sum (float o, float a, float **b, const int m, const int n)
	o = a + b

static void	float_mat_diff (float o, float a, float **b, const int m, const int n)
	o = a - b

static void	float_mat_transpose_square (float **a, const int n)
	transpose square matrix

static void	float_mat_transpose (float o, float a, const int n, const int m)
	transpose non-square matrix

static void	float_mat_mul (float o, float a, float **b, const int m, const int n, const int l)
	o = a * b

static void	float_mat_mul_transpose (float o, float a, float **b, const int m, const int n, const int l)
	o = a * b'

static void	float_mat_mul_copy (float o, float a, float **b, const int m, const int n, const int l)
	o = a * b

static void	float_mat_vect_mul (float o, float a, const float b, const int m, const int n)
	o = a * b

static void	float_mat_scale (float **a, float k, const int m, const int n)
	a *= k, where k is a scalar value

static void	float_mat_sum_scaled (float a, float b, const float k, const int m, const int n)
	a += k*b, where k is a scalar value

static void	float_mat_minor (float o, float a, const int m, const int n, const int d)
	matrix minor

static void	float_mat_vmul (float *o, const float v, const int n)
	o = I - v v^T

static void	float_mat_col (float o, float *a, const int m, const int c)
	o = c-th column of matrix a[m x n]

static void	float_mat_diagonal_scal (float **o, float v, const int n)
	Make an n x n identity matrix (for matrix passed as array)

static void	float_mat_div_scalar (float o, float a, const float scalar, const int m, const int n)
	Divide a matrix by a scalar.

static void	float_mat_mul_scalar (float o, float a, const float scalar, const int m, const int n)
	Multiply a matrix by a scalar.

bool	float_mat_inv_2d (float inv_out[4], const float mat_in[4])
	2x2 matrix inverse

void	float_mat2_mult (struct FloatVect2 *vect_out, const float mat[4], const struct FloatVect2 vect_in)
	Multiply 2D matrix with vector.

bool	float_mat_inv_3d (float inv_out[3][3], const float mat_in[3][3])
	3x3 matrix inverse

void	float_mat3_mult (struct FloatVect3 *vect_out, const float mat[3][3], const struct FloatVect3 vect_in)
	Multiply 3D matrix with vector.

bool	float_mat_inv_4d (float invOut[4][4], const float mat_in[4][4])
	4x4 Matrix inverse

void	float_vect3_bound_in_2d (struct FloatVect3 *vect3, const float bound)

void	float_vect3_bound_in_3d (struct FloatVect3 *vect3, const float bound)

void	float_vect3_scale_in_2d (struct FloatVect3 *vect3, const float norm_des)

void	float_vect2_bound_in_2d (struct FloatVect2 *vect2, const float bound)

void	float_vect2_scale_in_2d (struct FloatVect2 *vect2, const float norm_des)

Detailed Description

Paparazzi floating point algebra.

Definition in file pprz_algebra_float.h.

Data Structures

Macros

Functions

Detailed Description