Matrix Rotation Library

sdk/core/src/include/electronetsoft/arithmos/vectorspaces/gimbal_system.h

@@ -0,0 +1,37 @@
+#ifndef _GIMBAL_SYS_H_
+#define _GIMBAL_SYS_H_
+
+#include <math.h>
+#include <electronetsoft/arithmos/vectorspaces/matrix/matrix.h>
+#include <electronetsoft/arithmos/vectorspaces/vector3d/vector3d.h>
+#include <electronetsoft/util/types.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct rotation_metadata {
+    matrix *in_orientation;
+    matrix *out_orientation;
+};
+
+void preprocess_orientator(vector3d *v, vector3d *axis);
+
+status_code preprocess_mat3_orientator(vec3d_gimbal, vector3d *);
+
+status_code init_rotator_gimbal(vector3d axis, matrix *__rotator,
+                                              vec_component angle,
+                                              vec_component *angle1,
+                                              vec3d_gimbal *gimbal);
+
+status_code rotate_gimbal(vector3d axis, vec_component angle1,
+                                        matrix *__rotator,
+                                        vec3d_gimbal *in_gimbal,
+                                        vec3d_gimbal *out_gimbal,
+                                        vec3d_processors *procs);
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif

sdk/core/src/include/electronetsoft/arithmos/vectorspaces/matrix/matrix3.h

@@ -0,0 +1,31 @@
+#ifndef _MAT_3D_H_
+#define _MAT_3D_H_
+
+#include <math.h>
+#include <electronetsoft/arithmos/vectorspaces/matrix/matrix.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct mat3_gimbal {
+    matrix mat3d;
+    vec3d_gimbal gimbal3d;
+};
+
+struct mat3_processors {
+    mat_processors processors;
+    void (*on_gimbal_lock_trap)(mat3_gimbal rotated,
+            vector_gimbal gimbal,
+            vec_component angle);
+};
+
+status_code mat3_translate(matrix, matrix, matrix *, mat_processors);
+status_code mat3_rotate(mat3_gimbal m, vector3d axis,
+                        mat3_gimbal *out, vec_component angle,
+                        mat3_processors proc);
+#ifdef __cplusplus
+};
+#endif
+
+#endif

sdk/core/src/include/electronetsoft/arithmos_calculus.h

@@ -1,3 +1,4 @@
 #include <electronetsoft/arithmos/vectorspaces/matrix/matrix.h>
+#include <electronetsoft/arithmos/vectorspaces/matrix/matrix3.h>
 #include <electronetsoft/arithmos/vectorspaces/vector2d/vector2d.h>
 #include <electronetsoft/arithmos/vectorspaces/vector3d/vector3d.h>

sdk/core/src/include/electronetsoft/util/types.h

@@ -114,6 +114,8 @@ typedef enum mat_iterator {
     ROW_CONVENTION_ITERATOR = ((INT32_MAX >> 16) ^ INT32_MAX),
     COLUMN_CONVENTION_ITERATOR = ROW_CONVENTION_ITERATOR - 1,
 } mat_iterator;
+typedef struct mat3_gimbal (mat3_gimbal);
+typedef struct mat3_processors (mat3_processors);
 
 typedef struct caller_graph (caller_graph);
 

sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/init_rotator_gimbal.c

@@ -0,0 +1,72 @@
+#include <electronetsoft/arithmos/vectorspaces/gimbal_system.h>
+
+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
+        // Let the X-axis in the R(3) space be the X-axis in the 2D projection.
+        // Let the Y-axis in the R(3) space be the Y-axis in the 2D projection.
+
+        // work in XY plane
+        __rotator->element[0][0] = vector2d_cos(angle);
+        __rotator->element[0][1] = -vector2d_sin(angle);
+        __rotator->element[1][0] = vector2d_sin(angle);
+        __rotator->element[1][1] = vector2d_cos(angle);
+        __rotator->element[2][2] = 1;
+
+        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
+        // pre-processing automata
+        // Let the X-axis in the R(3) space be the X-axis in the 2D projection.
+        // Let the Z-axis in the R(3) space be the Y-axis in the 2D projection.
+
+        // work in XZ plane
+        __rotator->element[0][0] = vector2d_cos(angle);
+        __rotator->element[0][2] = -vector2d_sin(angle);
+        __rotator->element[2][0] = vector2d_sin(angle);
+        __rotator->element[2][2] = vector2d_cos(angle);
+        __rotator->element[1][1] = 1;
+
+        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
+        // pre-processing automata
+        // Let the Z-axis in the R(3) space be the X-axis in the 2D projection.
+        // Let the Y-axis in the R(3) space be the Y-axis in the 2D projection.
+
+        // work in ZY plane
+        __rotator->element[2][2] = vector2d_cos(angle);
+        __rotator->element[2][1] = -vector2d_sin(angle);
+        __rotator->element[1][2] = vector2d_sin(angle);
+        __rotator->element[1][1] = vector2d_cos(angle);
+        __rotator->element[0][0] = 1;
+
+        if (NULL != gimbal && NULL != angle1) {
+            gimbal->x_gimbal += angle;
+            *angle1 = gimbal->x_gimbal;
+        }
+
+    } else {
+        return EINCOMPATTYPE;
+    }
+
+    return PASS;
+}

sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/preprocess_mat3_orientator.c

@@ -0,0 +1,20 @@
+#include <electronetsoft/arithmos/vectorspaces/vector3d/vector3d.h>
+#include <electronetsoft/arithmos/vectorspaces/gimbal_system.h>
+
+status_code preprocess_mat3_orientator(vec3d_gimbal gimbal, vector3d *axis) {
+    if (NULL == axis ||
+         NULL == gimbal.orientation ||
+         NULL == gimbal.orientation->element) {
+        return EUNDEFINEDBUFFER;
+    }
+
+    vector3d v3 = {
+        .gimbal = (vec3d_gimbal) {
+            .orientation = gimbal.orientation
+        }
+    };
+
+    preprocess_orientator(&v3, axis);
+
+    return PASS;
+}

sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/preprocess_orientator.c

@@ -0,0 +1,35 @@
+#include <electronetsoft/arithmos/vectorspaces/gimbal_system.h>
+
+void preprocess_orientator(vector3d *v, vector3d *axis) {
+
+    // create a column vector matrix
+    vec_component axis_x[1] = {axis->x,};
+    vec_component axis_y[1] = {axis->y,};
+    vec_component axis_z[1] = {axis->z,};
+    vec_component *axis_comps[3] = {axis_x, axis_y, axis_z};
+    matrix __axis = {
+            .element = axis_comps,
+            .m = 3,
+            .n = 1
+    };
+
+    // find the position of the orientation vectors
+    vec_component _axis_x[1] = {0,};
+    vec_component _axis_y[1] = {0,};
+    vec_component _axis_z[1] = {0,};
+    vec_component *_axis_comps[3] = {_axis_x, _axis_y, _axis_z};
+    matrix ___axis = {
+            .element = _axis_comps,
+            .m = 3,
+            .n = 1
+    };
+
+    mat_processors proc = {
+    };
+
+    mat_product(*(v->gimbal.orientation), __axis, &___axis, proc);
+
+    axis->x = *(___axis.element[0]);
+    axis->y = *(___axis.element[1]);
+    axis->z = *(___axis.element[2]);
+}

sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/rotate_gimbal.c

@@ -0,0 +1,80 @@
+#include <electronetsoft/arithmos/vectorspaces/gimbal_system.h>
+
+static inline status_code __on_entry_iterated(mat_proc_sig proc_sig) {
+    struct rotation_metadata *metadata = proc_sig.metadata;
+    matrix *in_orientation = metadata->in_orientation;
+    matrix *out_orientation = metadata->out_orientation;
+    uint64_t row_index = proc_sig.row_index;
+    uint64_t col_index = proc_sig.col_index;
+
+    out_orientation->element[row_index][col_index] =
+            in_orientation->element[row_index][col_index];
+
+    return PASS;
+}
+
+status_code rotate_gimbal(vector3d axis, vec_component angle1,
+                          matrix *__rotator,
+                          vec3d_gimbal *in_gimbal,
+                          vec3d_gimbal *out_gimbal,
+                          vec3d_processors *procs) {
+
+    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
+    vec_component _orient_x[3] = {0, 0, 0,};
+    vec_component _orient_y[3] = {0, 0, 0,};
+    vec_component _orient_z[3] = {0, 0, 0,};
+    vec_component *_orient_comps[3] = {_orient_x, _orient_y, _orient_z};
+    matrix __orient = {
+            .element = _orient_comps,
+            .m = 3,
+            .n = 3
+    };
+
+    mat_processors mat_procs = {
+    };
+
+    __code = mat_product(*__rotator,
+                         *(in_gimbal->orientation),
+                         &__orient, mat_procs);
+    if (PASS != __code) {
+        if (NULL != procs && NULL != procs->on_op_failed) {
+            procs->on_op_failed(&vec3d_rotate, __code);
+        }
+        return __code;
+    }
+
+    struct rotation_metadata metadata = {
+            .in_orientation = &__orient,
+            .out_orientation = out_gimbal->orientation
+    };
+
+    mat_procs.on_entry_iterated = &__on_entry_iterated;
+    mat_procs.metadata = &metadata;
+
+    __code = mat_iterate_elements(__orient, ROW_CONVENTION_ITERATOR, mat_procs);
+    if (PASS != __code) {
+        if (NULL != procs && NULL != procs->on_op_failed) {
+            procs->on_op_failed(&vec3d_rotate, __code);
+        }
+        return __code;
+    }
+
+    if (get_vec_gimbal(axis) == GIMBAL_Z) {
+        out_gimbal->z_gimbal = 0;
+    } else if (get_vec_gimbal(axis) == GIMBAL_Y) {
+        out_gimbal->y_gimbal = 0;
+    } else {
+        out_gimbal->x_gimbal = 0;
+    }
+
+    return PASS;
+}