arithmos/vector3d: added Vector3d Calculus Unit tests and library optimizations

.github/workflows/build-test.yml

@@ -85,6 +85,30 @@ jobs:
         - name: Testing "hello_switching_mux.c"
           run: sudo ./helper-scripts/ci-cd/test-electrostatic.sh "hello_switching_mux.c" "hello-switching-mux"
 
+        - name: Testing Arithmos Calculus (2D Vector Rotation)
+          run: sudo ./helper-scripts/ci-cd/test-electrostatic.sh "calculus/test_vec2d_rotation.c" "test_2d_rotation"
+
+        - name: Testing Arithmos Calculus (3D Gimbal Rotation Sequence)
+          run: sudo ./helper-scripts/ci-cd/test-electrostatic.sh "calculus/test_vec3d_rotation.c" "test_3d_rotation"
+
+        - name: Testing Arithmos Calculus (3D Gimbal Rotation around X-axis)
+          run: sudo ./helper-scripts/ci-cd/test-electrostatic.sh "calculus/test_3d_x_gimbal_rotation.c" "test_3d_x_gimbal_rotation"
+
+        - name: Testing Arithmos Calculus (3D Gimbal Rotation around Y-axis)
+          run: sudo ./helper-scripts/ci-cd/test-electrostatic.sh "calculus/test_3d_y_gimbal_rotation.c" "test_3d_y_gimbal_rotation"
+
+        - name: Testing Arithmos Calculus (3D Gimbal Rotation around Z-axis)
+          run: sudo ./helper-scripts/ci-cd/test-electrostatic.sh "calculus/test_3d_z_gimbal_rotation.c" "test_3d_z_gimbal_rotation"
+
+        - name: Testing Arithmos Calculus (Matrix Algebra Operations -- mat_product)
+          run: sudo ./helper-scripts/ci-cd/test-electrostatic.sh "calculus/test_mat_product.c" "test_mat_product"
+
+        - name: Testing Arithmos Calculus (Matrix Algebra Operations -- mat_add)
+          run: sudo ./helper-scripts/ci-cd/test-electrostatic.sh "calculus/test_matrix_addition.c" "test_mat_addition"
+
+        - name: Testing Arithmos Calculus (Matrix Algebra Operations -- mat_iterate_elements)
+          run: sudo ./helper-scripts/ci-cd/test-electrostatic.sh "calculus/test_matrix_iteration.c" "test_mat_traversal"
+
         - name: Compiling electrostatic4j Java Binding API
           run: sudo ./helper-scripts/ci-cd/compile-e4j.sh
 

electrostatic-sandbox-framework/electrostatic-core/src/include/electrostatic/electronetsoft/algorithm/arithmos/vectorspaces/vector3d/vector3d.h

@@ -58,7 +58,28 @@ struct vec3d_polar {
     vec_component theta; // angle with +ve direction of xy plane
 };
 
