Kimelas/bgc-c
2
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Переделка методов turn3_find_direction_difference, возврат функций матриц поворота для turn3

Browse source
This commit is contained in:
Andrey Pokidov 2026-02-07 02:53:21 +07:00
parent 2ce4b64ca3
commit 78d1661c5d
2 changed files with 198 additions and 106 deletions
Download patch file Download diff file Expand all files Collapse all files

153
basic-geometry/turn3.c
View file

@ -201,92 +201,95 @@ void bgc_fp64_turn3_set_rotation(BGC_FP64_Turn3* turn, const double x1, const do
}
}
// ========= Make Direction Difference ========== //
static int _bgc_fp32_turn3_make_direction_turn(BGC_FP32_Turn3* turn, const BGC_FP32_Vector3* start, const BGC_FP32_Vector3* end, const float square_modulus_product)
{
BGC_FP32_Vector3 orthogonal_axis;
bgc_fp32_vector3_get_cross_product(&orthogonal_axis, start, end);
const float scalar_product = bgc_fp32_vector3_get_dot_product(start, end);
const float square_modulus = bgc_fp32_vector3_get_square_modulus(&orthogonal_axis);
const float square_sine = square_modulus / square_modulus_product;
if (square_sine > BGC_FP32_SQUARE_EPSILON) {
const float cosine = scalar_product / sqrtf(square_modulus_product);
const float angle = 0.5f * atan2f(sqrtf(square_sine), cosine);
const float multiplier = sinf(angle) * sqrtf(1.0f / square_modulus);
bgc_fp32_turn3_set_raw_values(turn, cosf(angle), orthogonal_axis.x1 * multiplier, orthogonal_axis.x2 * multiplier, orthogonal_axis.x3 * multiplier);
return BGC_SOME_TURN;
}
if (scalar_product < 0.0f) {
return BGC_OPPOSITE;
}
bgc_fp32_turn3_reset(turn);
return BGC_ZERO_TURN;
}
static int _bgc_fp64_turn3_make_direction_turn(BGC_FP64_Turn3* versor, const BGC_FP64_Vector3* start, const BGC_FP64_Vector3* end, const double square_modulus_product)
{
BGC_FP64_Vector3 orthogonal_axis;
bgc_fp64_vector3_get_cross_product(&orthogonal_axis, start, end);
const double scalar_product = bgc_fp64_vector3_get_dot_product(start, end);
const double square_modulus = bgc_fp64_vector3_get_square_modulus(&orthogonal_axis);
const double square_sine = square_modulus / square_modulus_product;
if (square_sine > BGC_FP64_SQUARE_EPSILON) {
const double cosine = scalar_product / sqrt(square_modulus_product);
const double angle = 0.5 * atan2(sqrt(square_sine), cosine);
const double multiplier = sin(angle) * sqrt(1.0f / square_modulus);
bgc_fp64_turn3_set_raw_values(versor, cos(angle), orthogonal_axis.x1 * multiplier, orthogonal_axis.x2 * multiplier, orthogonal_axis.x3 * multiplier);
return BGC_SOME_TURN;
}
if (scalar_product < 0.0) {
return BGC_OPPOSITE;
}
bgc_fp64_turn3_reset(versor);
return BGC_ZERO_TURN;
}
// ========= Find Direction Difference ========== //
int bgc_fp32_turn3_find_direction_difference(BGC_FP32_Turn3* difference, const BGC_FP32_Vector3* start, const BGC_FP32_Vector3* end)
int bgc_fp32_turn3_find_direction_difference(BGC_FP32_Turn3* turn, const BGC_FP32_Vector3* first, const BGC_FP32_Vector3* second)
{
const float start_square_modulus = bgc_fp32_vector3_get_square_modulus(start);
const float end_square_modulus = bgc_fp32_vector3_get_square_modulus(end);
const float first_square_modulus = bgc_fp32_vector3_get_square_modulus(first);
bgc_fp32_turn3_reset(turn);
if (start_square_modulus <= BGC_FP32_SQUARE_EPSILON || end_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
bgc_fp32_turn3_reset(difference);
return BGC_ZERO_TURN;
if (first_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
return BGC_ERROR_TURN3_FIRST_VECTOR_ZERO;
}
return _bgc_fp32_turn3_make_direction_turn(difference, start, end, start_square_modulus * end_square_modulus);
const float second_square_modulus = bgc_fp32_vector3_get_square_modulus(second);
if (second_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
return BGC_ERROR_TURN3_SECOND_VECTOR_ZERO;
}
BGC_FP32_Vector3 axis;
bgc_fp32_vector3_get_cross_product(&axis, first, second);
const float square_product = first_square_modulus * second_square_modulus;
const float dot_product = bgc_fp32_vector3_get_dot_product(first, second);
const float axis_square_modulus = bgc_fp32_vector3_get_square_modulus(&axis);
if (axis_square_modulus <= BGC_FP32_SQUARE_EPSILON * square_product) {
if (dot_product < 0.0f) {
return BGC_ERROR_TURN3_VECTORS_OPPOSITE;
}
return BGC_SUCCESS;
}
const float axis_modulus = sqrtf(axis_square_modulus);
const float trigonometry_fix = sqrtf(1.0f / square_product);
const float angle = 0.5f * atan2f(axis_modulus * trigonometry_fix, dot_product * trigonometry_fix);
const float vector_multiplier = sinf(angle) / axis_modulus;
bgc_fp32_turn3_set_raw_values(turn, cosf(angle), axis.x1 * vector_multiplier, axis.x2 * vector_multiplier, axis.x3 * vector_multiplier);
return BGC_SUCCESS;
}
int bgc_fp64_turn3_find_direction_difference(BGC_FP64_Turn3* difference, const BGC_FP64_Vector3* start, const BGC_FP64_Vector3* end)
int bgc_fp64_turn3_find_direction_difference(BGC_FP64_Turn3* turn, const BGC_FP64_Vector3* first, const BGC_FP64_Vector3* second)
{
const double start_square_modulus = bgc_fp64_vector3_get_square_modulus(start);
const double end_square_modulus = bgc_fp64_vector3_get_square_modulus(end);
const double first_square_modulus = bgc_fp64_vector3_get_square_modulus(first);
if (start_square_modulus <= BGC_FP64_SQUARE_EPSILON || end_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
bgc_fp64_turn3_reset(difference);
return BGC_ZERO_TURN;
bgc_fp64_turn3_reset(turn);
if (first_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
return BGC_ERROR_TURN3_FIRST_VECTOR_ZERO;
}
return _bgc_fp64_turn3_make_direction_turn(difference, start, end, start_square_modulus * end_square_modulus);
const double second_square_modulus = bgc_fp64_vector3_get_square_modulus(second);
if (second_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
return BGC_ERROR_TURN3_SECOND_VECTOR_ZERO;
}
BGC_FP64_Vector3 axis;
bgc_fp64_vector3_get_cross_product(&axis, first, second);
const double square_product = first_square_modulus * second_square_modulus;
const double dot_product = bgc_fp64_vector3_get_dot_product(first, second);
const double axis_square_modulus = bgc_fp64_vector3_get_square_modulus(&axis);
if (axis_square_modulus <= BGC_FP64_SQUARE_EPSILON * square_product) {
bgc_fp64_turn3_reset(turn);
if (dot_product < 0.0) {
return BGC_ERROR_TURN3_VECTORS_OPPOSITE;
}
return BGC_SUCCESS;
}
const double axis_modulus = sqrt(axis_square_modulus);
const double trigonometry_fix = sqrt(1.0 / square_product);
const double angle = 0.5 * atan2(axis_modulus * trigonometry_fix, dot_product * trigonometry_fix);
const double vector_multiplier = sin(angle) / axis_modulus;
bgc_fp64_turn3_set_raw_values(turn, cos(angle), axis.x1 * vector_multiplier, axis.x2 * vector_multiplier, axis.x3 * vector_multiplier);
return BGC_SUCCESS;
}
// ============ Make Orthogonal Pair ============ //
@ -334,7 +337,7 @@ static inline int _bgc_fp64_turn3_get_orthogonal_pair(BGC_FP64_Vector3* unit_mai
return _BGC_ERROR_TURN3_EMPTY_BRANCH;
}
bgc_fp64_vector3_multiply(unit_main, main, sqrtf(1.0 / main_square_modulus));
bgc_fp64_vector3_multiply(unit_main, main, sqrt(1.0 / main_square_modulus));
bgc_fp64_vector3_add_scaled(unit_branch, branch, unit_main, -bgc_fp64_vector3_get_dot_product(branch, unit_main));