From e6a94ab8d911cc1b526f69532b6424067240e534 Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Fri, 21 Mar 2025 03:34:20 +0700 Subject: [PATCH] =?UTF-8?q?=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=B5?= =?UTF-8?q?=D0=BD=D0=B8=D0=B5=20=D0=B1=D0=B0=D0=B7=D0=B8=D1=81=D0=BE=D0=B2?= =?UTF-8?q?=20=D0=BA=D0=B0=D0=BA=20=D0=B2=D1=81=D0=BF=D0=BE=D0=BC=D0=BE?= =?UTF-8?q?=D0=B3=D0=B0=D1=82=D0=B5=D0=BB=D1=8C=D0=BD=D0=BE=D0=B9=20=D1=81?= =?UTF-8?q?=D1=82=D1=80=D1=83=D0=BA=D1=82=D1=83=D1=80=D1=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- basic-geometry/basic-geometry.vcxproj | 2 + basic-geometry/basic-geometry.vcxproj.filters | 6 + basic-geometry/basis3.c | 232 ++++++++++++++++++ basic-geometry/basis3.h | 35 +++ basic-geometry/rotation3.h | 16 +- basic-geometry/utilities.c | 1 + basic-geometry/utilities.h | 14 ++ basic-geometry/vector3.c | 7 +- basic-geometry/vector3.h | 46 +++- 9 files changed, 345 insertions(+), 14 deletions(-) create mode 100644 basic-geometry/basis3.c create mode 100644 basic-geometry/basis3.h diff --git a/basic-geometry/basic-geometry.vcxproj b/basic-geometry/basic-geometry.vcxproj index 3614dc7..b8039b9 100644 --- a/basic-geometry/basic-geometry.vcxproj +++ b/basic-geometry/basic-geometry.vcxproj @@ -21,6 +21,7 @@ + @@ -40,6 +41,7 @@ + diff --git a/basic-geometry/basic-geometry.vcxproj.filters b/basic-geometry/basic-geometry.vcxproj.filters index 440dcb1..fb935d0 100644 --- a/basic-geometry/basic-geometry.vcxproj.filters +++ b/basic-geometry/basic-geometry.vcxproj.filters @@ -69,6 +69,9 @@ Файлы заголовков + + Файлы заголовков + @@ -110,5 +113,8 @@ Исходные файлы + + Исходные файлы + \ No newline at end of file diff --git a/basic-geometry/basis3.c b/basic-geometry/basis3.c new file mode 100644 index 0000000..566c909 --- /dev/null +++ b/basic-geometry/basis3.c @@ -0,0 +1,232 @@ +#include +#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; +} diff --git a/basic-geometry/basis3.h b/basic-geometry/basis3.h new file mode 100644 index 0000000..b1b995d --- /dev/null +++ b/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 diff --git a/basic-geometry/rotation3.h b/basic-geometry/rotation3.h index 3ba45d1..d55cdc3 100644 --- a/basic-geometry/rotation3.h +++ b/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; - rotation->axis.x2 = axis->x2; - rotation->axis.x3 = axis->x3; + bgc_vector3_copy_fp32(axis, &rotation->axis); - 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; - rotation->axis.x2 = axis->x2; - rotation->axis.x3 = axis->x3; + bgc_vector3_copy_fp64(axis, &rotation->axis); - 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 { diff --git a/basic-geometry/utilities.c b/basic-geometry/utilities.c index e863c4b..ec89f4f 100644 --- a/basic-geometry/utilities.c +++ b/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); diff --git a/basic-geometry/utilities.h b/basic-geometry/utilities.h index 4c4d3cb..d12b091 100644 --- a/basic-geometry/utilities.h +++ b/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); diff --git a/basic-geometry/vector3.c b/basic-geometry/vector3.c index 8c269ed..9b97d80 100644 --- a/basic-geometry/vector3.c +++ b/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_fp64(const BgcVector3FP64* vector, BgcVector3FP64* normalized); +extern inline int bgc_vector3_normalize_fp32(BgcVector3FP32* vector); +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); diff --git a/basic-geometry/vector3.h b/basic-geometry/vector3.h index 52fe86c..33f846e 100644 --- a/basic-geometry/vector3.h +++ b/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);