diff --git a/.github/workflows/build-test.yml b/.github/workflows/build-test.yml index baa4a425..8956c822 100644 --- a/.github/workflows/build-test.yml +++ b/.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 diff --git a/electrostatic-sandbox-framework/electrostatic-core/src/include/electrostatic/electronetsoft/algorithm/arithmos/vectorspaces/vector3d/vector3d.h b/electrostatic-sandbox-framework/electrostatic-core/src/include/electrostatic/electronetsoft/algorithm/arithmos/vectorspaces/vector3d/vector3d.h index c6eca5f7..f2209a63 100644 --- a/electrostatic-sandbox-framework/electrostatic-core/src/include/electrostatic/electronetsoft/algorithm/arithmos/vectorspaces/vector3d/vector3d.h +++ b/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. * diff --git a/electrostatic-sandbox-framework/electrostatic-core/src/libs/electrostatic-primer/electronetsoft/algorithm/arithmos/vector3d/vec3d_rotate.c b/electrostatic-sandbox-framework/electrostatic-core/src/libs/electrostatic-primer/electronetsoft/algorithm/arithmos/vector3d/vec3d_rotate.c index c048b970..4c711b0b 100644 --- a/electrostatic-sandbox-framework/electrostatic-core/src/libs/electrostatic-primer/electronetsoft/algorithm/arithmos/vector3d/vec3d_rotate.c +++ b/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) { diff --git a/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/archive/fol-orientation-vectors.jpeg b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/archive/fol-orientation-vectors.jpeg new file mode 100644 index 00000000..f014708e Binary files /dev/null and b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/archive/fol-orientation-vectors.jpeg differ diff --git a/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/archive/test_3d_x_gimbal_rotation.jpeg b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/archive/test_3d_x_gimbal_rotation.jpeg new file mode 100644 index 00000000..71600f5f Binary files /dev/null and b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/archive/test_3d_x_gimbal_rotation.jpeg differ diff --git a/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/archive/test_3d_y_gimbal_rotation.jpeg b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/archive/test_3d_y_gimbal_rotation.jpeg new file mode 100644 index 00000000..dd3787e8 Binary files /dev/null and b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/archive/test_3d_y_gimbal_rotation.jpeg differ diff --git a/electrostatic-sandbox-framework/electrostatic-examples/src/hello_vector2d.c b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/hello_vector2d.c similarity index 100% rename from electrostatic-sandbox-framework/electrostatic-examples/src/hello_vector2d.c rename to electrostatic-sandbox-framework/electrostatic-examples/src/calculus/hello_vector2d.c diff --git a/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_3d_x_gimbal_rotation.c b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_3d_x_gimbal_rotation.c new file mode 100644 index 00000000..a5eeae7f --- /dev/null +++ b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_3d_x_gimbal_rotation.c @@ -0,0 +1,156 @@ +#include +#include +#include +#include +#include + +static status_code assert_gimbals(vector3d v0, vector3d v, + vec_component angle) { + // assert the angle is resettled to ZERO after + // performing an angle that introduces a gimbal lock + + // When the system reaches or Floating-point approaches + // an angle that introduces a gimbal lock (e.g., PI/2 and 3*PI/2); + // the system rotates the 2 Planar Gimbals that are orthogonal + // to the axis of rotation using the accumulated gimbal angle + // from the structure [vec3d_gimbal], and then resets this gimbal + // angle. + // + // If the gimbal angle hasn't reached the gimbal lock yet; the + // system continues and skips the gimbal rotation, however, + // accumulates the gimbal angle inside the structure + // [vec3d_gimbal]. + if (v.gimbal.x_gimbal != angle) { + return ASSERTION_FAILURE; + } + + if (v.gimbal.y_gimbal != 0) { + return ASSERTION_FAILURE; + } + + if (v.gimbal.z_gimbal != 0) { + return ASSERTION_FAILURE; + } + + // assert the position of the X Gimbal (Which should never change!) + if ((v.gimbal.orientation->element[0][0] != 1) || + (v.gimbal.orientation->element[1][0] != 0) || + (v.gimbal.orientation->element[2][0] != 0)) { + return ASSERTION_FAILURE; + } + + // assert the new position of the Y Gimbal (After performing a Pi/2 Y-gimbal rotation) + if (!(vector2d_abs(v.gimbal.orientation->element[1][1]) <= + ___ROTATION_MIN_THRESHOLD) || + (v.gimbal.orientation->element[0][1] != 0) || + (v.gimbal.orientation->element[2][1] != -1)) { + return ASSERTION_FAILURE; + } + + // assert the new position of the Z Gimbal (After performing a Pi/2 Y-gimbal rotation). + if ((vector2d_abs(v.gimbal.orientation->element[1][2]) != 1) || + !(v.gimbal.orientation->element[0][2] <= ___ROTATION_MIN_THRESHOLD) || + !(vector2d_abs(v.gimbal.orientation->element[2][2]) <= ___ROTATION_MIN_THRESHOLD)){ + return ASSERTION_FAILURE; + } + + // assert the vector itself + v.y = roundf(v.y); + v.z = roundf(v.z); + + if (PASS != vec3d_are_equal(v, (vector3d) { + .x = v0.x, + .y = roundf(v0.z), + .z = roundf(-v0.y)}, NULL)) { + return ASSERTION_FAILURE; + } + + return ASSERTION_SUCCESS; +} + +static inline int64_t execute(void **inputs) { + + vec_component x_comp[3] = {1, 0, 0}; + vec_component y_comp[3] = {0, 1, 0}; + vec_component z_comp[3] = {0, 0, 1}; + + vec_component *comps[3] = {x_comp, y_comp, z_comp}; + + matrix orientator = { + .element = comps, + .m = 3, + .n = 3 + }; + + vector3d v = { + .x = 2, + .y = 3, + .z = 4, + .gimbal = (vec3d_gimbal) { + .x_gimbal = 0, + .y_gimbal = 0, + .z_gimbal = 0, + .orientation = &orientator + } + }; + + struct vec3d_processors processors = { + }; + + fprintf(stdout, "Perform a rotation around the X-axis for the following vector with these gimbal axes.\n"); + + fprintf(stdout, "V = (%f, %f, %f)\n", v.x, v.y, v.z); + fprintf(stdout, "\n"); + + fprintf(stdout, "Vx Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][0], v.gimbal.orientation->element[1][0], v.gimbal.orientation->element[2][0]); + fprintf(stdout, "Vy Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][1], v.gimbal.orientation->element[1][1], v.gimbal.orientation->element[2][1]); + fprintf(stdout, "Vz Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][2], v.gimbal.orientation->element[1][2], v.gimbal.orientation->element[2][2]); + fprintf(stdout, "\n"); + + status_code __code = vec3d_rotate(v, VEC3_X_COMPONENT, M_PI/2, &v, &processors); + if (__code != PASS) { + fprintf(stderr, "Error: %d\n", __code); + } + + fprintf(stdout, "Vx Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][0], v.gimbal.orientation->element[1][0], v.gimbal.orientation->element[2][0]); + fprintf(stdout, "Vy Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][1], v.gimbal.orientation->element[1][1], v.gimbal.orientation->element[2][1]); + fprintf(stdout, "Vz Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][2], v.gimbal.orientation->element[1][2], v.gimbal.orientation->element[2][2]); + + fprintf(stdout, "\n"); + fprintf(stdout, "V' = (%f, %f, %f)\n", v.x, v.y, v.z); + + return assert_gimbals((vector3d) {.x = 2, .y = 3, .z = 4}, + v, 0); // the gimbal angle is resettled to ZERO after approaching an angle + // that introduces a gimbal lock + // and the system rotates the 2 other Planar gimbals (i.e., the Y and Z Gimbals). +} + +static inline status_code assert(int64_t prop0, int64_t prop1) { + return ((status_code) (prop0 == prop1) && ASSERTION_SUCCESS); +} + +static inline void on_assertion_success(unit_test *test) { + fprintf(stdout, GREEN "Gimbals asserted against Gimbal Locks!\n" RESET); +} + +static inline void on_assertion_failure(unit_test *test) { + fprintf(stderr, RED "Gimbals failed to rotate to the target!\n" RESET); +} + +int main() { + + unit_test test = { + .on_assertion_success = &on_assertion_success, + .on_assertion_failure = &on_assertion_failure, + .assert = &assert, + .execute = &execute, + .proposition = ASSERTION_SUCCESS, + }; + + status_code __code = assert_test(&test); + if (ASSERTION_SUCCESS != __code) { + fprintf(stderr, RED "Error: %d\n" RESET, __code); + } + + return 0; +} \ No newline at end of file diff --git a/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_3d_y_gimbal_rotation.c b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_3d_y_gimbal_rotation.c new file mode 100644 index 00000000..15fac664 --- /dev/null +++ b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_3d_y_gimbal_rotation.c @@ -0,0 +1,177 @@ +#include +#include +#include +#include +#include + +static status_code assert_gimbals(vector3d v0, vector3d v, + vec_component angle) { + // assert the angle is resettled to ZERO after + // performing an angle that introduces a gimbal lock + + // When the system reaches or Floating-point approaches + // an angle that introduces a gimbal lock (e.g., PI/2 and 3*PI/2); + // the system rotates the 2 Planar Gimbals that are orthogonal + // to the axis of rotation using the accumulated gimbal angle + // from the structure [vec3d_gimbal], and then resets this gimbal + // angle. + // + // If the gimbal angle hasn't reached the gimbal lock yet; the + // system continues and skips the gimbal rotation, however, + // accumulates the gimbal angle inside the structure + // [vec3d_gimbal]. + if (v.gimbal.y_gimbal != angle) { + fprintf(stdout, RED "(1) Failed to assert the Y-Gimbal Angle Rotated Orthogonally!\n" RESET); + return ASSERTION_FAILURE; + } + + fprintf(stdout, GREEN "(1) Asserted the Y-Gimbal Angle Rotated Orthogonally!\n" RESET); + + if (v.gimbal.x_gimbal != 0) { + fprintf(stdout, RED "(2) Failed to assert the X-Gimbal Angle unchanged!\n" RESET); + return ASSERTION_FAILURE; + } + + fprintf(stdout, GREEN "(2) Asserted the X-Gimbal Angle unchanged!\n" RESET); + + if (v.gimbal.z_gimbal != 0) { + fprintf(stdout, RED "(3) Failed to assert the Z-Gimbal Angle unchanged!\n" RESET); + return ASSERTION_FAILURE; + } + + fprintf(stdout, GREEN "(3) Asserted the Z-Gimbal Angle unchanged!\n" RESET); + + // assert the position of the Y Gimbal (Which should never change!) + if ((v.gimbal.orientation->element[0][1] != 0) || + (v.gimbal.orientation->element[1][1] != 1) || + (v.gimbal.orientation->element[2][1] != 0)) { + fprintf(stdout, RED "(4) Failed to assert the Y-Gimbal orientation unchanged!\n" RESET); + return ASSERTION_FAILURE; + } + + fprintf(stdout, GREEN "(4) Asserted the Y-Gimbal orientation unchanged!\n" RESET); + + // assert the new position of the X Gimbal (After performing a Pi/2 Y-gimbal rotation) + if (!(vector2d_abs(v.gimbal.orientation->element[0][0]) <= + ___ROTATION_MIN_THRESHOLD) || + (v.gimbal.orientation->element[0][1] != 0) || + (v.gimbal.orientation->element[2][0] != 1)) { + fprintf(stdout, RED "(5) Failed to assert the new X-Gimbal orientation!\n" RESET); + return ASSERTION_FAILURE; + } + + fprintf(stdout, GREEN "(5) Asserted the new X-Gimbal orientation!\n" RESET); + + // assert the new position of the Z Gimbal (After performing a Pi/2 Y-gimbal rotation). + if ((vector2d_abs(v.gimbal.orientation->element[0][2]) != 1) || + !(v.gimbal.orientation->element[1][2] <= ___ROTATION_MIN_THRESHOLD) || + !(vector2d_abs(v.gimbal.orientation->element[2][2]) <= ___ROTATION_MIN_THRESHOLD)){ + fprintf(stdout, RED "(6) Failed to assert the new Z-Gimbal orientation!\n" RESET); + return ASSERTION_FAILURE; + } + + fprintf(stdout, GREEN "(6) Asserted the new Z-Gimbal orientation!\n" RESET); + + // assert the vector itself + v.x = roundf(v.x); + v.z = roundf(v.z); + + if (PASS != vec3d_are_equal(v, (vector3d) { + .x = roundf(-v0.z), + .y = v0.y, + .z = roundf(v0.x)}, NULL)) { + fprintf(stdout, RED "(7) Failed to assert the position of the new vector!\n" RESET); + return ASSERTION_FAILURE; + } + + fprintf(stdout, GREEN "(7) Asserted the position of the new vector!\n" RESET); + + return ASSERTION_SUCCESS; +} + +static inline int64_t execute(void **inputs) { + + vec_component x_comp[3] = {1, 0, 0}; + vec_component y_comp[3] = {0, 1, 0}; + vec_component z_comp[3] = {0, 0, 1}; + + vec_component *comps[3] = {x_comp, y_comp, z_comp}; + + matrix orientator = { + .element = comps, + .m = 3, + .n = 3 + }; + + vector3d v = { + .x = 2, + .y = 3, + .z = 4, + .gimbal = (vec3d_gimbal) { + .x_gimbal = 0, + .y_gimbal = 0, + .z_gimbal = 0, + .orientation = &orientator + } + }; + + struct vec3d_processors processors = { + }; + + fprintf(stdout, "Perform a rotation around the Y-axis for the following vector with these gimbal axes.\n"); + + fprintf(stdout, "V = (%f, %f, %f)\n", v.x, v.y, v.z); + fprintf(stdout, "\n"); + + fprintf(stdout, "Vx Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][0], v.gimbal.orientation->element[1][0], v.gimbal.orientation->element[2][0]); + fprintf(stdout, "Vy Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][1], v.gimbal.orientation->element[1][1], v.gimbal.orientation->element[2][1]); + fprintf(stdout, "Vz Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][2], v.gimbal.orientation->element[1][2], v.gimbal.orientation->element[2][2]); + fprintf(stdout, "\n"); + + status_code __code = vec3d_rotate(v, VEC3_Y_COMPONENT, M_PI/2, &v, &processors); + if (__code != PASS) { + fprintf(stderr, "Error: %d\n", __code); + } + + fprintf(stdout, "Vx Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][0], v.gimbal.orientation->element[1][0], v.gimbal.orientation->element[2][0]); + fprintf(stdout, "Vy Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][1], v.gimbal.orientation->element[1][1], v.gimbal.orientation->element[2][1]); + fprintf(stdout, "Vz Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][2], v.gimbal.orientation->element[1][2], v.gimbal.orientation->element[2][2]); + + fprintf(stdout, "\n"); + fprintf(stdout, "V' = (%f, %f, %f)\n", v.x, v.y, v.z); + + return assert_gimbals((vector3d) {.x = 2, .y = 3, .z = 4}, + v, 0); // the gimbal angle is resettled to ZERO after approaching an angle + // that introduces a gimbal lock + // and the system rotates the 2 other Planar gimbals (i.e., the X and Z Gimbals). +} + +static inline status_code assert(int64_t prop0, int64_t prop1) { + return ((status_code) (prop0 == prop1) && ASSERTION_SUCCESS); +} + +static inline void on_assertion_success(unit_test *test) { + fprintf(stdout, GREEN "Gimbals asserted against Gimbal Locks!\n" RESET); +} + +static inline void on_assertion_failure(unit_test *test) { + fprintf(stderr, RED "Gimbals failed to rotate to the target!\n" RESET); +} + +int main() { + + unit_test test = { + .on_assertion_success = &on_assertion_success, + .on_assertion_failure = &on_assertion_failure, + .assert = &assert, + .execute = &execute, + .proposition = ASSERTION_SUCCESS, + }; + + status_code __code = assert_test(&test); + if (ASSERTION_SUCCESS != __code) { + fprintf(stderr, RED "Error: %d\n" RESET, __code); + } + + return 0; +} \ No newline at end of file diff --git a/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_3d_z_gimbal_rotation.c b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_3d_z_gimbal_rotation.c new file mode 100644 index 00000000..1ba5c626 --- /dev/null +++ b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_3d_z_gimbal_rotation.c @@ -0,0 +1,177 @@ +#include +#include +#include +#include +#include + +static status_code assert_gimbals(vector3d v0, vector3d v, + vec_component angle) { + // assert the angle is resettled to ZERO after + // performing an angle that introduces a gimbal lock + + // When the system reaches or Floating-point approaches + // an angle that introduces a gimbal lock (e.g., PI/2 and 3*PI/2); + // the system rotates the 2 Planar Gimbals that are orthogonal + // to the axis of rotation using the accumulated gimbal angle + // from the structure [vec3d_gimbal], and then resets this gimbal + // angle. + // + // If the gimbal angle hasn't reached the gimbal lock yet; the + // system continues and skips the gimbal rotation, however, + // accumulates the gimbal angle inside the structure + // [vec3d_gimbal]. + if (v.gimbal.z_gimbal != angle) { + fprintf(stdout, RED "(1) Failed to assert the Z-Gimbal Angle Rotated Orthogonally!\n" RESET); + return ASSERTION_FAILURE; + } + + fprintf(stdout, GREEN "(1) Asserted Z-Gimbal Angle Rotated Orthogonally!\n" RESET); + + if (v.gimbal.x_gimbal != 0) { + fprintf(stdout, RED "(2) Failed to assert the X-Gimbal angle unchanged!\n" RESET); + return ASSERTION_FAILURE; + } + + fprintf(stdout, GREEN "(2) Asserted the X-Gimbal angle unchanged!\n" RESET); + + if (v.gimbal.y_gimbal != 0) { + fprintf(stdout, RED "(3) Failed to assert the Y-Gimbal angle unchanged!\n" RESET); + return ASSERTION_FAILURE; + } + + fprintf(stdout, GREEN "(3) Asserted the Y-Gimbal angle unchanged!\n" RESET); + + // assert the position of the Z Gimbal (Which should never change!) + if ((v.gimbal.orientation->element[2][0] != 0) || + (v.gimbal.orientation->element[2][1] != 0) || + (v.gimbal.orientation->element[2][2] != 1)) { + fprintf(stdout, RED "(4) Failed to assert the Z-Gimbal orientation vector unchanged!\n" RESET); + return ASSERTION_FAILURE; + } + + fprintf(stdout, GREEN "(4) Asserted the Z-Gimbal orientation vector unchanged!\n" RESET); + + // assert the new position of the X Gimbal (After performing a Pi/2 Y-gimbal rotation) + if (!(vector2d_abs(v.gimbal.orientation->element[0][0]) <= + ___ROTATION_MIN_THRESHOLD) || + (v.gimbal.orientation->element[1][0] != 1) || + (v.gimbal.orientation->element[2][0] != 0)) { + fprintf(stdout, RED "(5) Failed to assert the new X-Gimbal orientation vector!\n" RESET); + return ASSERTION_FAILURE; + } + + fprintf(stdout, GREEN "(5) Asserted the new X-Gimbal orientation vector!\n" RESET); + + // assert the new position of the Y Gimbal (After performing a Pi/2 Y-gimbal rotation). + if ((vector2d_abs(v.gimbal.orientation->element[0][1]) != 1) || + !(vector2d_abs(v.gimbal.orientation->element[1][1] <= ___ROTATION_MIN_THRESHOLD)) || + !(v.gimbal.orientation->element[2][1] <= ___ROTATION_MIN_THRESHOLD)) { + fprintf(stdout, RED "(6) Failed to assert the new X-Gimbal orientation vector!\n" RESET); + return ASSERTION_FAILURE; + } + + fprintf(stdout, GREEN "(6) Asserted the new X-Gimbal orientation vector!\n" RESET); + + // assert the vector itself + v.x = roundf(v.x); + v.y = roundf(v.y); + + if (PASS != vec3d_are_equal(v, (vector3d) { + .x = roundf(-v0.y), + .y = roundf(v0.x), + .z = v0.z}, NULL)) { + fprintf(stdout, RED "(7) Failed to assert the position of the new vector!\n" RESET); + return ASSERTION_FAILURE; + } + + fprintf(stdout, GREEN "(7) Asserted the position of the new vector!\n" RESET); + + return ASSERTION_SUCCESS; +} + +static inline int64_t execute(void **inputs) { + + vec_component x_comp[3] = {1, 0, 0}; + vec_component y_comp[3] = {0, 1, 0}; + vec_component z_comp[3] = {0, 0, 1}; + + vec_component *comps[3] = {x_comp, y_comp, z_comp}; + + matrix orientator = { + .element = comps, + .m = 3, + .n = 3 + }; + + vector3d v = { + .x = 2, + .y = 3, + .z = 4, + .gimbal = (vec3d_gimbal) { + .x_gimbal = 0, + .y_gimbal = 0, + .z_gimbal = 0, + .orientation = &orientator + } + }; + + struct vec3d_processors processors = { + }; + + fprintf(stdout, "Perform a rotation around the Z-axis for the following vector with these gimbal axes.\n"); + + fprintf(stdout, "V = (%f, %f, %f)\n", v.x, v.y, v.z); + fprintf(stdout, "\n"); + + fprintf(stdout, "Vx Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][0], v.gimbal.orientation->element[1][0], v.gimbal.orientation->element[2][0]); + fprintf(stdout, "Vy Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][1], v.gimbal.orientation->element[1][1], v.gimbal.orientation->element[2][1]); + fprintf(stdout, "Vz Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][2], v.gimbal.orientation->element[1][2], v.gimbal.orientation->element[2][2]); + fprintf(stdout, "\n"); + + status_code __code = vec3d_rotate(v, VEC3_Z_COMPONENT, M_PI/2, &v, &processors); + if (__code != PASS) { + fprintf(stderr, "Error: %d\n", __code); + } + + fprintf(stdout, "Vx Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][0], v.gimbal.orientation->element[1][0], v.gimbal.orientation->element[2][0]); + fprintf(stdout, "Vy Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][1], v.gimbal.orientation->element[1][1], v.gimbal.orientation->element[2][1]); + fprintf(stdout, "Vz Orientation = (%f, %f, %f)\n", v.gimbal.orientation->element[0][2], v.gimbal.orientation->element[1][2], v.gimbal.orientation->element[2][2]); + + fprintf(stdout, "\n"); + fprintf(stdout, "V' = (%f, %f, %f)\n", v.x, v.y, v.z); + + return assert_gimbals((vector3d) {.x = 2, .y = 3, .z = 4}, + v, 0); // the gimbal angle is resettled to ZERO after approaching an angle + // that introduces a gimbal lock + // and the system rotates the 2 other Planar gimbals (i.e., the X and Y Gimbals). +} + +static inline status_code assert(int64_t prop0, int64_t prop1) { + return ((status_code) (prop0 == prop1) && ASSERTION_SUCCESS); +} + +static inline void on_assertion_success(unit_test *test) { + fprintf(stdout, GREEN "Gimbals asserted against Gimbal Locks!\n" RESET); +} + +static inline void on_assertion_failure(unit_test *test) { + fprintf(stderr, RED "Gimbals failed to rotate to the target!\n" RESET); +} + +int main() { + + unit_test test = { + .on_assertion_success = &on_assertion_success, + .on_assertion_failure = &on_assertion_failure, + .assert = &assert, + .execute = &execute, + .proposition = ASSERTION_SUCCESS, + }; + + status_code __code = assert_test(&test); + if (ASSERTION_SUCCESS != __code) { + fprintf(stderr, RED "Error: %d\n" RESET, __code); + } + + return 0; +} \ No newline at end of file diff --git a/electrostatic-sandbox-framework/electrostatic-examples/src/test_mat_product.c b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_mat_product.c similarity index 100% rename from electrostatic-sandbox-framework/electrostatic-examples/src/test_mat_product.c rename to electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_mat_product.c diff --git a/electrostatic-sandbox-framework/electrostatic-examples/src/test_matrix_addition.c b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_matrix_addition.c similarity index 100% rename from electrostatic-sandbox-framework/electrostatic-examples/src/test_matrix_addition.c rename to electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_matrix_addition.c diff --git a/electrostatic-sandbox-framework/electrostatic-examples/src/test_matrix_iteration.c b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_matrix_iteration.c similarity index 100% rename from electrostatic-sandbox-framework/electrostatic-examples/src/test_matrix_iteration.c rename to electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_matrix_iteration.c diff --git a/electrostatic-sandbox-framework/electrostatic-examples/src/test_matrix_subtraction.c b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_matrix_subtraction.c similarity index 100% rename from electrostatic-sandbox-framework/electrostatic-examples/src/test_matrix_subtraction.c rename to electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_matrix_subtraction.c diff --git a/electrostatic-sandbox-framework/electrostatic-examples/src/test_vec2d_rotation.c b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_vec2d_rotation.c similarity index 100% rename from electrostatic-sandbox-framework/electrostatic-examples/src/test_vec2d_rotation.c rename to electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_vec2d_rotation.c diff --git a/electrostatic-sandbox-framework/electrostatic-examples/src/test_vec3d_rotation.c b/electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_vec3d_rotation.c similarity index 100% rename from electrostatic-sandbox-framework/electrostatic-examples/src/test_vec3d_rotation.c rename to electrostatic-sandbox-framework/electrostatic-examples/src/calculus/test_vec3d_rotation.c