[math] add FLOAT_QUAT_OF_ORIENTATION_VECT

paparazzi · Aug 6, 2013 · 3951bd7 · 3951bd7
1 parent 55c3ad0
commit 3951bd7
Showing 1 changed file with 16 additions and 0 deletions.
diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
@@ -701,6 +701,22 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
     _q.qz = san * _uv.z;					\
   }
 
+#define FLOAT_QUAT_OF_ORIENTATION_VECT(_q, _ov) {                       \
+    const float ov_norm = sqrtf((_ov).x*(_ov).x + (_ov).y*(_ov).y + (_ov).z*(_ov).z); \
+    if (ov_norm < 1e-8) {                                               \
+      (_q).qi = 1;                                                      \
+      (_q).qx = 0;                                                      \
+      (_q).qy = 0;                                                      \
+      (_q).qz = 0;                                                      \
+    } else {                                                            \
+      const float s2_normalized = sinf(ov_norm/2.0) / ov_norm;          \
+      (_q).qi = cosf(ov_norm/2.0);                                      \
+      (_q).qx = (_ov).x * s2_normalized;                                \
+      (_q).qy = (_ov).y * s2_normalized;                                \
+      (_q).qz = (_ov).z * s2_normalized;                                \
+    }                                                                   \
+  }
+
 #define FLOAT_QUAT_OF_RMAT(_q, _r) {					\
     const float tr = RMAT_TRACE(_r);					\
     if (tr > 0) {							\