+/**
+ * @brief Defines the encodings for a set of three gimbal axes.
+ * @note Those encodings are constant numbers; the don't swap orientation
+ * with one another.
+ */
+typedef enum {
+    GIMBAL_X = (INT16_MAX >> 8) ^ INT16_MAX,
+    GIMBAL_Y = GIMBAL_X - 1,
+    GIMBAL_Z = GIMBAL_Y - 1
+} vector_gimbal;
+
 struct vec3d_processors {
+    /**
+     * @brief A function pointer to be called on a gimbal lock trap. A gimbal trap is a
+     * software trap that is executed when a potential angle that could produce gimbal
+     * lock is being floating-point approached or reached.
+     * @param rotated the rotated vector in the R(3) vectorspace.
+     * @param gimbal the gimbal around which the rotational motion is being executed.
+     * @param angle the last angle that triggered this gimbal trap
+     *              (it shouldn't have to be PI/2 or -PI/2).
+     */
+    void (*on_gimbal_lock_trap)(vector3d rotated, vector_gimbal gimbal, vec_component angle);
     /**
      * @brief A function pointer to be called on a successful operation.
      * @param caller A void pointer to the original calling context or object.
@@ -79,6 +100,26 @@ extern vector3d VEC3_Y_COMPONENT;
 
 extern vector3d VEC3_Z_COMPONENT;
 
+static inline vector_gimbal get_vec_gimbal(vector3d axis) {
+    if ((axis.x > ___ROTATION_MIN_THRESHOLD) &&
+        ((axis.y >= 0) && (axis.y < 1)) &&
+        ((axis.z >= 0) && (axis.z < 1))) {
+
+        return GIMBAL_X;
+    } else if ((axis.y > ___ROTATION_MIN_THRESHOLD) &&
+               ((axis.x >= 0) && (axis.x < 1)) &&
+               ((axis.z >= 0) && (axis.z < 1))) {
+
+        return GIMBAL_Y;
+    } else if ((axis.z > ___ROTATION_MIN_THRESHOLD) &&
+               ((axis.y >= 0) && (axis.y < 1)) &&
+               ((axis.x >= 0) && (axis.x < 1))) {
+
+        return GIMBAL_Z;
+    }
+    return -1;
+}
+
 /**
  * @brief Adds a scalar value to the vector components and returns a new vector.
  *

electrostatic-sandbox-framework/electrostatic-core/src/libs/electrostatic-primer/electronetsoft/algorithm/arithmos/vector3d/vec3d_rotate.c

@@ -7,12 +7,6 @@ struct rotation_metadata {
     matrix *out_orientation;
 };
 
-typedef enum {
-    GIMBAL_X = (INT16_MAX >> 8) ^ INT16_MAX,
-    GIMBAL_Y = GIMBAL_X - 1,
-    GIMBAL_Z = GIMBAL_Y - 1
-} vector_gimbal;
-
 static inline void preprocess_orientator(vector3d *v, vector3d *axis) {
 
     // create a column vector matrix
@@ -60,30 +54,16 @@ static inline status_code __on_entry_iterated(mat_proc_sig proc_sig) {
     return PASS;
 }
 
-static inline vector_gimbal get_vec_gimbal(vector3d *axis) {
-    if ((axis->x > ___ROTATION_MIN_THRESHOLD) &&
-        ((axis->y >= 0) && (axis->y < 1)) &&
-        ((axis->z >= 0) && (axis->z < 1))) {
-
-        return GIMBAL_X;
-    } else if ((axis->y > ___ROTATION_MIN_THRESHOLD) &&
-                ((axis->x >= 0) && (axis->x < 1)) &&
-                ((axis->z >= 0) && (axis->z < 1))) {
-
-        return GIMBAL_Y;
-    } else if ((axis->z > ___ROTATION_MIN_THRESHOLD) &&
-                ((axis->y >= 0) && (axis->y < 1)) &&
-                ((axis->x >= 0) && (axis->x < 1))) {
-
-        return GIMBAL_Z;
-    }
-    return -1;
-}
-
-static inline status_code init_rotator_gimbal(vector3d *axis, matrix *__rotator,
+static inline status_code init_rotator_gimbal(vector3d axis,
+                                              matrix *__rotator,
                                               vec_component angle,
                                               vec_component *angle1,
                                               vec3d_gimbal *gimbal) {
+
+    if (NULL == __rotator || NULL == __rotator->element) {
+        return EUNDEFINEDBUFFER;
+    }
+
     if (get_vec_gimbal(axis) == GIMBAL_Z) {
         // rotate around z-axis
         // pre-processing automata
@@ -97,8 +77,10 @@ static inline status_code init_rotator_gimbal(vector3d *axis, matrix *__rotator,
         __rotator->element[1][1] = vector2d_cos(angle);
         __rotator->element[2][2] = 1;
 
-        gimbal->z_gimbal += angle;
-        *angle1 = gimbal->z_gimbal;
+        if (NULL != gimbal && NULL != angle1) {
+            gimbal->z_gimbal += angle;
+            *angle1 = gimbal->z_gimbal;
+        }
 
     } else if (get_vec_gimbal(axis) == GIMBAL_Y) {
         // rotate around y-axis
@@ -113,8 +95,11 @@ static inline status_code init_rotator_gimbal(vector3d *axis, matrix *__rotator,
         __rotator->element[2][2] = vector2d_cos(angle);
         __rotator->element[1][1] = 1;
 
-        gimbal->y_gimbal += angle;
-        *angle1 = gimbal->y_gimbal;
+        if (NULL != gimbal && NULL != angle1) {
+            gimbal->y_gimbal += angle;
+            *angle1 = gimbal->y_gimbal;
+        }
+
     } else if (get_vec_gimbal(axis) == GIMBAL_X) {
 
         // rotate around x-axis
@@ -129,8 +114,11 @@ static inline status_code init_rotator_gimbal(vector3d *axis, matrix *__rotator,
         __rotator->element[1][1] = vector2d_cos(angle);
         __rotator->element[0][0] = 1;
 
-        gimbal->x_gimbal += angle;
-        *angle1 = gimbal->x_gimbal;
+        if (NULL != gimbal && NULL != angle1) {
+            gimbal->x_gimbal += angle;
+            *angle1 = gimbal->x_gimbal;
+        }
+
     } else {
         return EINCOMPATTYPE;
     }
@@ -163,31 +151,19 @@ static inline status_code rotate_about_gimbal(matrix *__rotator,
     return PASS;
 }
 
-static inline status_code rotate_gimbal(vector3d *axis, vec_component angle1,
+static inline status_code rotate_gimbal(vector3d axis, vec_component angle1,
                                         matrix *__rotator,
                                         vec3d_gimbal *in_gimbal,
                                         vec3d_gimbal *out_gimbal,
                                         vec3d_processors *procs) {
-    if (get_vec_gimbal(axis) == GIMBAL_Z) {
-        __rotator->element[0][0] = vector2d_cos(angle1);
-        __rotator->element[0][1] = -vector2d_sin(angle1);
-        __rotator->element[1][0] = vector2d_sin(angle1);
-        __rotator->element[1][1] = vector2d_cos(angle1);
-        __rotator->element[2][2] = 1;
-    } else if (get_vec_gimbal(axis) == GIMBAL_Y) {
-        __rotator->element[0][0] = vector2d_cos(angle1);
-        __rotator->element[0][2] = -vector2d_sin(angle1);
-        __rotator->element[2][0] = vector2d_sin(angle1);
-        __rotator->element[2][2] = vector2d_cos(angle1);
-        __rotator->element[1][1] = 1;
-    } else if (get_vec_gimbal(axis) == GIMBAL_X) {
-        __rotator->element[2][2] = vector2d_cos(angle1);
-        __rotator->element[2][1] = -vector2d_sin(angle1);
-        __rotator->element[1][2] = vector2d_sin(angle1);
-        __rotator->element[1][1] = vector2d_cos(angle1);
-        __rotator->element[0][0] = 1;
-    } else {
-        return EINCOMPATTYPE;
+
+    status_code __code = init_rotator_gimbal(axis, __rotator, angle1,
+                                             NULL, NULL);
+    if (PASS != __code) {
+        if (NULL != procs && NULL != procs->on_op_failed) {
+            procs->on_op_failed(&vec3d_rotate, __code);
+        }
+        return __code;
     }
 
     // init the output orientation matrix
@@ -204,8 +180,7 @@ static inline status_code rotate_gimbal(vector3d *axis, vec_component angle1,
     mat_processors mat_procs = {
     };
 
-    status_code __code =
-            mat_product(*__rotator,
+    __code = mat_product(*__rotator,
                         *(in_gimbal->orientation),
                         &__orient, mat_procs);
     if (PASS != __code) {
@@ -301,7 +276,7 @@ status_code vec3d_rotate(vector3d v, vector3d axis, vec_component angle,
     preprocess_orientator(&v, &axis);
     vec3d_abs(axis, &axis, NULL);
 
-    __code = init_rotator_gimbal(&axis, &__rotator, angle,
+    __code = init_rotator_gimbal(axis, &__rotator, angle,
                                  &angle1, &out->gimbal);
 
     if (PASS != __code) {
@@ -323,8 +298,11 @@ status_code vec3d_rotate(vector3d v, vector3d axis, vec_component angle,
 
     if (vector2d_abs(vector2d_cos(angle1)) <= ___ROTATION_MIN_THRESHOLD
             && vector2d_abs(vector2d_sin(angle1)) == 1) {
+        if (NULL != procs && NULL != procs->on_gimbal_lock_trap) {
+            procs->on_gimbal_lock_trap(*out, get_vec_gimbal(axis), angle);
+        }
         // rotate the gimbals axes (the orientation)
-        __code = rotate_gimbal(&axis, angle1, &__rotator,
+        __code = rotate_gimbal(axis, angle1, &__rotator,
                                &(v.gimbal),
                                &out->gimbal, procs);
         if (PASS != __code) {