Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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)
#define FLOAT_VECT2_NORM(_v)
#define FLOAT_VECT2_NORMALIZE(_v)
#define FLOAT_VECT3_ZERO(_v)
#define FLOAT_VECT3_NORM(_v)
#define FLOAT_VECT3_NORMALIZE(_v)
#define FLOAT_RATES_ZERO(_r)
#define FLOAT_RATES_NORM(_v)
#define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2)
#define FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt)
#define FLOAT_RATES_INTEGRATE_FI(_ra, _racc, _dt)
#define 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)
#define FLOAT_RMAT_NORM(_m)
#define FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c)
#define FLOAT_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c)
#define FLOAT_RMAT_VMULT(_vb, _m_a2b, _va)
#define FLOAT_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
#define FLOAT_RMAT_RATEMULT(_rb, _m_a2b, _ra)
#define FLOAT_RMAT_TRANSP_RATEMULT(_rb, _m_b2a, _ra)
#define FLOAT_RMAT_OF_AXIS_ANGLE(_rm, _uv, _an)
#define FLOAT_RMAT_OF_EULERS(_rm, _e)
#define FLOAT_RMAT_OF_EULERS_321(_rm, _e)
#define FLOAT_RMAT_OF_EULERS_312(_rm, _e)
#define FLOAT_RMAT_OF_QUAT(_rm, _q)
#define FLOAT_RMAT_INTEGRATE_FI(_rm, _omega, _dt)
#define FLOAT_QUAT_NORM2(_q)
#define FLOAT_QUAT_EXTRACT(_vo, _qi)
#define FLOAT_QUAT_ZERO(_q)
#define FLOAT_QUAT_INVERT(_qo, _qi)
#define FLOAT_QUAT_WRAP_SHORTEST(_q)
#define FLOAT_QUAT_NORM(_q)
#define FLOAT_QUAT_NORMALIZE(_q)
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
#define FLOAT_QUAT_MULT(_a2c, _a2b, _b2c)
#define FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c)
#define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c)
#define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)
#define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c)
#define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c)
#define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt)
#define FLOAT_QUAT_INTEGRATE(_q, _omega, _dt)
#define FLOAT_QUAT_VMULT(v_out, q, v_in)
#define FLOAT_QUAT_DERIVATIVE(_qd, _r, _q)
#define FLOAT_QUAT_DERIVATIVE_LAGRANGE(_qd, _r, _q)
#define FLOAT_QUAT_OF_EULERS(_q, _e)
#define FLOAT_QUAT_OF_AXIS_ANGLE(_q, _uv, _an)
#define FLOAT_QUAT_OF_ORIENTATION_VECT(_q, _ov)
#define FLOAT_QUAT_OF_RMAT(_q, _r)
#define FLOAT_EULERS_ZERO(_e)
#define FLOAT_EULERS_OF_RMAT(_e, _rm)
#define FLOAT_EULERS_OF_QUAT(_e, _q)
#define 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.