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_MUL_VECT(_n, o, a, b)
	o = a * b
#define	MAT_INV33(_invS, _S)
#define	MAT_SUM(_i, _j, C, A, B)
#define	MAT_SUM_c(_i, _j, C, A, B, k_)
#define	MAT_MUL_c(_i, _k, _j, C, A, B, c_)
#define	MAT_PRINT(_i, _j, A)

Detailed Description

Simple matrix helper macros.

Definition in file pprz_simple_matrix.h.

Macro Definition Documentation

MAT_INV33

#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 99 of file pprz_simple_matrix.h.

MAT_MUL

#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.

MAT_MUL_c

#define MAT_MUL_c	(		_i,
			_k,
			_j,
			C,
			A,
			B,
			c_
	)

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] += c_*A[l][m]*B[m][c];        \
      }                                         \
  }

Definition at line 150 of file pprz_simple_matrix.h.

MAT_MUL_T

#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.

MAT_MUL_VECT

#define MAT_MUL_VECT	(		_n,
			o,
			a,
			b
	)

Value:

    {             \
    int l,m;                                    \
    for (l = 0; l < _n; l++) {                  \
      o[l] = 0;                               \
      for (m = 0; m < _n; m++) {              \
        o[l] += a[l][m] * b[m];               \
      }                                       \
    }                                         \
  }

o = a * b

a: [n x n] b: [n x 1] o: [n x 1]

Definition at line 86 of file pprz_simple_matrix.h.

MAT_PRINT

#define MAT_PRINT	(		_i,
			_j,
			A
	)

Value:

    {                  \
    int l,c;                                   \
    printf("float ");                          \
    printf(#A);                                \
    printf(" = {\n");                          \
    for (l=0; l<_i; l++){                      \
      printf("{");                             \
      for (c=0; c<_j; c++){                    \
        if(c<(_j-1)){printf("%f,",A[l][c]);}   \
        else{printf("%f",A[l][c]);}            \
      }                                        \
      if(l<(_i-1)){                            \
        printf("} ,\n");}                      \
      else{printf("}\n");}                     \
    }                                          \
    printf("}\n");                             \
  }

Definition at line 161 of file pprz_simple_matrix.h.

MAT_SUB

#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.

MAT_SUM

#define MAT_SUM	(		_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 129 of file pprz_simple_matrix.h.

MAT_SUM_c

#define MAT_SUM_c	(		_i,
			_j,
			C,
			A,
			B,
			k_
	)

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]*k_;         \
  }

Definition at line 140 of file pprz_simple_matrix.h.

warn_message

#define warn_message

(

...

)

do { } while(0)

Definition at line 40 of file pprz_simple_matrix.h.