added vector operations file

kvedala · kvedala · commit 1610a09e4892 · 2020-08-16T21:46:11.000-04:00
diff --git a/geometry/vectors_3d.c b/geometry/vectors_3d.c
@@ -0,0 +1,236 @@
+/**
+ * @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 vec_3d 3D Vector operations
+ * @{
+ */
+
+/**
+ * Subtract one vector from another. @f[
+ * \vec{c}=\vec{a}-\vec{b}=\left(a_x-b_x\right)\hat{i}+
+ * \left(a_y-b_y\right)\hat{j}+\left(a_z-b_z\right)\hat{k}@f]
+ * @param[in] a vector to subtract from
+ * @param[in] b vector to subtract
+ * @returns resultant vector
+ */
+vec_3d vector_sub(const vec_3d *a, const vec_3d *b)
+{
+    vec_3d out;
+#ifdef LIBQUAT_ARM
+    arm_sub_f32((float *)a, (float *)b, (float *)&out);
+#else
+    out.x = a->x - b->x;
+    out.y = a->y - b->y;
+    out.z = a->z - b->z;
+#endif
+
+    return out;
+}
+
+/**
+ * Add one vector to another. @f[
+ * \vec{c}=\vec{a}+\vec{b}=\left(a_x+b_x\right)\hat{i}+
+ * \left(a_y+b_y\right)\hat{j}+\left(a_z+b_z\right)\hat{k}@f]
+ * @param[in] a vector to add to
+ * @param[in] b vector to add
+ * @returns resultant vector
+ */
+vec_3d vector_add(const vec_3d *a, const vec_3d *b)
+{
+    vec_3d out;
+#ifdef LIBQUAT_ARM
+    arm_add_f32((float *)a, (float *)b, (float *)&out);
+#else
+    out.x = a->x + b->x;
+    out.y = a->y + b->y;
+    out.z = a->z + b->z;
+#endif
+
+    return out;
+}
+
+/**
+ * Obtain the dot product of two 3D vectors.
+ * @f[
+ * \vec{a}\cdot\vec{b}=a_xb_x + a_yb_y + a_zb_z
+ * @f]
+ * @param[in] a first vector
+ * @param[in] b second vector
+ * @returns resulting dot product
+ */
+float dot_prod(const vec_3d *a, const vec_3d *b)
+{
+    float dot;
+#ifdef LIBQUAT_ARM
+    arm_dot_prod_f32((float *)a, (float *)b, &dot);
+#else
+    dot = a->x * b->x;
+    dot += a->y * b->y;
+    dot += a->z * b->z;
+#endif
+
+    return dot;
+}
+
+/**
+ * Compute the vector product of two 3d vectors.
+ * @f[\begin{align*}
+ * \vec{a}\times\vec{b} &= \begin{vmatrix}
+ *  \hat{i} & \hat{j} & \hat{k}\\
+ *  a_x & a_y & a_z\\
+ *  b_x & b_y & b_z
+ *  \end{vmatrix}\\
+ *  &= \left(a_yb_z-b_ya_z\right)\hat{i} - \left(a_xb_z-b_xa_z\right)\hat{j}
+ * + \left(a_xb_y-b_xa_y\right)\hat{k} \end{align*}
+ * @f]
+ * @param[in] a first vector @f$\vec{a}@f$
+ * @param[in] b second vector @f$\vec{b}@f$
+ * @returns resultant vector @f$\vec{o}=\vec{a}\times\vec{b}@f$
+ */
+vec_3d vector_prod(const vec_3d *a, const vec_3d *b)
+{
+    vec_3d out;  // better this way to avoid copying results to input
+                 // vectors themselves
+    out.x = a->y * b->z - a->z * b->y;
+    out.y = -a->x * b->z + a->z * b->x;
+    out.z = a->x * b->y - a->y * b->x;
+
+    return out;
+}
+
+/**
+ * Print formatted vector on stdout.
+ * @param[in] a vector to print
+ * @param[in] name  name of the vector
+ * @returns string representation of vector
+ */
+const char *print_vector(const vec_3d *a, const char *name)
+{
+    static char vec_str[100];  // static to ensure the string life extends the
+                               // life of function
+
+    snprintf(vec_str, 99, "vec(%s) = (%.3g)i + (%.3g)j + (%.3g)k\n", name, a->x,
+             a->y, a->z);
+    return vec_str;
+}
+
+/**
+ * Compute the norm a vector.
+ * @f[\lVert\vec{a}\rVert = \sqrt{\vec{a}\cdot\vec{a}} @f]
+ * @param[in] a input vector
+ * @returns norm of the given vector
+ */
+float vector_norm(const vec_3d *a)
+{
+    float n = dot_prod(a, a);
+#ifdef LIBQUAT_ARM
+    arm_sqrt_f32(*n, n);
+#else
+    n = sqrtf(n);
+#endif
+
+    return n;
+}
+
+/**
+ * Obtain unit vector in the same direction as given vector.
+ * @f[\hat{a}=\frac{\vec{a}}{\lVert\vec{a}\rVert}@f]
+ * @param[in] a input vector
+ * @returns n unit vector in the direction of @f$\vec{a}@f$
+ */
+vec_3d unit_vec(const vec_3d *a)
+{
+    vec_3d n = {0};
+
+    float norm = vector_norm(a);
+    if (fabsf(norm) < EPSILON)  // detect possible divide by 0
+        return n;
+
+    if (norm != 1.F)  // perform division only if needed
+    {
+        n.x = a->x / norm;
+        n.y = a->y / norm;
+        n.z = a->z / norm;
+    }
+    return n;
+}
+
+/**
+ * The cross product of vectors can be represented as a matrix
+ * multiplication operation. This function obtains the `3x3` matrix
+ * of the cross-product operator from the first vector.
+ * @f[\begin{align*}
+ * \left(\vec{a}\times\right)\vec{b} &= \tilde{A}_a\vec{b}\\
+ * \tilde{A}_a &=
+ * \begin{bmatrix}0&-a_z&a_y\\a_z&0&-a_x\\-a_y&a_x&0\end{bmatrix}
+ * \end{align*}@f]
+ * @param[in] a input vector
+ * @returns the `3x3` matrix for the cross product operator
+ * @f$\left(\vec{a}\times\right)@f$
+ */
+mat_3x3 get_cross_matrix(const vec_3d *a)
+{
+    mat_3x3 A = {0., -a->z, a->y, a->z, 0., -a->x, -a->y, a->x, 0.};
+    return A;
+}
+
+/** @} */
+
+/**
+ * @brief Testing function
+ * @returns `void`
+ */
+static void test()
+{
+    vec_3d a = {1., 2., 3.};
+    vec_3d b = {1., 1., 1.};
+    float d;
+
+    // printf("%s", print_vector(&a, "a"));
+    // printf("%s", print_vector(&b, "b"));
+
+    d = vector_norm(&a);
+    // printf("|a| = %.4g\n", d);
+    assert(fabs(d - 3.742) < 0.01);
+    d = vector_norm(&b);
+    // printf("|b| = %.4g\n", d);
+    assert(fabs(d - 1.732) < 0.01);
+
+    d = dot_prod(&a, &b);
+    // printf("Dot product: %f\n", d);
+    assert(fabs(d - 6.f) < 0.01);
+
+    vec_3d c = vector_prod(&a, &b);
+    // printf("Vector product ");
+    // printf("%s", print_vector(&c, "c"));
+    assert(fabs(c.x - (-1)) < 0.01);
+    assert(fabs(c.y - (2)) < 0.01);
+    assert(fabs(c.z - (-1)) < 0.01);
+}
+
+/**
+ * @brief Main function
+ *
+ * @return 0 on exit
+ */
+int main(void)
+{
+    test();
+
+    return 0;
+}
