Добавление базисов как вспомогательной структуры

basic-geometry/basic-geometry.vcxproj

@@ -21,6 +21,7 @@
   <ItemGroup>
     <ClInclude Include="angle.h" />
     <ClInclude Include="basic-geometry.h" />
+    <ClInclude Include="basis3.h" />
     <ClInclude Include="complex.h" />
     <ClInclude Include="cotes-number.h" />
     <ClInclude Include="matrix2x2.h" />
@@ -40,6 +41,7 @@
     <ClCompile Include="angle.c" />
     <ClInclude Include="complex.c" />
     <ClInclude Include="cotes-number.c" />
+    <ClCompile Include="basis3.c" />
     <ClCompile Include="utilities.c" />
     <ClCompile Include="matrix2x2.c" />
     <ClCompile Include="matrix2x3.c" />

basic-geometry/basic-geometry.vcxproj.filters

@@ -69,6 +69,9 @@
     <ClInclude Include="slerp.h">
       <Filter>Файлы заголовков</Filter>
     </ClInclude>
+    <ClInclude Include="basis3.h">
+      <Filter>Файлы заголовков</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="angle.c">
@@ -110,5 +113,8 @@
     <ClCompile Include="slerp.c">
       <Filter>Исходные файлы</Filter>
     </ClCompile>
+    <ClCompile Include="basis3.c">
+      <Filter>Исходные файлы</Filter>
+    </ClCompile>
   </ItemGroup>
 </Project>

basic-geometry/basis3.c

@@ -0,0 +1,232 @@
+#include <math.h>
+#include "./basis3.h"
+
+typedef struct {
+    int e1, e2, e3;
+} _BgcBasis3State;
+
+static inline void _bgc_basis3_state_reset(_BgcBasis3State* state)
+{
+    state->e1 = 0;
+    state->e2 = 0;
+    state->e3 = 0;
+}
+
+static inline int _bgc_basis3_validate_directions(const int primary_direction, const int auxiliary_direction)
+{
+    if (!bgc_is_correct_direction(primary_direction)) {
+        return BGC_ERROR_BASIS3_PRIMARY_DIRECTION_UNKNOWN;
+    }
+
+    if (!bgc_is_correct_direction(auxiliary_direction)) {
+        return BGC_ERROR_BASIS3_AUXILIARY_DIRECTION_UNKNOWN;
+    }
+
+    if (primary_direction == auxiliary_direction || primary_direction == -auxiliary_direction) {
+        return BGC_ERROR_BASIS3_PRIMARY_AUXILIARY_PARALLEL;
+    }
+
+    return BGC_SUCCESS;
+}
+
+static inline void _bgc_basis3_load_axis_fp32(const int direction, const BgcVector3FP32* axis, _BgcBasis3State* state, BgcBasis3FP32* basis)
+{
+    switch (direction) {
+    case BGC_DIRECTION_X1:
+        bgc_vector3_copy_fp32(axis, &basis->e1);
+        state->e1 = 1;
+        break;
+
+    case -BGC_DIRECTION_X1:
+        bgc_vector3_reverse_fp32(axis, &basis->e1);
+        state->e1 = 1;
+        break;
+
+    case BGC_DIRECTION_X2:
+        bgc_vector3_copy_fp32(axis, &basis->e2);
+        state->e2 = 1;
+        break;
+
+    case -BGC_DIRECTION_X2:
+        bgc_vector3_reverse_fp32(axis, &basis->e2);
+        state->e2 = 1;
+        break;
+
+    case BGC_DIRECTION_X3:
+        bgc_vector3_copy_fp32(axis, &basis->e3);
+        state->e3 = 1;
+        break;
+
+    case -BGC_DIRECTION_X3:
+        bgc_vector3_reverse_fp32(axis, &basis->e3);
+        state->e3 = 1;
+        break;
+    }
+}
+
+int bgc_basis3_make_from_directions_fp32(
+    const int primary_direction, const BgcVector3FP32* primary_vector,
+    const int auxiliary_direction, const BgcVector3FP32* auxiliary_vector,
+    BgcBasis3FP32* basis
+) {
+    const int direstion_validation_code = _bgc_basis3_validate_directions(primary_direction, auxiliary_direction);
+
+    if (direstion_validation_code != BGC_SUCCESS) {
+        return direstion_validation_code;
+    }
+
+    const float primary_square_modulus = bgc_vector3_get_square_modulus_fp32(primary_vector);
+
+    if (primary_square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
+        return BGC_ERROR_BASIS3_PRIMARY_VECTOR_IS_ZERO;
+    }
+
+    const float auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp32(auxiliary_vector);
+
+    if (auxiliary_square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
+        return BGC_ERROR_BASIS3_AUXILIARY_VECTOR_IS_ZERO;
+    }
+
+    BgcVector3FP32 orthogonal;
+
+    bgc_vector3_get_cross_product_fp32(primary_vector, auxiliary_vector, &orthogonal);
+
+    const float orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp32(&orthogonal);
+
+    if (orthogonal_square_modulus <= BGC_SQUARE_EPSYLON_FP32 * primary_square_modulus * auxiliary_square_modulus) {
+        return BGC_ERROR_BASIS3_PRIMARY_AUXILIARY_PARALLEL;
+    }
+
+    BgcVector3FP32 primary_axis, auxiliary_axis;
+
+    bgc_vector3_multiply_fp32(primary_vector, sqrtf(1.0f / primary_square_modulus), &primary_axis);
+
+    bgc_vector3_subtract_scaled_fp32(auxiliary_vector, &primary_axis, bgc_vector3_get_scalar_product_fp32(auxiliary_vector, &primary_axis), &auxiliary_axis);
+
+    bgc_vector3_multiply_fp32(&auxiliary_axis, sqrtf(1.0f / bgc_vector3_get_square_modulus_fp32(&auxiliary_axis)), &auxiliary_axis);
+
+    _BgcBasis3State state;
+
+    _bgc_basis3_state_reset(&state);
+
+    _bgc_basis3_load_axis_fp32(primary_direction, &primary_axis, &state, basis);
+    _bgc_basis3_load_axis_fp32(auxiliary_direction, &auxiliary_axis, &state, basis);
+
+    if (!state.e1) {
+        bgc_vector3_get_cross_product_fp32(&basis->e2, &basis->e3, &basis->e1);
+        bgc_vector3_normalize_fp32(&basis->e1);
+        return BGC_SUCCESS;
+    }
+
+    if (!state.e2) {
+        bgc_vector3_get_cross_product_fp32(&basis->e3, &basis->e1, &basis->e2);
+        bgc_vector3_normalize_fp32(&basis->e2);
+        return BGC_SUCCESS;
+    }
+
+    bgc_vector3_get_cross_product_fp32(&basis->e1, &basis->e2, &basis->e3);
+    bgc_vector3_normalize_fp32(&basis->e3);
+
+    return BGC_SUCCESS;
+}
+
+static inline void _bgc_basis3_load_axis_fp64(const int direction, const BgcVector3FP64* axis, _BgcBasis3State* state, BgcBasis3FP64* basis)
+{
+    switch (direction) {
+    case BGC_DIRECTION_X1:
+        bgc_vector3_copy_fp64(axis, &basis->e1);
+        state->e1 = 1;