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

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;
+        break;
+
+    case -BGC_DIRECTION_X1:
+        bgc_vector3_reverse_fp64(axis, &basis->e1);
+        state->e1 = 1;
+        break;
+
+    case BGC_DIRECTION_X2:
+        bgc_vector3_copy_fp64(axis, &basis->e2);
+        state->e2 = 1;
+        break;
+
+    case -BGC_DIRECTION_X2:
+        bgc_vector3_reverse_fp64(axis, &basis->e2);
+        state->e2 = 1;
+        break;
+
+    case BGC_DIRECTION_X3:
+        bgc_vector3_copy_fp64(axis, &basis->e3);
+        state->e3 = 1;
+        break;
+
+    case -BGC_DIRECTION_X3:
+        bgc_vector3_reverse_fp64(axis, &basis->e3);
+        state->e3 = 1;
+        break;
+    }
+}
+
+int bgc_basis3_make_from_directions_fp64(
+    const int primary_direction, const BgcVector3FP64* primary_vector,
+    const int auxiliary_direction, const BgcVector3FP64* auxiliary_vector,
+    BgcBasis3FP64* 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 double primary_square_modulus = bgc_vector3_get_square_modulus_fp64(primary_vector);
+
+    if (primary_square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
+        return BGC_ERROR_BASIS3_PRIMARY_VECTOR_IS_ZERO;
+    }
+
+    const double auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp64(auxiliary_vector);
+
+    if (auxiliary_square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
+        return BGC_ERROR_BASIS3_AUXILIARY_VECTOR_IS_ZERO;
+    }
+
+    BgcVector3FP64 orthogonal;
+
+    bgc_vector3_get_cross_product_fp64(primary_vector, auxiliary_vector, &orthogonal);
+
+    const double orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp64(&orthogonal);
+
+    if (orthogonal_square_modulus <= BGC_SQUARE_EPSYLON_FP64 * primary_square_modulus * auxiliary_square_modulus) {
+        return BGC_ERROR_BASIS3_PRIMARY_AUXILIARY_PARALLEL;
+    }
+
+    BgcVector3FP64 primary_axis, auxiliary_axis;
+
+    bgc_vector3_multiply_fp64(primary_vector, sqrt(1.0 / primary_square_modulus), &primary_axis);
+
+    bgc_vector3_subtract_scaled_fp64(auxiliary_vector, &primary_axis, bgc_vector3_get_scalar_product_fp64(auxiliary_vector, &primary_axis), &auxiliary_axis);
+
+    bgc_vector3_multiply_fp64(&auxiliary_axis, sqrt(1.0 / bgc_vector3_get_square_modulus_fp64(&auxiliary_axis)), &auxiliary_axis);
+
+    _BgcBasis3State state;
+
+    _bgc_basis3_state_reset(&state);
+
+    _bgc_basis3_load_axis_fp64(primary_direction, &primary_axis, &state, basis);
+    _bgc_basis3_load_axis_fp64(auxiliary_direction, &auxiliary_axis, &state, basis);
+
+    if (!state.e1) {
+        bgc_vector3_get_cross_product_fp64(&basis->e2, &basis->e3, &basis->e1);
+        bgc_vector3_normalize_fp64(&basis->e1);
+        return BGC_SUCCESS;
+    }
+
+    if (!state.e2) {
+        bgc_vector3_get_cross_product_fp64(&basis->e3, &basis->e1, &basis->e2);
+        bgc_vector3_normalize_fp64(&basis->e2);
+        return BGC_SUCCESS;
+    }
+
+    bgc_vector3_get_cross_product_fp64(&basis->e1, &basis->e2, &basis->e3);
+    bgc_vector3_normalize_fp64(&basis->e3);
+
+    return BGC_SUCCESS;
+}

basic-geometry/basis3.h

