ArDrone SDK 1.8 added

[mardrone] / mardrone / ARDrone_SDK_Version_1_8_20110726 / ARDroneLib / Soft / Lib / Maths / quaternions.c

diff --git a/mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/Maths/quaternions.c b/mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/Maths/quaternions.c
new file mode 100644 (file)
index 0000000..bb14ef0
--- /dev/null
+++ b/mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/Maths/quaternions.c
@@ -0,0 +1,129 @@
+/**
+ *  \file     quaternions.c
+ *  \brief    Quaternions library used by Mykonos
+ *  \author   Fran�ois Callou <francois.callou@parrot.com>
+ *  \version  1.0
+ */
+
+#include <VP_Os/vp_os_assert.h>
+#include <Maths/quaternions.h>
+#include <Maths/maths.h>
+
+const quaternion_t quat_unitary = { 1.0f, {{{ 0.0f, 0.0f, 0.0f}}} };
+
+void mul_quat( quaternion_t* out, quaternion_t* q1, quaternion_t* q2)
+{
+  vector31_t temp_v;
+  
+  /// You can't have output & input pointing to the same location
+  VP_OS_ASSERT( out != q1 );
+  VP_OS_ASSERT( out != q2 );
+
+  // scalar result
+  out->a = q1->a*q2->a - (q1->v.x*q2->v.x + q1->v.y*q2->v.y + q1->v.z*q2->v.z); 
+  
+  // pure quaternion result
+  cross_vec( &out->v , &q1->v,  &q2->v );
+  mulconst_vec( &temp_v, &q2->v, q1->a );
+  add_vec( &out->v, &out->v, &temp_v);
+  mulconst_vec( &temp_v, &q1->v, q2->a );
+  add_vec( &out->v, &out->v, &temp_v);
+}
+
+void add_quat( quaternion_t* out, quaternion_t* q1, quaternion_t* q2 )
+{
+  // scalar result
+  out->a = q1->a + q2->a;
+  // pure quaternion result
+  add_vec( &out->v, &q1->v, &q2->v );
+}
+
+void mulconst_quat( quaternion_t* out, quaternion_t* q, float32_t k )
+{
+  out->a = (q->a) * k;
+  mulconst_vec( &out->v, &q->v, k );
+}
+
+void conjugate_quat( quaternion_t* out, quaternion_t* q )
+{
+  out->a = q->a;
+  out->v.x = -q->v.x;
+  out->v.y = -q->v.y;
+  out->v.z = -q->v.z;
+}
+
+float32_t norm_quat( quaternion_t *q )
+{
+  return sqrtf( q->a*q->a + q->v.x * q->v.x + q->v.y * q->v.y + q->v.z * q->v.z );
+}
+
+bool_t normalize_quat( quaternion_t* q )
+{
+  bool_t ret;
+  float32_t norm;
+
+  norm = norm_quat( q );
+
+  if( f_is_zero( norm ) )
+  {
+    q->a   = 0.0f;
+    q->v.x = 0.0f;
+    q->v.y = 0.0f;
+    q->v.z = 0.0f;
+
+    ret = FALSE;
+  }
+  else
+  {
+    q->a   = f_zero( q->a / norm );
+    q->v.x = f_zero( q->v.x / norm );
+    q->v.y = f_zero( q->v.y / norm );
+    q->v.z = f_zero( q->v.z / norm );
+
+    ret = TRUE;
+  }
+
+  return ret;
+}
+
+
+void quat_to_euler_rot_mat(matrix33_t* m, quaternion_t* q)
+{
+  //to use with normalised quaternion
+  m->m11 = 1.0f - 2*q->v.y*q->v.y - 2*q->v.z*q->v.z;
+  m->m12 = 2*q->v.x*q->v.y - 2*q->v.z*q->a;
+  m->m13 = 2*q->v.z*q->v.x + 2*q->v.y*q->a;
+  m->m21 = 2*q->v.x*q->v.y + 2*q->v.z*q->a;
+  m->m22 = 1.0f - 2*q->v.x*q->v.x - 2*q->v.z*q->v.z;
+  m->m23 = 2*q->v.z*q->v.y - 2*q->v.x*q->a;
+  m->m31 = 2*q->v.z*q->v.x - 2*q->v.y*q->a;
+  m->m32 = 2*q->v.z*q->v.y + 2*q->v.x*q->a;
+  m->m33 = 1.0f - 2*q->v.x*q->v.x - 2*q->v.y*q->v.y;
+}
+
+
+
+void quat_to_euler_angles(angles_t* a, quaternion_t* q)
+{ 
+       //to use with normalised quaternion
+       float32_t sqvx = q->v.x*q->v.x;
+       float32_t sqvy = q->v.y*q->v.y;
+  float32_t sqvz = q->v.z*q->v.z;
+
+ /* if ( f_is_zero(test -0.5) ) { // singularity at north pole
+               a->psi   = 2 * atan2(q->a,q->v.z);
+               a->theta = PI/2;
+               a->phi   = 0;
+               return;
+       }
+       if ( f_is_zero(test + 0.5) ) { // singularity at south pole
+               a->psi   = -2 * atan2(q->a,q->v.z);
+               a->theta = - PI/2;
+               a->phi   = 0;
+               return;
+       }*/
+  
+       a->phi   = atan2(2*q->v.y*q->v.z+2*q->a*q->v.x , 1 - 2*sqvx - 2*sqvy);
+       a->theta = asin(2*(q->a*q->v.y - q->v.x*q->v.z ));
+       a->psi   = atan2(2*q->v.x*q->v.y+2*q->a*q->v.z , 1 - 2*sqvy - 2*sqvz);
+}