vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/Maths/matrices.c

   1 #include <VP_Os/vp_os_assert.h>
   2
   3 #include <Maths/matrices.h>
   4 #include <Maths/maths.h>
   5
   6 const matrix33_t matrix_id3 =  { 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f };
   7 const vector31_t vector31_zero = { { { 0.0f, 0.0f, 0.0f } } };
   8 const vector31_t vector31_z = { { { 0.0f, 0.0f, 1.0f } } };
   9
  10 void mul_mat( matrix33_t* out, matrix33_t *m1, matrix33_t *m2 )
  11 {
  12   /// You can't have output & input pointing to the same location
  13   VP_OS_ASSERT( out != m1 );
  14   VP_OS_ASSERT( out != m2 );
  15
  16   out->m11 = (m1->m11)*(m2->m11) + (m1->m12)*(m2->m21) + (m1->m13)*(m2->m31);
  17   out->m12 = (m1->m11)*(m2->m12) + (m1->m12)*(m2->m22) + (m1->m13)*(m2->m32);
  18   out->m13 = (m1->m11)*(m2->m13) + (m1->m12)*(m2->m23) + (m1->m13)*(m2->m33);
  19
  20   out->m21 = (m1->m21)*(m2->m11) + (m1->m22)*(m2->m21) + (m1->m23)*(m2->m31);
  21   out->m22 = (m1->m21)*(m2->m12) + (m1->m22)*(m2->m22) + (m1->m23)*(m2->m32);
  22   out->m23 = (m1->m21)*(m2->m13) + (m1->m22)*(m2->m23) + (m1->m23)*(m2->m33);
  23
  24   out->m31 = (m1->m31)*(m2->m11) + (m1->m32)*(m2->m21) + (m1->m33)*(m2->m31);
  25   out->m32 = (m1->m31)*(m2->m12) + (m1->m32)*(m2->m22) + (m1->m33)*(m2->m32);
  26   out->m33 = (m1->m31)*(m2->m13) + (m1->m32)*(m2->m23) + (m1->m33)*(m2->m33);
  27 }
  28
  29 void add_mat( matrix33_t* out, matrix33_t *m1, matrix33_t *m2 )
  30 {
  31   out->m11 = (m1->m11) + (m2->m11);
  32   out->m12 = (m1->m12) + (m2->m12);
  33   out->m13 = (m1->m13) + (m2->m13);
  34
  35   out->m21 = (m1->m21) + (m2->m21);
  36   out->m22 = (m1->m22) + (m2->m22);
  37   out->m23 = (m1->m23) + (m2->m23);
  38
  39   out->m31 = (m1->m31) + (m2->m31);
  40   out->m32 = (m1->m32) + (m2->m32);
  41   out->m33 = (m1->m33) + (m2->m33);
  42 }
  43
  44 void mulvec_mat( vector31_t* out, matrix33_t *m1, vector31_t *v1 )
  45 {
  46   out->x = (m1->m11) * (v1->x) + (m1->m12) * (v1->y) + (m1->m13) * (v1->z);
  47   out->y = (m1->m21) * (v1->x) + (m1->m22) * (v1->y) + (m1->m23) * (v1->z);
  48   out->z = (m1->m31) * (v1->x) + (m1->m32) * (v1->y) + (m1->m33) * (v1->z);
  49 }
  50
  51 void transpose_mat( matrix33_t *out, matrix33_t *m1 )
  52 {
  53   out->m11 = m1->m11;
  54   out->m12 = m1->m21;
  55   out->m13 = m1->m31;
  56
  57   out->m21 = m1->m12;
  58   out->m22 = m1->m22;
  59   out->m23 = m1->m32;
  60
  61   out->m31 = m1->m13;
  62   out->m32 = m1->m23;
  63   out->m33 = m1->m33;
  64 }
  65
  66 void mulconst_mat( matrix33_t *out, matrix33_t *m1, float32_t k )
  67 {
  68   out->m11 = m1->m11 * k;
  69   out->m12 = m1->m12 * k;
  70   out->m13 = m1->m13 * k;
  71
  72   out->m21 = m1->m21 * k;
  73   out->m22 = m1->m22 * k;
  74   out->m23 = m1->m23 * k;
  75
  76   out->m31 = m1->m31 * k;
  77   out->m32 = m1->m32 * k;
  78   out->m33 = m1->m33 * k;
  79 }
  80
  81 void cross_vec( vector31_t* out, vector31_t *v1, vector31_t *v2 )
  82 {
  83   /// You can't have output & input pointing to the same location
  84   VP_OS_ASSERT( out != v1 );
  85   VP_OS_ASSERT( out != v2 );
  86
  87   out->x = (v1->y) * (v2->z) - (v1->z) * (v2->y);
  88   out->y = (v1->z) * (v2->x) - (v1->x) * (v2->z);
  89   out->z = (v1->x) * (v2->y) - (v1->y) * (v2->x);
  90 }
  91
  92 void dot_vec( float32_t* out, const vector31_t *v1, const vector31_t *v2 )
  93 {
  94   *out = v1->x * v2->x + v1->y * v2->y + v1->z * v2->z;
  95 }
  96
  97 void add_vec( vector31_t* out, vector31_t *v1, vector31_t *v2 )
  98 {
  99   out->x = (v1->x) + (v2->x);
 100   out->y = (v1->y) + (v2->y);
 101   out->z = (v1->z) + (v2->z);
 102 }
 103
 104 void mulconst_vec( vector31_t *out, vector31_t *v1, float32_t k )
 105 {
 106   out->x = (v1->x) * k;
 107   out->y = (v1->y) * k;
 108   out->z = (v1->z) * k;
 109 }
 110
 111 void skew_anti_symetric_vec( matrix33_t *out, vector31_t *v )
 112 {
 113   out->m11 = 0.0f;
 114   out->m12 =-v->z;
 115   out->m13 = v->y;
 116
 117   out->m21 = v->z;
 118   out->m22 = 0.0f;
 119   out->m23 =-v->x;
 120
 121   out->m31 =-v->y;
 122   out->m32 = v->x;
 123   out->m33 = 0.0f;
 124 }
 125
 126 void vex( vector31_t *out, matrix33_t *m )
 127 {
 128   out->x = m->m32;
 129   out->y = m->m13;
 130   out->z = m->m21;
 131 }
 132
 133 float32_t norm_vec( vector31_t *v )
 134 {
 135   return sqrtf( (v->x)*(v->x) + (v->y)*(v->y) + (v->z)*(v->z) );
 136 }
 137
 138 bool_t normalize_vec( vector31_t* v )
 139 {
 140   bool_t ret;
 141   float32_t l;
 142
 143   l = norm_vec( v );
 144
 145   if( f_is_zero( l ) )
 146   {
 147     v->x = 0.0f;
 148     v->y = 0.0f;
 149     v->z = 0.0f;
 150
 151     ret = FALSE;
 152   }
 153   else
 154   {
 155     v->x = f_zero( v->x / l );
 156     v->y = f_zero( v->y / l );
 157     v->z = f_zero( v->z / l );
 158
 159     ret = TRUE;
 160   }
 161
 162   return ret;
 163 }
 164