@@ -0,0 +1,35 @@
+#ifndef _BGC_BASIS3_H_
+#define _BGC_BASIS3_H_
+
+#include "./vector3.h"
+
+#define BGC_ERROR_BASIS3_PRIMARY_DIRECTION_UNKNOWN -3001
+#define BGC_ERROR_BASIS3_PRIMARY_VECTOR_IS_ZERO -3002
+
+#define BGC_ERROR_BASIS3_AUXILIARY_DIRECTION_UNKNOWN -3011
+#define BGC_ERROR_BASIS3_AUXILIARY_VECTOR_IS_ZERO -3012
+
+#define BGC_ERROR_BASIS3_PRIMARY_AUXILIARY_PARALLEL -3021
+
+
+typedef struct {
+    BgcVector3FP32 e1, e2, e3;
+} BgcBasis3FP32;
+
+typedef struct {
+    BgcVector3FP64 e1, e2, e3;
+} BgcBasis3FP64;
+
+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
+);
+
+int bgc_basis3_make_from_directions_fp64(
+    const int primary_direction, const BgcVector3FP64* primary_vector,
+    const int auxiliary_direction, const BgcVector3FP64* auxiliary_vector,
+    BgcBasis3FP64* basis
+);
+
+#endif

basic-geometry/rotation3.h

