git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/Maths/matrix3d.c

   1 /**
   2  *  \brief    Matrix 3d implementation
   3  *  \author   Sylvain Gaeremynck <sylvain.gaeremynck@parrot.fr>
   4  *  \version  1.0
   5  *  \date     04/06/2007
   6  *  \warning  Subject to completion
   7  */
   8
   9 #include <Maths/matrix3d.h>
  10 #include <Maths/matrices.h>
  11
  12 #include <VP_Os/vp_os_malloc.h>
  13
  14 #include <math.h>
  15
  16 matrix3d_t matrix3d_id = {
  17   1.0f, 0.0f, 0.0f, 0.0f,
  18   0.0f, 1.0f, 0.0f, 1.0f,
  19   0.0f, 0.0f, 1.0f, 0.0f,
  20   0.0f, 0.0f, 0.0f, 1.0f
  21 };
  22
  23 static void vector_X( vector31_t* v )
  24 {
  25   v->x = 1.0f;
  26   v->y = 0.0f;
  27   v->z = 0.0f;
  28 }
  29
  30 static void vector_Y( vector31_t* v )
  31 {
  32   v->x = 0.0f;
  33   v->y = 1.0f;
  34   v->z = 0.0f;
  35 }
  36
  37 #if 0 // it is not used ... fix warning
  38 static void vector_Z( vector31_t* v )
  39 {
  40   v->x = 0.0f;
  41   v->y = 0.0f;
  42   v->z = 1.0f;
  43 }
  44 #endif
  45
  46 void matrix3d_zero(matrix3d_t* m)
  47 {
  48   m->m00 = m->m01 = m->m02 = m->m03 = 0.0f;
  49   m->m10 = m->m11 = m->m12 = m->m13 = 0.0f;
  50   m->m20 = m->m21 = m->m22 = m->m23 = 0.0f;
  51   m->m30 = m->m31 = m->m32 = m->m33 = 0.0f;
  52 }
  53
  54 void matrix3d_identity(matrix3d_t* m)
  55 {
  56   m->m00 = m->m11 = m->m22 = m->m33 = 1.0f;
  57
  58   m->m01 = m->m02 = m->m03 = 0.0f;
  59   m->m10 = m->m12 = m->m13 = 0.0f;
  60   m->m20 = m->m21 = m->m23 = 0.0f;
  61   m->m30 = m->m31 = m->m32 = 0.0f;
  62 }
  63
  64 C_RESULT matrix3d_euler(matrix3d_t* m, float32_t phi, float32_t theta, float32_t psi)
  65 {
  66   float32_t c_phi, s_phi, c_theta, s_theta, c_psi, s_psi;
  67
  68   c_phi   = cosf( phi );
  69   s_phi   = sinf( phi );
  70   c_theta = cosf( theta );
  71   s_theta = sinf( theta );
  72   c_psi   = cosf( psi );
  73   s_psi   = sinf( psi );
  74
  75   matrix3d_identity(m);
  76
  77   m->m00 = c_theta * c_psi;
  78   m->m01 = c_theta * s_psi;
  79   m->m02 = -s_theta;
  80
  81   m->m10 = s_phi * s_theta * c_psi - c_phi * s_psi;
  82   m->m11 = s_phi * s_theta * s_psi + c_phi * c_psi;
  83   m->m12 = s_phi * c_theta;
  84
  85   m->m20 = c_phi * s_theta * c_psi + s_phi * s_psi;
  86   m->m21 = c_phi * s_theta * s_psi - s_phi * c_psi;
  87   m->m22 = c_phi * c_theta;
  88
  89   return C_OK;
  90 }
  91
  92 C_RESULT matrix3d_vector(matrix3d_t* m, vector31_t* pos, vector31_t* dir, vector31_t* right, vector31_t* up)
  93 {
  94   normalize_vec( dir );
  95   vector_Y( up );
  96
  97   cross_vec( right, dir, up );
  98   if( normalize_vec( right ) )
  99   {
 100     cross_vec( up, right, dir );
 101   }
 102   else
 103   {
 104     vector_X( right );
 105
 106     cross_vec( up, right, dir );
 107     cross_vec( right, dir, up );
 108     normalize_vec( right );
 109   }
 110
 111   normalize_vec( up );
 112
 113   return matrix3d_orientation( m, pos, dir, right, up );
 114 }
 115
 116 C_RESULT matrix3d_orientation(matrix3d_t* m, const vector31_t* pos, const vector31_t* dir,
 117                                              const vector31_t* right, const vector31_t* up)
 118 {
 119   float32_t d0, d1, d2;
 120
 121   m->m00 =  right->x;
 122   m->m01 =  up->x;
 123   m->m02 = -dir->x;
 124
 125   m->m10 =  right->y;
 126   m->m11 =  up->y;
 127   m->m12 = -dir->y;
 128
 129   m->m20 =  right->z;
 130   m->m21 =  up->z;
 131   m->m22 = -dir->z;
 132
 133   dot_vec( &d0, right, pos );
 134   dot_vec( &d1, up,    pos );
 135   dot_vec( &d2, dir,   pos );
 136
 137   m->m30 = -d0;
 138   m->m31 = -d1;
 139   m->m32 = -d2;
 140
 141   m->m03 =  0.0f;
 142   m->m13 =  0.0f;
 143   m->m23 =  0.0f;
 144   m->m33 =  1.0f;
 145
 146   return C_OK;
 147 }
 148
 149 #define MATRIX_EXCHANGE( out, in ) temp = out; out = in; in = out
 150
 151 C_RESULT matrix3d_transpose(matrix3d_t* out, matrix3d_t* in)
 152 {
 153   if( out != in )
 154   {
 155     out->m00 = in->m00;
 156     out->m10 = in->m01;
 157     out->m20 = in->m02;
 158     out->m30 = in->m03;
 159
 160     out->m01 = in->m10;
 161     out->m11 = in->m11;
 162     out->m21 = in->m12;
 163     out->m31 = in->m13;
 164
 165     out->m02 = in->m20;
 166     out->m12 = in->m21;
 167     out->m22 = in->m22;
 168     out->m32 = in->m23;
 169
 170     out->m03 = in->m30;
 171     out->m13 = in->m31;
 172     out->m23 = in->m32;
 173     out->m33 = in->m33;
 174   }
 175   else
 176   {
 177     float32_t temp;
 178
 179     MATRIX_EXCHANGE( out->m01, out->m10 );
 180     MATRIX_EXCHANGE( out->m02, out->m20 );
 181     MATRIX_EXCHANGE( out->m03, out->m30 );
 182     MATRIX_EXCHANGE( out->m12, out->m21 );
 183     MATRIX_EXCHANGE( out->m13, out->m31 );
 184     MATRIX_EXCHANGE( out->m23, out->m32 );
 185   }
 186
 187   return C_OK;
 188 }
 189
 190 C_RESULT matrix3d_add(matrix3d_t* out, matrix3d_t* m1, matrix3d_t* m2)
 191 {
 192   out->m00 = m1->m00 + m2->m00;
 193   out->m01 = m1->m01 + m2->m01;
 194   out->m02 = m1->m02 + m2->m02;
 195   out->m03 = m1->m03 + m2->m03;
 196
 197   out->m10 = m1->m10 + m2->m10;
 198   out->m11 = m1->m11 + m2->m11;
 199   out->m12 = m1->m12 + m2->m12;
 200   out->m13 = m1->m13 + m2->m13;
 201
 202   out->m20 = m1->m20 + m2->m20;
 203   out->m21 = m1->m21 + m2->m21;
 204   out->m22 = m1->m22 + m2->m22;
 205   out->m23 = m1->m23 + m2->m23;
 206
 207   out->m30 = m1->m30 + m2->m30;
 208   out->m31 = m1->m31 + m2->m31;
 209   out->m32 = m1->m32 + m2->m32;
 210   out->m33 = m1->m33 + m2->m33;
 211
 212   return C_OK;
 213 }
 214
 215 C_RESULT matrix3d_sub(matrix3d_t* out, matrix3d_t* m1, matrix3d_t* m2)
 216 {
 217   out->m00 = m1->m00 - m2->m00;
 218   out->m01 = m1->m01 - m2->m01;
 219   out->m02 = m1->m02 - m2->m02;
 220   out->m03 = m1->m03 - m2->m03;
 221
 222   out->m10 = m1->m10 - m2->m10;
 223   out->m11 = m1->m11 - m2->m11;
 224   out->m12 = m1->m12 - m2->m12;
 225   out->m13 = m1->m13 - m2->m13;
 226
 227   out->m20 = m1->m20 - m2->m20;
 228   out->m21 = m1->m21 - m2->m21;
 229   out->m22 = m1->m22 - m2->m22;
 230   out->m23 = m1->m23 - m2->m23;
 231
 232   out->m30 = m1->m30 - m2->m30;
 233   out->m31 = m1->m31 - m2->m31;
 234   out->m32 = m1->m32 - m2->m32;
 235   out->m33 = m1->m33 - m2->m33;
 236
 237   return C_OK;
 238 }
 239
 240 #define MATRIX_MUL_PART(a,b)          \
 241   t1 = ( M1[a*4 + 0] * M2[b + 0 ] );  \
 242   t2 = ( M1[a*4 + 1] * M2[b + 4 ] );  \
 243   t3 = ( M1[a*4 + 2] * M2[b + 8 ] );  \
 244   t4 = ( M1[a*4 + 3] * M2[b + 12] );  \
 245   MOUT[a*4 + b] = ( t1 + t2 + t3 + t4 );
 246
 247 C_RESULT matrix3d_mul(matrix3d_t* out, matrix3d_t* m1, matrix3d_t* m2)
 248 {
 249   float32_t t1, t2, t3, t4;
 250
 251   float32_t* MOUT = (float32_t*) out;
 252   float32_t* M1  = (float32_t*) m1;
 253   float32_t* M2  = (float32_t*) m2;
 254
 255   MATRIX_MUL_PART( 0, 0 ); MATRIX_MUL_PART( 0, 1 ); MATRIX_MUL_PART( 0, 2 ); MATRIX_MUL_PART( 0, 3 );
 256   MATRIX_MUL_PART( 1, 0 ); MATRIX_MUL_PART( 1, 1 ); MATRIX_MUL_PART( 1, 2 ); MATRIX_MUL_PART( 1, 3 );
 257   MATRIX_MUL_PART( 2, 0 ); MATRIX_MUL_PART( 2, 1 ); MATRIX_MUL_PART( 2, 2 ); MATRIX_MUL_PART( 2, 3 );
 258   MATRIX_MUL_PART( 3, 0 ); MATRIX_MUL_PART( 3, 1 ); MATRIX_MUL_PART( 3, 2 ); MATRIX_MUL_PART( 3, 3 );
 259
 260   return C_OK;
 261 }
 262
 263 C_RESULT matrix3d_translate(matrix3d_t* m, vector31_t* tr)
 264 {
 265   m->m30 =  tr->x;
 266   m->m31 =  tr->y;
 267   m->m32 =  tr->z;
 268   m->m33 =  1.0f;
 269
 270   return C_OK;
 271 }
 272
 273 C_RESULT matrix3d_add_translate(matrix3d_t* m, vector31_t* tr)
 274 {
 275   m->m30 +=  tr->x;
 276   m->m31 +=  tr->y;
 277   m->m32 +=  tr->z;
 278   m->m33 =  1.0f;
 279
 280   return C_OK;
 281 }
 282
 283 C_RESULT matrix3d_rotate_euler(matrix3d_t* m, float32_t phi, float32_t theta, float32_t psi)
 284 {
 285   matrix3d_t mat_rot;
 286   matrix3d_t mat_res;
 287
 288   matrix3d_euler(&mat_rot, phi, theta, psi);
 289   matrix3d_mul(&mat_res, &mat_rot, m);
 290
 291   vp_os_memcpy( (void *)m, (void *)&mat_res, sizeof(matrix3d_t) );
 292
 293   return C_OK;
 294 }
 295
 296 C_RESULT matrix3d_rotate_axis(matrix3d_t* m, vector31_t* axis, float32_t value)
 297 {
 298   return C_OK;
 299 }
 300
 301 C_RESULT matrix3d_transform(matrix3d_t* m1, struct _vector31_t* v1)
 302 {
 303   float32_t x = (m1->m00) * (v1->x) + (m1->m01) * (v1->y) + (m1->m02) * (v1->z) + m1->m30;
 304   float32_t y = (m1->m10) * (v1->x) + (m1->m11) * (v1->y) + (m1->m12) * (v1->z) + m1->m31;
 305   float32_t z = (m1->m20) * (v1->x) + (m1->m21) * (v1->y) + (m1->m22) * (v1->z) + m1->m32;
 306   float32_t w = (m1->m30) * (v1->x) + (m1->m31) * (v1->y) + (m1->m32) * (v1->z) + m1->m33;
 307
 308   v1->x = x / w;
 309   v1->y = y / w;
 310   v1->z = z / w;
 311
 312   return C_OK;
 313 }