Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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.