@@ -47,7 +47,7 @@ inline void bgc_rotation3_set_values_fp32(const float x1, const float x2, const
     rotation->axis.x2 = x2;
     rotation->axis.x3 = x3;
 
-    if (bgc_vector3_normalize_fp32(&rotation->axis, &rotation->axis)) {
+    if (bgc_vector3_normalize_fp32(&rotation->axis)) {
         rotation->radians = bgc_angle_to_radians_fp32(angle, unit);
     }
     else {
@@ -62,7 +62,7 @@ inline void bgc_rotation3_set_values_fp64(const double x1, const double x2, cons
     rotation->axis.x2 = x2;
     rotation->axis.x3 = x3;
 
-    if (bgc_vector3_normalize_fp64(&rotation->axis, &rotation->axis)) {
+    if (bgc_vector3_normalize_fp64(&rotation->axis)) {
         rotation->radians = bgc_angle_to_radians_fp64(angle, unit);
     }
     else {
@@ -72,11 +72,9 @@ inline void bgc_rotation3_set_values_fp64(const double x1, const double x2, cons
 
 inline void bgc_rotation3_set_with_axis_fp32(const BgcVector3FP32* axis, const float angle, const BgcAngleUnitEnum unit, BgcRotation3FP32* rotation)
 {
-    rotation->axis.x1 = axis->x1;
+    bgc_vector3_copy_fp32(axis, &rotation->axis);
-    rotation->axis.x2 = axis->x2;
-    rotation->axis.x3 = axis->x3;
 
-    if (bgc_vector3_normalize_fp32(&rotation->axis, &rotation->axis)) {
+    if (bgc_vector3_normalize_fp32(&rotation->axis)) {
         rotation->radians = bgc_angle_to_radians_fp32(angle, unit);
     }
     else {
@@ -86,11 +84,9 @@ inline void bgc_rotation3_set_with_axis_fp32(const BgcVector3FP32* axis, const f
 
 inline void bgc_rotation3_set_with_axis_fp64(const BgcVector3FP64* axis, const double angle, const BgcAngleUnitEnum unit, BgcRotation3FP64* rotation)
 {
-    rotation->axis.x1 = axis->x1;
+    bgc_vector3_copy_fp64(axis, &rotation->axis);
-    rotation->axis.x2 = axis->x2;
-    rotation->axis.x3 = axis->x3;
 
-    if (bgc_vector3_normalize_fp64(&rotation->axis, &rotation->axis)) {
+    if (bgc_vector3_normalize_fp64(&rotation->axis)) {
         rotation->radians = bgc_angle_to_radians_fp64(angle, unit);
     }
     else {

basic-geometry/utilities.c

@@ -1,5 +1,6 @@
 #include "utilities.h"
 
+extern inline int bgc_is_correct_direction(const int direction);
 
 extern inline int bgc_is_zero_fp32(const float square_value);
 extern inline int bgc_is_zero_fp64(const double square_value);

basic-geometry/utilities.h

@@ -29,6 +29,20 @@
 #define BGC_GOLDEN_RATIO_HIGH_FP64 1.61803398874989485
 #define BGC_GOLDEN_RATIO_LOW_FP64 0.61803398874989485
 
+#define BGC_SUCCESS 0
+#define BGC_FAILED -1
+
+#define BGC_DIRECTION_X1 1
+#define BGC_DIRECTION_X2 2
+#define BGC_DIRECTION_X3 3
+
+inline int bgc_is_correct_direction(const int direction)
+{
+    return direction == BGC_DIRECTION_X1 || direction == -BGC_DIRECTION_X1
+        || direction == BGC_DIRECTION_X2 || direction == -BGC_DIRECTION_X2
+        || direction == BGC_DIRECTION_X3 || direction == -BGC_DIRECTION_X3;
+}
+
 inline int bgc_is_zero_fp32(const float value)
 {
     return (-BGC_EPSYLON_FP32 <= value) && (value <= BGC_EPSYLON_FP32);

basic-geometry/vector3.c

@@ -30,8 +30,11 @@ extern inline void bgc_vector3_swap_fp64(BgcVector3FP64* vector1, BgcVector3FP64
 extern inline void bgc_vector3_reverse_fp32(const BgcVector3FP32* vector, BgcVector3FP32* reverse);
 extern inline void bgc_vector3_reverse_fp64(const BgcVector3FP64* vector, BgcVector3FP64* reverse);
 
-extern inline int bgc_vector3_normalize_fp32(const BgcVector3FP32* vector, BgcVector3FP32* normalized);
+extern inline int bgc_vector3_normalize_fp32(BgcVector3FP32* vector);
-extern inline int bgc_vector3_normalize_fp64(const BgcVector3FP64* vector, BgcVector3FP64* normalized);
+extern inline int bgc_vector3_normalize_fp64(BgcVector3FP64* vector);
+
+extern inline int bgc_vector3_get_normalized_fp32(const BgcVector3FP32* vector, BgcVector3FP32* normalized);
+extern inline int bgc_vector3_get_normalized_fp64(const BgcVector3FP64* vector, BgcVector3FP64* normalized);
 
 extern inline void bgc_vector3_add_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, BgcVector3FP32* sum);
 extern inline void bgc_vector3_add_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, BgcVector3FP64* sum);

basic-geometry/vector3.h

@@ -176,7 +176,49 @@ inline void bgc_vector3_reverse_fp64(const BgcVector3FP64* vector, BgcVector3FP6
 
 // ================= Normalize ================== //
 
-inline int bgc_vector3_normalize_fp32(const BgcVector3FP32* vector, BgcVector3FP32* normalized)
+inline int bgc_vector3_normalize_fp32(BgcVector3FP32* vector)
+{
+    const float square_modulus = bgc_vector3_get_square_modulus_fp32(vector);
+
+    if (bgc_is_sqare_unit_fp32(square_modulus)) {
+        return 1;
+    }
+
+    if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) {
+        return 0;
+    }
+
+    const float multiplicand = sqrtf(1.0f / square_modulus);
+
+    vector->x1 *= multiplicand;
+    vector->x2 *= multiplicand;
+    vector->x3 *= multiplicand;
+
+    return 1;
+}
+
+inline int bgc_vector3_normalize_fp64(BgcVector3FP64* vector)
+{
+    const double square_modulus = bgc_vector3_get_square_modulus_fp64(vector);
+
+    if (bgc_is_sqare_unit_fp64(square_modulus)) {
+        return 1;
+    }
+
+    if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) {
+        return 0;
+    }
+
+    const double multiplicand = sqrt(1.0 / square_modulus);
+
+    vector->x1 *= multiplicand;
+    vector->x2 *= multiplicand;
+    vector->x3 *= multiplicand;
+
+    return 1;
+}
+
+inline int bgc_vector3_get_normalized_fp32(const BgcVector3FP32* vector, BgcVector3FP32* normalized)
 {
     const float square_modulus = bgc_vector3_get_square_modulus_fp32(vector);
 
@@ -200,7 +242,7 @@ inline int bgc_vector3_normalize_fp32(const BgcVector3FP32* vector, BgcVector3FP
     return 1;
 }
 
-inline int bgc_vector3_normalize_fp64(const BgcVector3FP64* vector, BgcVector3FP64* normalized)
+inline int bgc_vector3_get_normalized_fp64(const BgcVector3FP64* vector, BgcVector3FP64* normalized)
 {
     const double square_modulus = bgc_vector3_get_square_modulus_fp64(vector);