git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/Maths/matrices.h

   1 /**
   2  *  \file     matrices.h
   3  *  \brief    Matrices library used by ARDrone
   4  *  \author   Jean-Baptiste Lanfrey <jean-baptiste.lanfrey@parrot.com>
   5  *  \version  1.0
   6  */
   7
   8 #ifndef _MATRICES_H_
   9 #define _MATRICES_H_
  10
  11 #include <VP_Os/vp_os_types.h>
  12
  13 typedef struct _matrix33_t
  14 {
  15   float32_t m11;
  16   float32_t m12;
  17   float32_t m13;
  18   float32_t m21;
  19   float32_t m22;
  20   float32_t m23;
  21   float32_t m31;
  22   float32_t m32;
  23   float32_t m33;
  24 } matrix33_t;
  25
  26 typedef struct _vector31_t {
  27   union {
  28     float32_t v[3];
  29     struct
  30     {
  31       float32_t x;
  32       float32_t y;
  33       float32_t z;
  34     };
  35   };
  36 } vector31_t;
  37
  38 typedef union _vector21_t {
  39   float32_t v[2];
  40   struct
  41   {
  42     float32_t x;
  43     float32_t y;
  44   };
  45 } vector21_t;
  46
  47 extern const matrix33_t matrix_id3;
  48 extern const vector31_t vector31_zero;
  49 extern const vector31_t vector31_z;
  50
  51 // TODO Documentation
  52
  53 // Multiplies two matrices m1 & m2. Stores result in out.
  54 void mul_mat( matrix33_t* out, matrix33_t *m1, matrix33_t *m2 );
  55
  56
  57 void add_mat( matrix33_t* out, matrix33_t *m1, matrix33_t *m );
  58
  59
  60 void mulvec_mat( vector31_t* out, matrix33_t *m1, vector31_t *v1 );
  61
  62
  63 void transpose_mat( matrix33_t *out, matrix33_t *m1 );
  64
  65
  66 void mulconst_mat( matrix33_t *out, matrix33_t *m1, float32_t k );
  67
  68
  69 void cross_vec( vector31_t* out, vector31_t *v1, vector31_t *v2 );
  70
  71
  72 void dot_vec( float32_t* out, const vector31_t *v1, const vector31_t *v2 );
  73
  74
  75 void add_vec( vector31_t* out, vector31_t *v1, vector31_t *v2 );
  76
  77
  78 void mulconst_vec( vector31_t *out, vector31_t *V1, float32_t k );
  79
  80
  81 void skew_anti_symetric_vec( matrix33_t *out, vector31_t *v );
  82
  83
  84 void vex( vector31_t *out, matrix33_t *m );
  85
  86
  87 float32_t norm_vec( vector31_t *v );
  88
  89
  90 bool_t normalize_vec( vector31_t* v );
  91
  92
  93 #endif // _MATRICES_H_