Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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_log_n (float v, float n)
 
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_mult (struct FloatEulers *rb, struct FloatRMat *m_a2b, struct FloatEulers *ra)
 rotate angle by rotation matrix. More...
 
void float_rmat_transp_mult (struct FloatEulers *rb, struct FloatRMat *m_b2a, struct FloatEulers *ra)
 rotate angle 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_eulers_zxy (struct FloatQuat *q, struct FloatEulers *e)
 quat from euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch More...
 
void float_quat_of_eulers_yxz (struct FloatQuat *q, 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. 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)
 euler rotation 'ZYX' More...
 
void float_eulers_of_quat_zxy (struct FloatEulers *e, struct FloatQuat *q)
 euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch More...
 
void float_eulers_of_quat_yxz (struct FloatEulers *e, 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. More...
 
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_vect_scale (float *a, const float s, const int n)
 a *= s More...
 
static float float_vect_dot_product (const float *a, const float *b, const int n)
 a.b More...
 
void float_mat_invert (float **o, float **mat, int n)
 Calculate inverse of any n x n matrix (passed as C array) o = mat^-1 Algorithm verified with Matlab. More...
 
void float_mat_exp (float **a, float **o, int n)
 
float float_mat_norm_li (float **o, int m, int n)
 
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_square (float **a, int n)
 transpose square matrix More...
 
static void float_mat_transpose (float **o, float **a, int n, int m)
 transpose non-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_mul_copy (float **o, float **a, float **b, int m, int n, int l)
 o = a * b More...
 
static void float_mat_vect_mul (float *o, float **a, float *b, int m, int n)
 o = a * b More...
 
static void float_mat_scale (float **a, float k, int m, int n)
 a *= k, where k is a scalar value More...
 
static void float_mat_sum_scaled (float **a, float **b, float k, int m, int n)
 a += k*b, where k is a scalar value 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...
 
static void float_mat_diagonal_scal (float **o, float v, int n)
 Make an n x n identity matrix (for matrix passed as array) More...
 
bool float_mat_inv_2d (float inv_out[4], float mat_in[4])
 2x2 matrix inverse More...
 
void float_mat2_mult (struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in)
 Multiply 2D matrix with vector. More...
 
bool float_mat_inv_4d (float invOut[16], float mat_in[16])
 4x4 Matrix inverse More...
 
void vect_bound_in_2d (struct FloatVect3 *vect3, float bound)
 
void vect_scale (struct FloatVect3 *vect3, float norm_des)
 

Detailed Description

Paparazzi floating point algebra.

Definition in file pprz_algebra_float.h.