added quaternions operations

Author: kvedala

geometry/quaternions.c

@@ -0,0 +1,173 @@
+/**
+ * @file
+ * @brief API Functions related to 3D vector operations.
+ * @author Krishna Vedala
+ */
+
+#include <stdio.h>
+#ifdef __arm__  // if compiling for ARM-Cortex processors
+#define LIBQUAT_ARM
+#include <arm_math.h>
+#else
+#include <math.h>
+#endif
+#include <assert.h>
+
+#include "geometry_datatypes.h"
+
+/**
+ * @addtogroup quats 3D Quaternion operations
+ * @{
+ */
+
+/**
+ * Function to convert given Euler angles to a quaternion.
+ * \f{eqnarray*}{
+ * q_{0} & =
+ * &\cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
+ * +
+ * \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
+ * q_{1} & =
+ * &\sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
+ * -
+ * \cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
+ * q_{2} & =
+ * &\cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
+ * +
+ * \sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
+ * q_{3} & =
+ * &\cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)
+ * -
+ * \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)\\
+ * \f}
+ *
+ * @param [in] in_euler input Euler angles instance
+ * @returns converted quaternion
+ */
+quaternion quat_from_euler(const euler *in_euler)
+{
+    quaternion out_quat;
+
+    if (!in_euler)  // if null
+    {
+        fprintf(stderr, "%s: Invalid input.", __func__);
+        return out_quat;
+    }
+
+    quaternion temp;
+
+    float cy = cosf(in_euler->yaw * 0.5f);
+    float sy = sinf(in_euler->yaw * 0.5f);
+    float cp = cosf(in_euler->pitch * 0.5f);
+    float sp = sinf(in_euler->pitch * 0.5f);
+    float cr = cosf(in_euler->roll * 0.5f);
+    float sr = sinf(in_euler->roll * 0.5f);
+
+    temp.w = cr * cp * cy + sr * sp * sy;
+    temp.q1 = sr * cp * cy - cr * sp * sy;
+    temp.q2 = cr * sp * cy + sr * cp * sy;
+    temp.q3 = cr * cp * sy - sr * sp * cy;
+
+    return temp;
+}
+
+/**
+ * Function to convert given quaternion to Euler angles.
+ * \f{eqnarray*}{
+ * \phi & = &
+ * \tan^{-1}\left[\frac{2\left(q_0q_1+q_2q_3\right)}{1-2\left(q_1^2+q_2^2\right)}\right]\\
+ * \theta & =
+ * &-\sin^{-1}\left[2\left(q_0q_2-q_3q_1\right)\right]\\
+ * \psi & = &
+ * \tan^{-1}\left[\frac{2\left(q_0q_3+q_1q_2\right)}{1-2\left(q_2^2+q_3^2\right)}\right]\\
+ * \f}
+ *
+ * @param [in] in_quat input quaternion instance
+ * @returns converted euler angles
+ */
+euler euler_from_quat(const quaternion *in_quat)
+{
+    euler out_euler;
+    if (!in_quat)  // if null
+    {
+        fprintf(stderr, "%s: Invalid input.", __func__);
+        return out_euler;
+    }
+
+    out_euler.roll = atan2f(
+        2.f * (in_quat->w * in_quat->q1 + in_quat->q2 * in_quat->q3),
+        1.f - 2.f * (in_quat->q1 * in_quat->q1 + in_quat->q2 * in_quat->q2));
+    out_euler.pitch =
+        asinf(2.f * (in_quat->w * in_quat->q2 + in_quat->q1 * in_quat->q3));
+    out_euler.yaw = atan2f(
+        2.f * (in_quat->w * in_quat->q3 + in_quat->q1 * in_quat->q2),
+        1.f - 2.f * (in_quat->q2 * in_quat->q2 + in_quat->q3 * in_quat->q3));
+
+    return out_euler;
+}
+
+/**
+ * Function to multiply two quaternions.
+ * \f{eqnarray*}{
+ * \mathbf{c} & = & \mathbf{a}\otimes\mathbf{b}\\
+ * & = & \begin{bmatrix}a_{0} & a_{1} & a_{2} &
+ *  a_{3}\end{bmatrix}\otimes\begin{bmatrix}b_{0} & b_{1} & b_{2} &
+ *  b_{3}\end{bmatrix}\\
+ * & = &
+ * \begin{bmatrix}
+ *  a_{0}b_{0}-a_{1}b_{1}-a_{2}b_{2}-a_{3}b_{3}\\
+ *  a_{0}b_{1}+a_{1}b_{0}+a_{2}b_{3}-a_{3}b_{2}\\
+ *  a_{0}b_{2}-a_{1}b_{3}+a_{2}b_{0}+a_{3}b_{1}\\
+ *  a_{0}b_{3}+a_{1}b_{2}-a_{2}b_{1}+a_{3}b_{0}
+ * \end{bmatrix}^{T}
+ * \f}
+ *
+ * @param [in] in_quat1 first input quaternion instance
+ * @param [in] in_quat2 second input quaternion instance
+ * @returns resultant quaternion
+ */
+quaternion quaternion_multiply(const quaternion *in_quat1,
+                               const quaternion *in_quat2)
+{
+    quaternion out_quat;
+    if (!in_quat1 || !in_quat2)  // if null
+    {
+        fprintf(stderr, "%s: Invalid input.", __func__);
+        return out_quat;
+    }
+
+    out_quat.w = in_quat1->w * in_quat2->w - in_quat1->q1 * in_quat2->q1 -
+                 in_quat1->q2 * in_quat2->q2 - in_quat1->q3 * in_quat2->q3;
+    out_quat.q1 = in_quat1->w * in_quat2->q1 + in_quat1->q1 * in_quat2->w +
+                  in_quat1->q2 * in_quat2->q3 - in_quat1->q3 * in_quat2->q2;
+    out_quat.q2 = in_quat1->w * in_quat2->q2 - in_quat1->q1 * in_quat2->q3 +
+                  in_quat1->q2 * in_quat2->w + in_quat1->q3 * in_quat2->q1;
+    out_quat.q3 = in_quat1->w * in_quat2->q3 + in_quat1->q1 * in_quat2->q2 -
+                  in_quat1->q2 * in_quat2->q1 + in_quat1->q3 * in_quat2->w;
+
+    return out_quat;
+}
+
+/** @} */
+
+static void test()
+{
+    quaternion quat = {0.7071f, 0.7071f, 0.f, 0.f};
+    euler eul = euler_from_quat(&quat);
+    printf("Euler: %.4g, %.4g, %.4g\n", eul.pitch, eul.roll, eul.yaw);
+
+    quaternion test_quat = quat_from_euler(&eul);
+    printf("Quaternion: %.4g %+.4g %+.4g %+.4g\n", test_quat.w,
+           test_quat.dual.x, test_quat.dual.y, test_quat.dual.z);
+
+    assert(fabs(test_quat.w - quat.w) < .01);
+    assert(fabs(test_quat.q1 - quat.q1) < .01);
+    assert(fabs(test_quat.q2 - quat.q2) < .01);
+    assert(fabs(test_quat.q3 - quat.q3) < .01);
+}
+
+int main()
+{
+    test();
+    return 0;
+}