pprz_simple_matrix.h File Reference

Simple matrix helper macros. More...

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

Include dependency graph for pprz_simple_matrix.h:

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

Go to the source code of this file.

Macros
#define	warn_message(...)   do { } while(0)
#define	MAT_MUL(_i, _k, _j, C, A, B)
#define	MAT_MUL_T(_i, _k, _j, C, A, B)
#define	MAT_SUB(_i, _j, C, A, B)
#define	MAT_INV33(_invS, _S)

Detailed Description

Simple matrix helper macros.

Definition in file pprz_simple_matrix.h.

Macro Definition Documentation

#define MAT_INV33	(		_invS,
			_S
	)

Value:

{                                          \
    const float m00 = _S[1][1]*_S[2][2] - _S[1][2]*_S[2][1];            \
    const float m10 = _S[0][1]*_S[2][2] - _S[0][2]*_S[2][1];            \
    const float m20 = _S[0][1]*_S[1][2] - _S[0][2]*_S[1][1];            \
    const float m01 = _S[1][0]*_S[2][2] - _S[1][2]*_S[2][0];            \
    const float m11 = _S[0][0]*_S[2][2] - _S[0][2]*_S[2][0];            \
    const float m21 = _S[0][0]*_S[1][2] - _S[0][2]*_S[1][0];            \
    const float m02 = _S[1][0]*_S[2][1] - _S[1][1]*_S[2][0];            \
    const float m12 = _S[0][0]*_S[2][1] - _S[0][1]*_S[2][0];            \
    const float m22 = _S[0][0]*_S[1][1] - _S[0][1]*_S[1][0];            \
    float det = _S[0][0]*m00 - _S[1][0]*m10 + _S[2][0]*m20;             \
    if (fabs(det) < FLT_EPSILON) {                                      \
      /* If the determinant is too small then set it to epsilon preserving sign. */ \
      warn_message("warning: %s:%d MAT_INV33 trying to invert non-invertable matrix '%s' and put result in '%s'.\n", __FILE__, __LINE__, #_S, #_invS); \
      det = copysignf(FLT_EPSILON, det);                                \
    }                                                                   \
    _invS[0][0] =  m00 / det;                                           \
    _invS[1][0] = -m01 / det;                                           \
    _invS[2][0] =  m02 / det;                                           \
    _invS[0][1] = -m10 / det;                                           \
    _invS[1][1] =  m11 / det;                                           \
    _invS[2][1] = -m12 / det;                                           \
    _invS[0][2] =  m20 / det;                                           \
    _invS[1][2] = -m21 / det;                                           \
    _invS[2][2] =  m22 / det;                                           \
  }

Definition at line 86 of file pprz_simple_matrix.h.

Referenced by update_state(), and update_state_heading().

#define MAT_MUL	(		_i,
			_k,
			_j,
			C,
			A,
			B
	)

Value:

{          \
    int l,c,m;                                  \
    for (l=0; l<_i; l++)                        \
      for (c=0; c<_j; c++) {                    \
        C[l][c] = 0.;                           \
        for (m=0; m<_k; m++)                    \
          C[l][c] += A[l][m]*B[m][c];           \
      }                                         \
  }

Definition at line 46 of file pprz_simple_matrix.h.

Referenced by fit_linear_flow_field(), propagate_state(), update_state(), and update_state_heading().

#define MAT_MUL_T	(		_i,
			_k,
			_j,
			C,
			A,
			B
	)

Value:

{        \
    int l,c,m;                                  \
    for (l=0; l<_i; l++)                        \
      for (c=0; c<_j; c++) {                    \
        C[l][c] = 0.;                           \
        for (m=0; m<_k; m++)                    \
          C[l][c] += A[l][m]*B[c][m];           \
      }                                         \
  }

Definition at line 59 of file pprz_simple_matrix.h.

Referenced by propagate_state(), update_state(), and update_state_heading().

#define MAT_SUB	(		_i,
			_j,
			C,
			A,
			B
	)

Value:

{              \
    int l,c;                                    \
    for (l=0; l<_i; l++)                        \
      for (c=0; c<_j; c++)                      \
        C[l][c] = A[l][c] - B[l][c];            \
  }

Definition at line 73 of file pprz_simple_matrix.h.

Referenced by fit_linear_flow_field(), update_state(), and update_state_heading().

#define warn_message

(

...

)

do { } while(0)

Definition at line 40 of file pprz_simple_matrix.h.