diff --git a/.github/workflows/build-test.yml b/.github/workflows/build-test.yml index c80d8328..08de87a5 100644 --- a/.github/workflows/build-test.yml +++ b/.github/workflows/build-test.yml @@ -68,55 +68,55 @@ jobs: chmod +rwx ./helper-scripts/project-impl/*.sh && chmod +rwx ./helper-scripts/ci-cd/*.sh - name: Compiling electrostatic-core Native Binaries - run: sudo ./helper-scripts/ci-cd/compile-electrostatic.sh "-primer" + run: ./helper-scripts/ci-cd/compile-electrostatic.sh "-primer" - name: Testing electrostatic-core Native Binaries - run: sudo ./helper-scripts/ci-cd/test-electrostatic.sh "hello_comm.c" "hello-comm" + run: ./helper-scripts/ci-cd/test-electrostatic.sh "hello_comm.c" "hello-comm" # - name: Testing "hello_vector2d.c" # run: sudo ./helper-scripts/ci-cd/test-electrostatic.sh "hello_vector2d.c" "hello-vec2d" - name: Testing "hello_contiguous_buffer.c" - run: sudo ./helper-scripts/ci-cd/test-electrostatic.sh "hello_lists.c" "hello-listadt" + run: ./helper-scripts/ci-cd/test-electrostatic.sh "hello_lists.c" "hello-listadt" - name: Testing "hello_unittest.c" - run: sudo ./helper-scripts/ci-cd/test-electrostatic.sh "hello_unittest.c" "hello-unittest" + run: ./helper-scripts/ci-cd/test-electrostatic.sh "hello_unittest.c" "hello-unittest" - name: Testing "hello_switching_mux.c" - run: sudo ./helper-scripts/ci-cd/test-electrostatic.sh "hello_switching_mux.c" "hello-switching-mux" + run: ./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" + run: ./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" + run: ./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" + run: ./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" + run: ./helper-scripts/ci-cd/test-electrostatic.sh "calculus/test_3d_z_gimbal_rotation.c" "test_3d_z_gimbal_rotation" - name: Testing Arithmos Matrix Rotation API - run: sudo ./helper-scripts/ci-cd/test-electrostatic.sh "calculus/test_3d_mat_gimbal_rotation.c" "test_3d_mat_gimbal_rotation" + run: ./helper-scripts/ci-cd/test-electrostatic.sh "calculus/test_3d_mat_gimbal_rotation.c" "test_3d_mat_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" + run: ./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" + run: ./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" + run: ./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 + run: ./helper-scripts/ci-cd/compile-e4j.sh - name: Testing electrostatic4j Java Binding API - run: sudo ./helper-scripts/ci-cd/test-e4j.sh + run: ./helper-scripts/ci-cd/test-e4j.sh - name: Building Serial4j Framework run: sudo ./helper-scripts/ci-cd/compile-serial4j.sh diff --git a/helper-scripts/project-impl/compile-all-android.sh b/helper-scripts/project-impl/compile-all-android.sh index af21b9d1..1ce654cc 100755 --- a/helper-scripts/project-impl/compile-all-android.sh +++ b/helper-scripts/project-impl/compile-all-android.sh @@ -17,24 +17,24 @@ util_module="${electronetsoft}/util/" ./helper-scripts/project-impl/compile-electrostatic.sh \ "${COMMISSION_LIB}" "${CLANG_BIN}" "${CLANGPP_BIN}" "ON" "ON" "OFF" "-O3 -fPIC" \ - "--target=${ARM_64}" \ + "--target=${ARM_64} -D_ELECTRO_ANDROID" \ "${electrostatic_core_headers}" "${platform_module} ${comm_module} \ ${algorithm_module} ${util_module}" "${NULL}" "m;c;dl" "${source_dir}" "android" "${ARM_64}" "${POST_COMPILE_TRUE}" ./helper-scripts/project-impl/compile-electrostatic.sh \ "${COMMISSION_LIB}" "${CLANG_BIN}" "${CLANGPP_BIN}" "ON" "ON" "OFF" "-O3 -fPIC" \ - "--target=${ARM_32}" \ + "--target=${ARM_32} -D_ELECTRO_ANDROID" \ "${electrostatic_core_headers}" "${platform_module} ${comm_module} \ ${algorithm_module} ${util_module}" "${NULL}" "m;c;dl" "${source_dir}" "android" "${ARM_32}" "${POST_COMPILE_TRUE}" ./helper-scripts/project-impl/compile-electrostatic.sh \ "${COMMISSION_LIB}" "${CLANG_BIN}" "${CLANGPP_BIN}" "ON" "ON" "OFF" "-O3 -fPIC" \ - "--target=${ANDROID_x86}" \ + "--target=${ANDROID_x86} -D_ELECTRO_ANDROID" \ "${electrostatic_core_headers}" "${platform_module} ${comm_module} \ ${algorithm_module} ${util_module}" "${NULL}" "m;c;dl" "${source_dir}" "android" "${ANDROID_x86}" "${POST_COMPILE_TRUE}" ./helper-scripts/project-impl/compile-electrostatic.sh \ "${COMMISSION_LIB}" "${CLANG_BIN}" "${CLANGPP_BIN}" "ON" "ON" "OFF" "-O3 -fPIC" \ - "--target=${ANDROID_x86_64}" \ + "--target=${ANDROID_x86_64} -D_ELECTRO_ANDROID" \ "${electrostatic_core_headers}" "${platform_module} ${comm_module} \ ${algorithm_module} ${util_module}" "${NULL}" "m;c;dl" "${source_dir}" "android" "${ANDROID_x86_64}" "${POST_COMPILE_TRUE}" diff --git a/helper-scripts/project-impl/compile-all.sh b/helper-scripts/project-impl/compile-all.sh index 77baec06..3d17183e 100755 --- a/helper-scripts/project-impl/compile-all.sh +++ b/helper-scripts/project-impl/compile-all.sh @@ -20,12 +20,12 @@ electrokio_module="${electrokio}" ./helper-scripts/project-impl/compile-electrostatic.sh \ "${COMMISSION_LIB}" "${GCC_BIN_x86}" "${GPP_BIN_x86}" "ON" "ON" "OFF" "-O3 -fPIC" \ - "${TARGET_x86_64}" "${TOOLCHAIN_INCLUDES_x86};${electrostatic_core_headers}" "${platform_module} \ - ${comm_module} ${algorithm_module} ${util_module} ${electrokio_module}" "${NULL}" "m;pthread;dl" "${source_dir}" \ + "${TARGET_x86_64} -D__LINUX_x86_64" "${TOOLCHAIN_INCLUDES_x86};${electrostatic_core_headers}" "${platform_module} \ + ${comm_module} ${algorithm_module} ${util_module} ${electrokio_module}" "${NULL}" "m;pthread;dl;glfw;GL" "${source_dir}" \ "linux" "${x86_64}" "${POST_COMPILE_TRUE}" ./helper-scripts/project-impl/compile-electrostatic.sh \ "${COMMISSION_LIB}" "${GCC_BIN_x86}" "${GPP_BIN_x86}" "ON" "ON" "OFF" "-O3 -fPIC" \ - "${TARGET_x86}" "${TOOLCHAIN_INCLUDES_x86};${electrostatic_core_headers}" "${platform_module} \ + "${TARGET_x86} -D__LINUX_x86" "${TOOLCHAIN_INCLUDES_x86};${electrostatic_core_headers}" "${platform_module} \ ${comm_module} ${algorithm_module} ${util_module}" "${NULL}" "m;pthread;dl" "${source_dir}" \ "linux" "${x86}" "${POST_COMPILE_TRUE}" diff --git a/helper-scripts/project-impl/compile-examples.sh b/helper-scripts/project-impl/compile-examples.sh index 0715b6ae..edd3084e 100755 --- a/helper-scripts/project-impl/compile-examples.sh +++ b/helper-scripts/project-impl/compile-examples.sh @@ -17,7 +17,7 @@ dependencies="$(pwd)/${examples_dir}/dependencies/libs/${SYSTEM_DIR}/${BUILD_DIR compile "${EXECUTABLE}" "${GCC_BIN_x86}" "${GPP_BIN_x86}" \ "OFF" "OFF" "ON" "${INPUT_COMPILER_OPTIONS}" \ "${TARGET_MACHINE}" "${TOOLCHAIN_INCLUDES_x86};${electrostatic_core_headers}" \ - "${examples_dir}" "${sources}" "${dependencies};m;pthread;dl" \ + "${examples_dir}" "${sources}" "${dependencies};m;pthread;dl;glfw;GL" \ "${SYSTEM_DIR}/${BUILD_DIR}" "." cd "${project_root}" || exit $? diff --git a/helper-scripts/setup-environment/setup-sandbox.sh b/helper-scripts/setup-environment/setup-sandbox.sh index e01a2f2c..71b9aa4e 100755 --- a/helper-scripts/setup-environment/setup-sandbox.sh +++ b/helper-scripts/setup-environment/setup-sandbox.sh @@ -15,6 +15,8 @@ source "${setup_avr_toolchains_script}" # Downloading prerequisite binaries ## echo "Downloading missing binaries" + +sudo apt update # typically for WSL machines sudo apt-get install make sudo apt-get install usbutils @@ -24,6 +26,17 @@ sudo apt-get install pulseaudio sudo apt-get install gcc sudo apt-get install g++ +sudo apt install \ + build-essential \ + pkg-config \ + libgl1-mesa-dev \ + libglu1-mesa-dev \ + freeglut3-dev \ + libglfw3-dev \ + libglew-dev + +sudo apt update + ## # Prepare the sandbox workspace ## @@ -90,4 +103,4 @@ cleanup_ndk # create_ide_symbol source "$(pwd)/helper-scripts/setup-environment/create-symbols.sh" -source "$(pwd)/helper-scripts/setup-environment/add-user-to-serial-groups.sh" \ No newline at end of file +source "$(pwd)/helper-scripts/setup-environment/add-user-to-serial-groups.sh" diff --git a/sdk/core/src/include/electronetsoft/arithmos/automata/info.txt b/sdk/core/src/include/electronetsoft/arithmos/eca/info.txt similarity index 100% rename from sdk/core/src/include/electronetsoft/arithmos/automata/info.txt rename to sdk/core/src/include/electronetsoft/arithmos/eca/info.txt diff --git a/sdk/core/src/include/electronetsoft/arithmos/ecs/info.txt b/sdk/core/src/include/electronetsoft/arithmos/ecs/info.txt deleted file mode 100644 index a7ae814d..00000000 --- a/sdk/core/src/include/electronetsoft/arithmos/ecs/info.txt +++ /dev/null @@ -1 +0,0 @@ -Entry for Electrostatic-ES module. diff --git a/sdk/core/src/include/electronetsoft/arithmos/vectorspaces/gimbal_system.h b/sdk/core/src/include/electronetsoft/arithmos/vectorspaces/gimbal_system.h index 19901469..05a88a11 100644 --- a/sdk/core/src/include/electronetsoft/arithmos/vectorspaces/gimbal_system.h +++ b/sdk/core/src/include/electronetsoft/arithmos/vectorspaces/gimbal_system.h @@ -15,17 +15,11 @@ struct rotation_metadata { matrix *out_orientation; }; -void preprocess_orientator(vector3d *v, vector3d *axis); +status_code init_rotator_gimbal(vector3d axis, matrix orientation, + matrix *__rotator, + vec_component angle); -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, +status_code rotate_gimbal(vector3d axis, matrix __rotator, vec3d_gimbal *in_gimbal, vec3d_gimbal *out_gimbal, vec3d_processors *procs); diff --git a/sdk/core/src/include/electronetsoft/arithmos/vectorspaces/gimbal_visualizer.h b/sdk/core/src/include/electronetsoft/arithmos/vectorspaces/gimbal_visualizer.h new file mode 100644 index 00000000..d42f074b --- /dev/null +++ b/sdk/core/src/include/electronetsoft/arithmos/vectorspaces/gimbal_visualizer.h @@ -0,0 +1,34 @@ +// dissociate this file from the ElectroMIO Library +#if !(defined GIMBAL_VISUALIZER_H && (defined __LINUX_x86 || defined _ELECTRO_MIO || defined _ELECTRO_ANDROID)) + +#define GIMBAL_VISUALIZER_H + +#include +#include +#include +#include + +typedef struct { + GLFWwindow *window; + int width; + int height; + float rotation_x; + float rotation_y; + float rotation_z; + vec3d_gimbal gimbal; +} gimbal_visualizer; + +/* Initialize the OpenGL window and visualizer */ +gimbal_visualizer* gimbal_visualizer_init(int width, int height, vec3d_gimbal *gimbal); + +/* Update and Render the gimbal visualization */ +void gimbal_visualizer_render(gimbal_visualizer *viz, mat3_gimbal *gimbal); +void gimbal_visualizer_update(gimbal_visualizer *viz, mat3_gimbal *mat_gimbal); + +/* Check if the visualizer window should close */ +int gimbal_visualizer_should_close(gimbal_visualizer *viz); + +/* Cleanup and destroy the visualizer */ +void gimbal_visualizer_destroy(gimbal_visualizer *viz); + +#endif \ No newline at end of file diff --git a/sdk/core/src/include/electronetsoft/arithmos/vectorspaces/matrix/matrix3.h b/sdk/core/src/include/electronetsoft/arithmos/vectorspaces/matrix/matrix3.h index 65b4989d..4bfe5fc7 100644 --- a/sdk/core/src/include/electronetsoft/arithmos/vectorspaces/matrix/matrix3.h +++ b/sdk/core/src/include/electronetsoft/arithmos/vectorspaces/matrix/matrix3.h @@ -3,6 +3,7 @@ #include #include +#include #ifdef __cplusplus extern "C" { @@ -17,13 +18,16 @@ struct mat3_processors { mat_processors processors; void (*on_gimbal_lock_trap)(mat3_gimbal rotated, vector_gimbal gimbal, - vec_component angle); + vec_component angle, + mat_processors); }; 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); +status_code mat3_get_axis_orientation(mat3_gimbal m, vector3d axis, vector3d *out); +status_code mat3_get_vector(mat3_gimbal m, vector3d axis, vector3d *out); #ifdef __cplusplus }; #endif diff --git a/sdk/core/src/include/electronetsoft/arithmos/vectorspaces/vector3d/vector3d.h b/sdk/core/src/include/electronetsoft/arithmos/vectorspaces/vector3d/vector3d.h index 52f8aade..ed97d62f 100644 --- a/sdk/core/src/include/electronetsoft/arithmos/vectorspaces/vector3d/vector3d.h +++ b/sdk/core/src/include/electronetsoft/arithmos/vectorspaces/vector3d/vector3d.h @@ -22,6 +22,10 @@ struct vec3d_gimbal { vec_component y_gimbal; vec_component z_gimbal; + vec_component delta_x; + vec_component delta_y; + vec_component delta_z; + matrix *orientation; }; @@ -102,18 +106,18 @@ 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))) { + ((axis.y >= 0) && (axis.y < axis.x)) && + ((axis.z >= 0) && (axis.z < axis.x))) { return GIMBAL_X; } else if ((axis.y > ___ROTATION_MIN_THRESHOLD) && - ((axis.x >= 0) && (axis.x < 1)) && - ((axis.z >= 0) && (axis.z < 1))) { + ((axis.x >= 0) && (axis.x < axis.y)) && + ((axis.z >= 0) && (axis.z < axis.y))) { return GIMBAL_Y; } else if ((axis.z > ___ROTATION_MIN_THRESHOLD) && - ((axis.y >= 0) && (axis.y < 1)) && - ((axis.x >= 0) && (axis.x < 1))) { + ((axis.y >= 0) && (axis.y < axis.z)) && + ((axis.x >= 0) && (axis.x < axis.z))) { return GIMBAL_Z; } diff --git a/sdk/core/src/include/electronetsoft/util/utilities.h b/sdk/core/src/include/electronetsoft/util/utilities.h index 8ef6932c..86b4b6c5 100644 --- a/sdk/core/src/include/electronetsoft/util/utilities.h +++ b/sdk/core/src/include/electronetsoft/util/utilities.h @@ -5,8 +5,12 @@ #include #include #include +#include +#include #include +#define ___DELTA ((float) 1 * vector2d_pow(10.0f, -10.0f)) + #ifdef __cplusplus extern "C" { // disable C++ name mangling by declaring function prototypes // a C externally linked space @@ -100,6 +104,43 @@ static inline typed_pointer get_typed_pointer(void *address, pointer_type type) return pointer; } +/** + * @brief Improved spreading: uses a 64-bit Mixer (MurmurHash3 style) + * to distribute entropy across the entire 64-bit range. + */ +static inline status_code spread_64(uint64_t in, uint64_t *out) { + if (rvalue(out) == NULL) { + return EUNDEFINEDBUFFER; + } + + in = (in ^ (in >> 30)) * 0xbf58476d1ce4e5b9ULL; + in = (in ^ (in >> 27)) * 0x94d049bb133111ebULL; + in = in ^ (in >> 31); + *out = in; + + return PASS; +} + +static inline status_code hash_key(const char *key, uint64_t *hash) { + if (rvalue((void *) key) == NULL || rvalue(hash) == NULL) { + return EUNDEFINEDBUFFER; + } + + // Use a large prime constant to initialize (FNV-1a style) + uint64_t h = 0xcbf29ce484222325ULL; + + // Process every character + for (const char *address = key; *address != '\0'; address++) { + // 1. XOR the character into the hash + h ^= (uint8_t) (*address); + // 2. Multiply by a large prime (this spreads the bits) + h *= 0x100000001b3ULL; + } + + // Apply your spread_64 as a finalizer to ensure high entropy + return spread_64(h, hash); +} + #ifdef __cplusplus }; #endif diff --git a/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/gimbal_visualizer.c b/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/gimbal_visualizer.c new file mode 100644 index 00000000..ce3dc005 --- /dev/null +++ b/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/gimbal_visualizer.c @@ -0,0 +1,272 @@ +#if !(defined __LINUX_x86 || defined _ELECTRO_MIO || defined _ELECTRO_ANDROID) +#include +#include +#include +#include + +/* Draw a 3D axis with colored lines */ +static void draw_axis(float length, vector3d x_orientation, + vector3d y_orientation, + vector3d z_orientation) { + glLineWidth(2.0f); + glBegin(GL_LINES); + + /* X-axis (Red) */ + glColor3f(1.0f, 0.0f, 0.0f); + glVertex3f(- length * x_orientation.x, + - length * x_orientation.y, + - length * x_orientation.z); + glVertex3f(length * x_orientation.x, + length * x_orientation.y, + length * x_orientation.z); + + /* Y-axis (Green) */ + glColor3f(0.0f, 1.0f, 0.0f); + glVertex3f(- length * y_orientation.x, + - length * y_orientation.y, + - length * y_orientation.z); + glVertex3f(length * y_orientation.x, + length * y_orientation.y, + length * y_orientation.z); + + /* Z-axis (Blue) */ + glColor3f(0.0f, 0.0f, 1.0f); + glVertex3f(- length * z_orientation.x, + - length * z_orientation.y, + - length * z_orientation.z); + glVertex3f(length * z_orientation.x, + length * z_orientation.y, + length * z_orientation.z); + glEnd(); + glLineWidth(1.0f); +} + +/* Draw a gimbal ring (circle) */ +static void draw_gimbal_ring(float radius, int segments) { + glLineWidth(1.5f); + glBegin(GL_LINE_LOOP); + glColor4f(0.7f, 0.7f, 0.7f, 0.6f); + + for (int i = 0; i < segments; i++) { + float angle = 2.0f * 3.14159265f * i / segments; + float x = radius * cos(angle); + float y = radius * sin(angle); + glVertex3f(x, y, 0.0f); + } + + glEnd(); + glLineWidth(1.0f); +} + +/* Draw a cube to represent the gimbal object */ +static void draw_cube(float size) { + float s = size / 2.0f; + + glColor3f(0.2f, 0.5f, 0.8f); + + glBegin(GL_QUADS); + /* Front face */ + glColor3f(0.2f, 0.5f, 0.8f); + glVertex3f(-s, -s, s); + glVertex3f(s, -s, s); + glVertex3f(s, s, s); + glVertex3f(-s, s, s); + + /* Back face */ + glColor3f(0.15f, 0.4f, 0.6f); + glVertex3f(-s, -s, -s); + glVertex3f(-s, s, -s); + glVertex3f(s, s, -s); + glVertex3f(s, -s, -s); + + /* Top face */ + glColor3f(0.3f, 0.6f, 0.9f); + glVertex3f(-s, s, -s); + glVertex3f(-s, s, s); + glVertex3f(s, s, s); + glVertex3f(s, s, -s); + + /* Bottom face */ + glColor3f(0.1f, 0.3f, 0.5f); + glVertex3f(-s, -s, -s); + glVertex3f(s, -s, -s); + glVertex3f(s, -s, s); + glVertex3f(-s, -s, s); + + /* Right face */ + glColor3f(0.25f, 0.55f, 0.85f); + glVertex3f(s, -s, -s); + glVertex3f(s, s, -s); + glVertex3f(s, s, s); + glVertex3f(s, -s, s); + + /* Left face */ + glColor3f(0.18f, 0.45f, 0.75f); + glVertex3f(-s, -s, -s); + glVertex3f(-s, -s, s); + glVertex3f(-s, s, s); + glVertex3f(-s, s, -s); + + glEnd(); +} + +gimbal_visualizer* gimbal_visualizer_init(int width, int height, vec3d_gimbal *gimbal) { + if (!glfwInit()) { + fprintf(stderr, "Failed to initialize GLFW\n"); + return NULL; + } + + GLFWwindow *window = glfwCreateWindow(width, height, "Gimbal Rotation Visualizer", NULL, NULL); + if (!window) { + fprintf(stderr, "Failed to create GLFW window\n"); + glfwTerminate(); + return NULL; + } + + glfwMakeContextCurrent(window); + glfwSwapInterval(1); /* Enable vsync */ + + gimbal_visualizer *viz = (gimbal_visualizer *)malloc(sizeof(gimbal_visualizer)); + if (!viz) { + glfwDestroyWindow(window); + glfwTerminate(); + return NULL; + } + + viz->window = window; + viz->width = width; + viz->height = height; + viz->gimbal = *gimbal; + + /* Setup OpenGL perspective */ + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + float aspect = (float)width / (float)height; + float fov = 45.0f * 3.14159265f / 180.0f; + float f = 1.0f / tanf(fov / 2.0f); + float near = 0.1f, far = 100.0f; + + glFrustum(-aspect * near / f, aspect * near / f, + -near / f, near / f, near, far); + + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); + glTranslatef(0.0f, 0.0f, -5.0f); + + glEnable(GL_DEPTH_TEST); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + glClearColor(0.1f, 0.1f, 0.1f, 1.0f); + + return viz; +} + +void gimbal_visualizer_update(gimbal_visualizer *viz, mat3_gimbal *p_gimbal) { + if (!viz) return; + + mat3_gimbal gimbal = *p_gimbal; + + viz->rotation_x = (float)gimbal.gimbal3d.x_gimbal * 180.0f / 3.14159265f; + viz->rotation_y = (float)gimbal.gimbal3d.y_gimbal * 180.0f / 3.14159265f; + viz->rotation_z = (float)gimbal.gimbal3d.z_gimbal * 180.0f / 3.14159265f; +} + + +void gimbal_visualizer_render(gimbal_visualizer *viz, mat3_gimbal *p_gimbal) { + if (!viz) return; + + mat3_gimbal gimbal = *p_gimbal; + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + glMatrixMode(GL_MODELVIEW); + glPushMatrix(); + + vector3d X_AXIS = { + }; + + vector3d Y_AXIS = { + }; + + vector3d Z_AXIS = { + }; + + mat3_get_axis_orientation(gimbal, VEC3_X_COMPONENT, &X_AXIS); + mat3_get_axis_orientation(gimbal, VEC3_Y_COMPONENT, &Y_AXIS); + mat3_get_axis_orientation(gimbal, VEC3_Z_COMPONENT, &Z_AXIS); + + /* Apply gimbal rotations with the gimbals orientations */ + glRotatef(viz->rotation_x, + X_AXIS.x, + X_AXIS.y, + X_AXIS.z); + glRotatef(viz->rotation_y, + Y_AXIS.x, + Y_AXIS.y, + Y_AXIS.z); + glRotatef(viz->rotation_z, + Z_AXIS.x, + Z_AXIS.y, + Z_AXIS.z); + /* Draw reference axes */ + draw_axis(1.5f, X_AXIS, Y_AXIS, Z_AXIS); + glPopMatrix(); + + glPushMatrix(); + /* Draw gimbal rings */ + glRotatef(viz->rotation_x, + X_AXIS.x, + X_AXIS.y, + X_AXIS.z); + draw_gimbal_ring(1.0f, 32); + + glRotatef(viz->rotation_y, + Y_AXIS.x, + Y_AXIS.y, + Y_AXIS.z); + draw_gimbal_ring(1.0f, 32); + + glRotatef(viz->rotation_z, + Z_AXIS.x, + Z_AXIS.y, + Z_AXIS.z); + draw_gimbal_ring(1.0f, 32); + glPopMatrix(); + + /* Draw the central object */ + glPushMatrix(); + + glRotatef(viz->rotation_x, + X_AXIS.x, + X_AXIS.y, + X_AXIS.z); + glRotatef(viz->rotation_y, + Y_AXIS.x, + Y_AXIS.y, + Y_AXIS.z); + glRotatef(viz->rotation_z, + Z_AXIS.x, + Z_AXIS.y, + Z_AXIS.z); + draw_cube(0.5f); + glPopMatrix(); + + glfwSwapBuffers(viz->window); + glfwPollEvents(); +} + +int gimbal_visualizer_should_close(gimbal_visualizer *viz) { + if (!viz) return 1; + return glfwWindowShouldClose(viz->window); +} + +void gimbal_visualizer_destroy(gimbal_visualizer *viz) { + if (!viz) return; + + if (viz->window) { + glfwDestroyWindow(viz->window); + } + glfwTerminate(); + free(viz); +} +#endif \ No newline at end of file diff --git a/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/init_rotator_gimbal.c b/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/init_rotator_gimbal.c index d2d75c99..5de22055 100644 --- a/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/init_rotator_gimbal.c +++ b/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/init_rotator_gimbal.c @@ -1,14 +1,28 @@ #include +#include +#include -status_code init_rotator_gimbal(vector3d axis, matrix *__rotator, - vec_component angle, - vec_component *angle1, - vec3d_gimbal *gimbal) { +status_code init_rotator_gimbal(vector3d axis, matrix orientation, + matrix *__rotator, + vec_component angle) { if (NULL == __rotator || NULL == __rotator->element) { return EUNDEFINEDBUFFER; } + mat3_gimbal gimbal = { + .gimbal3d = (vec3d_gimbal) { + .orientation = &orientation + } + }; + + status_code __code = mat3_get_axis_orientation(gimbal, axis, &axis) & + vec3d_abs(axis, &axis, NULL); + + if (PASS != __code) { + return __code; + } + if (get_vec_gimbal(axis) == GIMBAL_Z) { // rotate around z-axis // pre-processing automata @@ -16,16 +30,16 @@ status_code init_rotator_gimbal(vector3d axis, matrix *__rotator, // 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[0][2] = 0.0f; __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; - } + __rotator->element[1][2] = 0.0f; + __rotator->element[2][0] = 0.0f; + __rotator->element[2][1] = 0.0f; + __rotator->element[2][2] = 1.0f; } else if (get_vec_gimbal(axis) == GIMBAL_Y) { // rotate around y-axis @@ -35,15 +49,14 @@ status_code init_rotator_gimbal(vector3d axis, matrix *__rotator, // work in XZ plane __rotator->element[0][0] = vector2d_cos(angle); + __rotator->element[0][1] = 0.0f; __rotator->element[0][2] = -vector2d_sin(angle); + __rotator->element[1][0] = 0.0f; + __rotator->element[1][1] = 1; + __rotator->element[1][2] = 0.0f; __rotator->element[2][0] = vector2d_sin(angle); + __rotator->element[2][1] = 0.0f; __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) { @@ -53,16 +66,15 @@ status_code init_rotator_gimbal(vector3d axis, matrix *__rotator, // 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; - } + __rotator->element[0][1] = 0.0f; + __rotator->element[0][2] = 0.0f; + __rotator->element[1][0] = 0.0f; + __rotator->element[1][1] = vector2d_cos(angle); + __rotator->element[1][2] = vector2d_sin(angle); + __rotator->element[2][0] = 0.0f; + __rotator->element[2][1] = -vector2d_sin(angle); + __rotator->element[2][2] = vector2d_cos(angle); } else { return EINCOMPATTYPE; diff --git a/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/preprocess_mat3_orientator.c b/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/preprocess_mat3_orientator.c deleted file mode 100644 index a1f31205..00000000 --- a/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/preprocess_mat3_orientator.c +++ /dev/null @@ -1,20 +0,0 @@ -#include -#include - -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; -} \ No newline at end of file diff --git a/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/preprocess_orientator.c b/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/preprocess_orientator.c deleted file mode 100644 index 952950cc..00000000 --- a/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/preprocess_orientator.c +++ /dev/null @@ -1,35 +0,0 @@ -#include - -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]); -} diff --git a/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/rotate_gimbal.c b/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/rotate_gimbal.c index 4c0644a0..6d4007ff 100644 --- a/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/rotate_gimbal.c +++ b/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/gimbal/rotate_gimbal.c @@ -13,21 +13,12 @@ static inline status_code __on_entry_iterated(mat_proc_sig proc_sig) { return PASS; } -status_code rotate_gimbal(vector3d axis, vec_component angle1, - matrix *__rotator, +status_code rotate_gimbal(vector3d axis, + 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,}; @@ -42,7 +33,7 @@ status_code rotate_gimbal(vector3d axis, vec_component angle1, mat_processors mat_procs = { }; - __code = mat_product(*__rotator, + status_code __code = mat_product(__rotator, *(in_gimbal->orientation), &__orient, mat_procs); if (PASS != __code) { @@ -68,13 +59,5 @@ status_code rotate_gimbal(vector3d axis, vec_component angle1, 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; } \ No newline at end of file diff --git a/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/matrix3/mat3_get_axis_orientation.c b/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/matrix3/mat3_get_axis_orientation.c new file mode 100644 index 00000000..8f4cea51 --- /dev/null +++ b/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/matrix3/mat3_get_axis_orientation.c @@ -0,0 +1,43 @@ +#include +#include + +status_code mat3_get_axis_orientation(mat3_gimbal mat, vector3d axis, vector3d *out) { + if (NULL == mat.gimbal3d.orientation || NULL == out) { + return EUNDEFINEDBUFFER; + } + out->gimbal.orientation = NULL; + + vec_component __gimbal_x[1] = {axis.x}; + vec_component __gimbal_y[1] = {axis.y}; + vec_component __gimbal_z[1] = {axis.z}; + vec_component *__gimbal_comps[3] = {__gimbal_x, __gimbal_y, __gimbal_z}; + + matrix m_axis = { + .element = __gimbal_comps, + .m = 3, + .n = 1 + }; + + vec_component __out_x[1] = {0}; + vec_component __out_y[1] = {0}; + vec_component __out_z[1] = {0}; + vec_component *__out_comps[3] = {__out_x, __out_y, __out_z}; + + matrix out_mat = { + .element = __out_comps, + .m = 3, + .n = 1 + }; + + status_code __code = mat_product(*mat.gimbal3d.orientation, m_axis, &out_mat, (mat_processors) {}); + + if (PASS != __code) { + return __code; + } + + out->x = out_mat.element[0][0]; + out->y = out_mat.element[1][0]; + out->z = out_mat.element[2][0]; + + return PASS; +} \ No newline at end of file diff --git a/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/matrix3/mat3_get_vector.c b/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/matrix3/mat3_get_vector.c new file mode 100644 index 00000000..ccd0b933 --- /dev/null +++ b/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/matrix3/mat3_get_vector.c @@ -0,0 +1,43 @@ +#include +#include + +status_code mat3_get_vector(mat3_gimbal mat, vector3d axis, vector3d *out) { + if (NULL == mat.mat3d.element || mat.mat3d.m != 3 || + mat.mat3d.m != mat.mat3d.n || NULL == out) { + return EUNDEFINEDBUFFER; + } + + vec_component __x_comp[1] = {axis.x}; + vec_component __y_comp[1] = {axis.y}; + vec_component __z_comp[1] = {axis.z}; + vec_component *__vec_comps[3] = {__x_comp, __y_comp, __z_comp}; + + matrix m_axis = { + .element = __vec_comps, + .m = 3, + .n = 1 + }; + + vec_component __out_x[1] = {0}; + vec_component __out_y[1] = {0}; + vec_component __out_z[1] = {0}; + vec_component *__out_comps[3] = {__out_x, __out_y, __out_z}; + + matrix out_mat = { + .element = __out_comps, + .m = 3, + .n = 1 + }; + + status_code __code = mat_product(mat.mat3d, m_axis, &out_mat, (mat_processors) {}); + + if (PASS != __code) { + return __code; + } + + out->x = out_mat.element[0][0]; + out->y = out_mat.element[1][0]; + out->z = out_mat.element[2][0]; + + return PASS; +} \ No newline at end of file diff --git a/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/matrix3/mat3_rotate.c b/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/matrix3/mat3_rotate.c index 5d070151..5863a373 100644 --- a/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/matrix3/mat3_rotate.c +++ b/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/matrix3/mat3_rotate.c @@ -74,24 +74,29 @@ status_code mat3_rotate(mat3_gimbal in, vector3d axis, proc_sig.mat = out->mat3d; status_code __code; - vec_component angle1 = 0; - - // preprocessing automata -- initialize the input column vector - __code = preprocess_mat3_orientator(in.gimbal3d, &axis); - if (PASS != __code) { - if (NULL != proc.processors.on_op_failure) { - proc.processors.on_op_failure(proc_sig, __code); - } - return __code; + vec_component *gimbal_angle = NULL; + vector_gimbal rotator_gimbal; + + if (get_vec_gimbal(axis) == GIMBAL_X) { + out->gimbal3d.x_gimbal += angle; + out->gimbal3d.delta_x = angle; + gimbal_angle = &(out->gimbal3d.x_gimbal); + rotator_gimbal = GIMBAL_X; + } else if (get_vec_gimbal(axis) == GIMBAL_Y) { + out->gimbal3d.y_gimbal += angle; + out->gimbal3d.delta_y = angle; + gimbal_angle = &(out->gimbal3d.y_gimbal); + rotator_gimbal = GIMBAL_Y; + } else if (get_vec_gimbal(axis) == GIMBAL_Z) { + out->gimbal3d.z_gimbal += angle; + out->gimbal3d.delta_z = angle; + gimbal_angle = &(out->gimbal3d.z_gimbal); + rotator_gimbal = GIMBAL_Z; + } else { + return EINCOMPATTYPE; } - __code = vec3d_abs(axis, &axis, NULL); - if (PASS != __code) { - if (NULL != proc.processors.on_op_failure) { - proc.processors.on_op_failure(proc_sig, __code); - } - return __code; - } + // preprocessing automata -- initialize the input column vector // preprocessing automata -- allocate rotator matrix for the angular motion vec_component __rotate_x[3] = {0, 0, 0,}; @@ -106,8 +111,7 @@ status_code mat3_rotate(mat3_gimbal in, vector3d axis, // preprocessing automata -- init rotator matrix for the angular motion with an angular // rotator vector matrix - __code = init_rotator_gimbal(axis, &__rotator, angle, - &angle1, &out->gimbal3d); + __code = init_rotator_gimbal(axis, *(in.gimbal3d.orientation), &__rotator, angle); if (PASS != __code) { if (NULL != proc.processors.on_op_failure) { proc.processors.on_op_failure(proc_sig, __code); @@ -116,22 +120,26 @@ status_code mat3_rotate(mat3_gimbal in, vector3d axis, } // processing automata -- rotate the matrix - __code = mat_product(in.mat3d, __rotator, &(out->mat3d), proc.processors); + __code = mat_product(__rotator, in.mat3d, &(out->mat3d), proc.processors); if (PASS != __code) { if (NULL != proc.processors.on_op_failure) { proc.processors.on_op_failure(proc_sig, __code); } return __code; } - - // post-processing automata -- handle gimbals and gimbal rotation - if (vector2d_abs(vector2d_cos(angle1)) <= ___ROTATION_MIN_THRESHOLD - && vector2d_abs(vector2d_sin(angle1)) == 1) { - if (NULL != proc.on_gimbal_lock_trap) { - proc.on_gimbal_lock_trap(*out, get_vec_gimbal(axis), angle); - } + // post-processing automata -- handle gimbals and gimbal rotation + if (fabsf(vector2d_abs(vector2d_cos(*gimbal_angle)) - ___ROTATION_MIN_THRESHOLD) < ___DELTA || + fabsf(vector2d_abs(vector2d_sin(*gimbal_angle)) - 1.0f) < ___DELTA) { // rotate the gimbals axes (the orientation) - __code = rotate_gimbal(axis, angle1, &__rotator, + __code = init_rotator_gimbal(axis, *(in.gimbal3d.orientation), &__rotator, *gimbal_angle); + if (PASS != __code) { + if (NULL != proc.processors.on_op_failure) { + proc.processors.on_op_failure(proc_sig, __code); + } + return __code; + } + + __code = rotate_gimbal(axis, __rotator, &(in.gimbal3d), &out->gimbal3d, NULL); @@ -142,6 +150,14 @@ status_code mat3_rotate(mat3_gimbal in, vector3d axis, } return __code; } + + // execute the gimbal trap post-processor + if (NULL != proc.on_gimbal_lock_trap) { + proc.on_gimbal_lock_trap(*out, rotator_gimbal, *gimbal_angle, + proc.processors); + } + + *gimbal_angle = 0.0f; } if (NULL != proc.processors.on_op_success) { diff --git a/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/vector3d/vec3d_rotate.c b/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/vector3d/vec3d_rotate.c index 90c9fd49..df04ebec 100644 --- a/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/vector3d/vec3d_rotate.c +++ b/sdk/core/src/libs/electrostatic-primer/electronetsoft/arithmos/vector3d/vec3d_rotate.c @@ -1,6 +1,7 @@ #include #include #include +#include #include static inline status_code rotate_about_gimbal(matrix *__rotator, @@ -82,13 +83,29 @@ status_code vec3d_rotate(vector3d v, vector3d axis, vec_component angle, }; status_code __code; - vec_component angle1 = 0; - - preprocess_orientator(&v, &axis); - vec3d_abs(axis, &axis, NULL); + vec_component *gimbal_angle = NULL; + vector_gimbal rotator_gimbal; + + if (get_vec_gimbal(axis) == GIMBAL_X) { + out->gimbal.x_gimbal += angle; + out->gimbal.delta_x = angle; + gimbal_angle = &(out->gimbal.x_gimbal); + rotator_gimbal = GIMBAL_X; + } else if (get_vec_gimbal(axis) == GIMBAL_Y) { + out->gimbal.y_gimbal += angle; + out->gimbal.delta_y = angle; + gimbal_angle = &(out->gimbal.y_gimbal); + rotator_gimbal = GIMBAL_Y; + } else if (get_vec_gimbal(axis) == GIMBAL_Z) { + out->gimbal.z_gimbal += angle; + out->gimbal.delta_z = angle; + gimbal_angle = &(out->gimbal.z_gimbal); + rotator_gimbal = GIMBAL_Z; + } else { + return EINCOMPATTYPE; + } - __code = init_rotator_gimbal(axis, &__rotator, angle, - &angle1, &out->gimbal); + __code = init_rotator_gimbal(axis, *(v.gimbal.orientation), &__rotator, angle); if (PASS != __code) { if (NULL != procs && NULL != procs->on_op_failed) { @@ -107,13 +124,17 @@ status_code vec3d_rotate(vector3d v, vector3d axis, vec_component angle, return __code; } - 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); - } + if (fabsf(vector2d_abs(vector2d_cos(*gimbal_angle)) - ___ROTATION_MIN_THRESHOLD) < ___DELTA || fabsf(vector2d_abs(vector2d_sin(*gimbal_angle)) - 1.0f) < ___DELTA) { // rotate the gimbals axes (the orientation) - __code = rotate_gimbal(axis, angle1, &__rotator, + __code = init_rotator_gimbal(axis, *(v.gimbal.orientation), &__rotator, *gimbal_angle); + if (PASS != __code) { + if (NULL != procs && NULL != procs->on_op_failed) { + procs->on_op_failed(&vec3d_rotate, __code); + } + return __code; + } + + __code = rotate_gimbal(axis, __rotator, &(v.gimbal), &out->gimbal, procs); if (PASS != __code) { @@ -122,6 +143,12 @@ status_code vec3d_rotate(vector3d v, vector3d axis, vec_component angle, } return __code; } + + if (NULL != procs && NULL != procs->on_gimbal_lock_trap) { + procs->on_gimbal_lock_trap(*out, get_vec_gimbal(axis), *gimbal_angle); + } + + *gimbal_angle = 0.0f; } if (NULL != procs && NULL != procs->on_op_success) { diff --git a/sdk/examples/src/calculus/test_gimbal_orientation.c b/sdk/examples/src/calculus/test_gimbal_orientation.c new file mode 100644 index 00000000..bf53a879 --- /dev/null +++ b/sdk/examples/src/calculus/test_gimbal_orientation.c @@ -0,0 +1,54 @@ +#include +#include +#include +#include +#include + +int main() { + + vec_component __in_x[3] = {0, -1, 0}; + vec_component __in_y[3] = {0, 0, 1}; + vec_component __in_z[3] = {-1, 0, 0}; + vec_component *__in_comps[3] = {__in_x, __in_y, __in_z}; + + matrix in_mat = { + .element = __in_comps, + .m = 3, + .n = 3 + }; + + vec_component __in_axis_x[1] = {VEC3_Z_COMPONENT.x}; + vec_component __in_axis_y[1] = {VEC3_Z_COMPONENT.y}; + vec_component __in_axis_z[1] = {VEC3_Z_COMPONENT.z}; + vec_component *__in_axis[3] = {__in_axis_x, __in_axis_y, __in_axis_z}; + + matrix in_axis_mat = { + .element = __in_axis, + .m = 3, + .n = 1 + }; + + vec_component __out_x[1] = {0}; + vec_component __out_y[1] = {0}; + vec_component __out_z[1] = {0}; + vec_component *__out_comps[3] = {__out_x, __out_y, __out_z}; + + matrix out_mat = { + .element = __out_comps, + .m = 3, + .n = 1 + }; + + mat_processors proc = { + }; + + status_code __code = mat_product(in_mat, in_axis_mat, &out_mat, proc); + + if (PASS != __code) { + return __code; + } + + fprintf(stdout, "Axis out = [%f, %f, %f]\n", out_mat.element[0][0], out_mat.element[1][0], out_mat.element[2][0]); + + return 0; +} \ No newline at end of file diff --git a/sdk/examples/src/calculus/test_opengl_gimbal.c b/sdk/examples/src/calculus/test_opengl_gimbal.c new file mode 100644 index 00000000..8045e5a5 --- /dev/null +++ b/sdk/examples/src/calculus/test_opengl_gimbal.c @@ -0,0 +1,256 @@ +#include +#include +#include +#include +#include +#include +#include + +vec_component ref_angle = 0.0f; +vec_component delta = 0.0f; +vec_component phi = 0.0f; + +typedef struct { + mat3_gimbal *mat_gimbal; + vec_component a0; + vec_component a1; + vec_component delta; + vec_component phi; + vec_component etta; + vector3d axis; + float lerp; + uint8_t finished; + uint8_t loop; + char *name; +} rotation_transformer; + +static void print_gimbal_state(const char *label, mat3_gimbal gimbal) { + fprintf(stdout, CYAN "%s\n" RESET, label); + fprintf(stdout, CYAN " X-Gimbal angle: %f\n" RESET, gimbal.gimbal3d.x_gimbal); + fprintf(stdout, CYAN " Y-Gimbal angle: %f\n" RESET, gimbal.gimbal3d.y_gimbal); + fprintf(stdout, CYAN " Z-Gimbal angle: %f\n" RESET, gimbal.gimbal3d.z_gimbal); +} + +// Helper function to print gimbal orientation matrix +static void print_gimbal_orientation(const char *label, mat3_gimbal gimbal) { + + vector3d X_AXIS = { + }; + + vector3d Y_AXIS = { + }; + + vector3d Z_AXIS = { + }; + + mat3_get_axis_orientation(gimbal, VEC3_X_COMPONENT, &X_AXIS); + mat3_get_axis_orientation(gimbal, VEC3_Y_COMPONENT, &Y_AXIS); + mat3_get_axis_orientation(gimbal, VEC3_Z_COMPONENT, &Z_AXIS); + + fprintf(stdout, CYAN "%s\n" RESET, label); + fprintf(stdout, CYAN " X-Gimbal orientation: [%f, %f, %f]\n" RESET, + X_AXIS.x, + X_AXIS.y, + X_AXIS.z); + fprintf(stdout, CYAN " Y-Gimbal orientation: [%f, %f, %f]\n" RESET, + Y_AXIS.x, + Y_AXIS.y, + Y_AXIS.z); + fprintf(stdout, CYAN " Z-Gimbal orientation: [%f, %f, %f]\n" RESET, + Z_AXIS.x, + Z_AXIS.y, + Z_AXIS.z); +} + +void on_gimbal_lock_trap(mat3_gimbal rotated, + vector_gimbal gimbal, + vec_component angle, + mat_processors proc) { + rotation_transformer **transformers = proc.metadata; + int gimbal_index = 0; + if (GIMBAL_X == gimbal) { + gimbal_index = 0; + } else if (GIMBAL_Y == gimbal) { + gimbal_index = 1; + } else if (GIMBAL_Z == gimbal) { + gimbal_index = 2; + } + if (transformers[gimbal_index]->loop == 1) { + transformers[gimbal_index]->a0 = ref_angle; + transformers[gimbal_index]->phi = phi; + transformers[gimbal_index]->delta = delta; + transformers[gimbal_index]->finished = 0; + } else { + transformers[gimbal_index]->finished = 1; + } + fprintf(stdout, "Gimbal lock trap triggered!\n"); + print_gimbal_state(YELLOW "Rotated Gimbal Angles:" RESET, rotated); + print_gimbal_orientation(YELLOW "Rotated Gimbal Orientations:" RESET, rotated); +} + +static void mat3_interpolate_rotation(rotation_transformer *transformer, + mat3_processors processors) { + + transformer->phi = (transformer->a1 - transformer->a0) * transformer->lerp + + transformer->a0; + // calculate the delta + transformer->delta = transformer->phi - transformer->a0; + transformer->a0 = transformer->phi; + + status_code __code = + mat3_rotate(*transformer->mat_gimbal, + transformer->axis, + transformer->mat_gimbal, + transformer->delta, processors); + + if (PASS != __code) { + return; + } +} + +/* [Previous test functions remain the same: print_gimbal_state, print_gimbal_orientation, + assert_x_rotation_90, assert_y_rotation_90, assert_z_rotation_90, + assert, on_assertion_success, on_assertion_failure] */ + +/* Execute test 1 with visualization */ +static inline int64_t execute_x_rotation_visual(void **inputs) { + vec_component __rotate_x[3] = {1, 0, 0,}; + vec_component __rotate_y[3] = {0, 1, 0,}; + vec_component __rotate_z[3] = {0, 0, 1,}; + vec_component *__rotate_comps[3] = {__rotate_x, __rotate_y, __rotate_z}; + + vec_component __gimbal_x[3] = {1, 0, 0,}; + vec_component __gimbal_y[3] = {0, 1, 0,}; + vec_component __gimbal_z[3] = {0, 0, 1,}; + vec_component *__gimbal_comps[3] = {__gimbal_x, __gimbal_y, __gimbal_z}; + + matrix __gimbal = { + .element = __gimbal_comps, + .m = 3, + .n = 3, + }; + + mat3_gimbal mat_gimbal = { + .mat3d = (matrix) { + .element = __rotate_comps, + .m = 3, + .n = 3, + }, + .gimbal3d = (vec3d_gimbal) { + .orientation = &__gimbal, + .x_gimbal = 0.0f, + .y_gimbal = 0.0f, + .z_gimbal = 0.0f + } + }; + + fprintf(stdout, "\n=== Test 1: X-axis Rotation (90°) with Visualization ===\n"); + + gimbal_visualizer *viz = gimbal_visualizer_init(800, 600, &mat_gimbal.gimbal3d); + if (!viz) { + fprintf(stderr, RED "Failed to initialize visualizer\n" RESET); + return ASSERTION_FAILURE; + } + + + rotation_transformer transformer_y = { + .a0 = ref_angle, + .a1 = 3.0f * M_PI / 2.0f, + .delta = delta, + .phi = phi, + .etta = ___DELTA, + .mat_gimbal = &mat_gimbal, + .axis = VEC3_Y_COMPONENT, + .lerp = .008f, + .loop = 1, + .name = "Y-Axis Rotation" + }; + + + rotation_transformer transformer_x = { + .a0 = ref_angle, + .a1 = 5.0f * M_PI / 2.0f, + .delta = delta, + .phi = phi, + .etta = ___DELTA, + .mat_gimbal = &mat_gimbal, + .axis = VEC3_X_COMPONENT, + .lerp = .008f, + .loop = 1, + .name = "X-Axis Rotation" + }; + + rotation_transformer transformer_z = { + .a0 = ref_angle, + .a1 = M_PI / 2.0f, + .delta = delta, + .phi = phi, + .etta = ___DELTA, + .mat_gimbal = &mat_gimbal, + .axis = VEC3_Z_COMPONENT, + .lerp = .008f, + .loop = 1, + .name = "Z-Axis Rotation" + }; + + rotation_transformer *transformer[3] = {&transformer_x, + &transformer_y, + &transformer_z}; + + mat3_processors processors = { + .processors = { + .metadata = transformer + }, + .on_gimbal_lock_trap = &on_gimbal_lock_trap, + }; + + for (; ;) { + if (transformer_x.finished == 0 || 1) { + mat3_interpolate_rotation(&transformer_x, + processors); + } + + // after this; the Z-axis rotator is swapped with the Y-axis rotator + // now; ordering rotation around the Y-axis would use the Z-axis rotator + // and vice versa + + if (transformer_x.finished == 1 || 1) { + mat3_interpolate_rotation(&transformer_y, + processors); + } + + // after this; the Z-axis rotator (which was Y rotator) is swapped with the X-axis rotator + // now; ordering rotation around the Z-axis would use the X-axis rotator + + if (transformer_y.finished == 1 || 1) { + mat3_interpolate_rotation(&transformer_z, + processors); + } + + gimbal_visualizer_update(viz, &mat_gimbal); + gimbal_visualizer_render(viz, &mat_gimbal); + + if (gimbal_visualizer_should_close(viz)) break; + usleep(16667); + } + + gimbal_visualizer_destroy(viz); + + fprintf(stdout, GREEN "✓ Visualization completed\n" RESET); + return ASSERTION_SUCCESS; +} + +int main() { + fprintf(stdout, "\n========================================\n"); + fprintf(stdout, "Matrix3 Gimbal Rotation - Visual Tests\n"); + fprintf(stdout, "========================================\n"); + + fprintf(stdout, "\nRunning X-axis rotation test with OpenGL visualization...\n"); + execute_x_rotation_visual(NULL); + + fprintf(stdout, "\n========================================\n"); + fprintf(stdout, "Tests completed!\n"); + fprintf(stdout, "========================================\n"); + + return 0; +} \ No newline at end of file