From 9d7011e81e286f5f2cec221fcf9a835b249a1d00 Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Mon, 17 Mar 2025 09:56:56 +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=D1=81=D1=84=D0=B5=D1=80=D0=B8=D1=87=D0=B5?= =?UTF-8?q?=D1=81=D0=BA=D0=BE=D0=B9=20=D0=B8=D0=BD=D1=82=D0=B5=D1=80=D0=BF?= =?UTF-8?q?=D0=BE=D0=BB=D1=8F=D1=86=D0=B8=D0=B8,=20=D0=BF=D0=B5=D1=80?= =?UTF-8?q?=D0=B5=D1=85=D0=BE=D0=B4=20=D0=BE=D1=82=20=D0=BF=D1=80=D0=B8?= =?UTF-8?q?=D0=BC=D0=B5=D0=BD=D0=B5=D0=BD=D0=B8=D1=8F=20acos=20=D0=BA=20?= =?UTF-8?q?=D0=BF=D1=80=D0=B8=D0=BC=D0=B5=D0=BD=D0=B5=D0=BD=D0=B8=D1=8E=20?= =?UTF-8?q?atan2,=20=D0=B8=D1=81=D0=BF=D1=80=D0=B0=D0=B2=D0=BB=D0=B5=D0=BD?= =?UTF-8?q?=D0=B8=D0=B5=20=D0=BE=D1=88=D0=B8=D0=B1=D0=BE=D0=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- basic-geometry-dev/main.c | 41 +++-- basic-geometry/basic-geometry.h | 3 +- basic-geometry/basic-geometry.vcxproj | 10 +- basic-geometry/basic-geometry.vcxproj.filters | 30 ++-- basic-geometry/complex.c | 4 +- basic-geometry/complex.h | 4 +- basic-geometry/quaternion.c | 92 +++++++++- basic-geometry/quaternion.h | 12 +- basic-geometry/slerp.c | 129 ++++++++++++++ basic-geometry/slerp.h | 94 ++++++++++ basic-geometry/utilities.h | 2 + basic-geometry/vector2.c | 40 ++--- basic-geometry/vector2.h | 4 +- basic-geometry/vector3.c | 44 ++--- basic-geometry/vector3.h | 4 +- basic-geometry/versor.c | 165 +++++++++++++----- basic-geometry/versor.h | 14 +- 17 files changed, 558 insertions(+), 134 deletions(-) create mode 100644 basic-geometry/slerp.c create mode 100644 basic-geometry/slerp.h diff --git a/basic-geometry-dev/main.c b/basic-geometry-dev/main.c index 901b4e3..6dcf2ae 100644 --- a/basic-geometry-dev/main.c +++ b/basic-geometry-dev/main.c @@ -138,17 +138,36 @@ int main() { return 0; } */ - +/* int main() { - const float exponent = 2.0f; - - BgcVersorFP32 turn, result; - - bgc_versor_set_turn_fp32(0, 0, 1, 120, BGC_ANGLE_UNIT_DEGREES, &turn); - - bgc_versor_get_exponation_fp32(&turn, exponent, &result); - - printf("(%f, %f, %f, %f) ^ %f = (%f, %f, %f, %f)\n", turn.s0, turn.x1, turn.x2, turn.x3, exponent, result.s0, result.x1, result.x2, result.x3); - + BgcVersorFP32 start = { 1.0f, 0.0f, 0.0f, 0.0f }; + BgcVersorFP32 end = { 0.0f, 1.0f, 0.0f, 0.0f }; + BgcVersorFP32 result; + bgc_versor_spherical_interpolation_fp32(&start, &end, 0.5f, &result); + printf("Result: %0.12f, %0.12f, %0.12f, %0.12f\n", result.s0, result.x1, result.x2, result.x3); + return 0; +} +*/ + +int main() { + //BgcVersorFP32 start = { 1.0f, 0.0f, 0.0f, 0.0f }; + //BgcVersorFP32 end = { 0.0f, 1.0f, 0.0f, 0.0f }; + /* + BgcVersorFP32 start = { 1.0f, 0.0f, 0.0f, 0.0f }; + BgcVersorFP32 end = { 0.9999f, 0.01414f, 0.0f, 0.0f }; + BgcSlerpFP32 slerp; + BgcVersorFP32 result; + bgc_slerp_make_fp32(&start, &end, &slerp); + bgc_slerp_get_turn_for_phase_fp32(&slerp, 0.5f, &result); + printf("Result: %0.12f, %0.12f, %0.12f, %0.12f\n", result.s0, result.x1, result.x2, result.x3); + */ + + BgcVersorFP64 start = { 1.0, 0.0, 0.0, 0.0 }; + BgcVersorFP64 end = { -0.707, 0.707, 0.0, 0.0 }; + BgcVersorFP64 result; + BgcSlerpFP64 slerp; + bgc_slerp_make_full_fp64(&start, &end, &slerp); + bgc_slerp_get_turn_for_phase_fp64(&slerp, 0.5f, &result); + printf("Result: %0.15f, %0.15f, %0.15f, %0.15f\n", result.s0, result.x1, result.x2, result.x3); return 0; } diff --git a/basic-geometry/basic-geometry.h b/basic-geometry/basic-geometry.h index fb83f61..fcaf5bb 100644 --- a/basic-geometry/basic-geometry.h +++ b/basic-geometry/basic-geometry.h @@ -13,7 +13,7 @@ #include "./matrix2x3.h" #include "./matrix3x2.h" #include "./matrix3x3.h" - + #include "./complex.h" #include "./cotes-number.h" @@ -21,5 +21,6 @@ #include "./quaternion.h" #include "./versor.h" +#include "./slerp.h" #endif diff --git a/basic-geometry/basic-geometry.vcxproj b/basic-geometry/basic-geometry.vcxproj index 94fb723..3614dc7 100644 --- a/basic-geometry/basic-geometry.vcxproj +++ b/basic-geometry/basic-geometry.vcxproj @@ -21,8 +21,8 @@ - - + + @@ -31,14 +31,15 @@ + - - + + @@ -47,6 +48,7 @@ + diff --git a/basic-geometry/basic-geometry.vcxproj.filters b/basic-geometry/basic-geometry.vcxproj.filters index 260e3e3..440dcb1 100644 --- a/basic-geometry/basic-geometry.vcxproj.filters +++ b/basic-geometry/basic-geometry.vcxproj.filters @@ -18,12 +18,12 @@ Файлы заголовков - - Файлы заголовков - - - Файлы заголовков - + + Файлы заголовков + + + Файлы заголовков + Файлы заголовков @@ -60,17 +60,20 @@ Файлы заголовков + + Исходные файлы + + + Исходные файлы + + + Файлы заголовков + Исходные файлы - - Исходные файлы - - - Исходные файлы - Исходные файлы @@ -104,5 +107,8 @@ Исходные файлы + + Исходные файлы + \ No newline at end of file diff --git a/basic-geometry/complex.c b/basic-geometry/complex.c index 59c6979..ef3a0bd 100644 --- a/basic-geometry/complex.c +++ b/basic-geometry/complex.c @@ -69,8 +69,8 @@ extern inline void bgc_complex_get_mean_of_two_fp64(const BgcComplexFP64* number extern inline void bgc_complex_get_mean_of_three_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, const BgcComplexFP32* number3, BgcComplexFP32* mean); extern inline void bgc_complex_get_mean_of_three_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, const BgcComplexFP64* number3, BgcComplexFP64* mean); -extern inline void bgc_complex_get_linear_interpolation_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, const float phase, BgcComplexFP32* interpolation); -extern inline void bgc_complex_get_linear_interpolation_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, const double phase, BgcComplexFP64* interpolation); +extern inline void bgc_complex_interpolate_linearly_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, const float phase, BgcComplexFP32* interpolation); +extern inline void bgc_complex_interpolate_linearly_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, const double phase, BgcComplexFP64* interpolation); extern inline void bgc_complex_minimize_fp32(const BgcComplexFP32* number, BgcComplexFP32* minimal); extern inline void bgc_complex_minimize_fp64(const BgcComplexFP64* number, BgcComplexFP64* minimal); diff --git a/basic-geometry/complex.h b/basic-geometry/complex.h index c7b77ff..e551609 100644 --- a/basic-geometry/complex.h +++ b/basic-geometry/complex.h @@ -428,7 +428,7 @@ inline void bgc_complex_get_mean_of_three_fp64(const BgcComplexFP64* number1, co // =================== Linear =================== // -inline void bgc_complex_get_linear_interpolation_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, const float phase, BgcComplexFP32* interpolation) +inline void bgc_complex_interpolate_linearly_fp32(const BgcComplexFP32* number1, const BgcComplexFP32* number2, const float phase, BgcComplexFP32* interpolation) { const float counterphase = 1.0f - phase; @@ -436,7 +436,7 @@ inline void bgc_complex_get_linear_interpolation_fp32(const BgcComplexFP32* numb interpolation->imaginary = number1->imaginary * counterphase + number2->imaginary * phase; } -inline void bgc_complex_get_linear_interpolation_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, const double phase, BgcComplexFP64* interpolation) +inline void bgc_complex_interpolate_linearly_fp64(const BgcComplexFP64* number1, const BgcComplexFP64* number2, const double phase, BgcComplexFP64* interpolation) { const double counterphase = 1.0 - phase; diff --git a/basic-geometry/quaternion.c b/basic-geometry/quaternion.c index 963dba7..df057b2 100644 --- a/basic-geometry/quaternion.c +++ b/basic-geometry/quaternion.c @@ -1,3 +1,4 @@ +#include #include "quaternion.h" extern inline void bgc_quaternion_reset_fp32(BgcQuaternionFP32* quaternion); @@ -63,8 +64,8 @@ extern inline void bgc_quaternion_multiply_fp64(const BgcQuaternionFP64* multipl extern inline void bgc_quaternion_divide_fp32(const BgcQuaternionFP32* dividend, const float divisor, BgcQuaternionFP32* quotient); extern inline void bgc_quaternion_divide_fp64(const BgcQuaternionFP64* dividend, const double divisor, BgcQuaternionFP64* quotient); -extern inline void bgc_quaternion_get_linear_interpolation_fp32(const BgcQuaternionFP32* vector1, const BgcQuaternionFP32* vector2, const float phase, BgcQuaternionFP32* interpolation); -extern inline void bgc_quaternion_get_linear_interpolation_fp64(const BgcQuaternionFP64* vector1, const BgcQuaternionFP64* vector2, const double phase, BgcQuaternionFP64* interpolation); +extern inline void bgc_quaternion_interpolate_linearly_fp32(const BgcQuaternionFP32* vector1, const BgcQuaternionFP32* vector2, const float phase, BgcQuaternionFP32* interpolation); +extern inline void bgc_quaternion_interpolate_linearly_fp64(const BgcQuaternionFP64* vector1, const BgcQuaternionFP64* vector2, const double phase, BgcQuaternionFP64* interpolation); extern inline int bgc_quaternion_get_rotation_matrix_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* rotation); extern inline int bgc_quaternion_get_rotation_matrix_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* rotation); @@ -75,6 +76,89 @@ extern inline int bgc_quaternion_get_reverse_matrix_fp64(const BgcQuaternionFP64 extern inline int bgc_quaternion_get_both_matrixes_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* rotation, BgcMatrix3x3FP32* reverse); extern inline int bgc_quaternion_get_both_matrixes_fp64(const BgcQuaternionFP64* quaternion, BgcMatrix3x3FP64* rotation, BgcMatrix3x3FP64* reverse); +extern inline int bgc_quaternion_are_close_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2); +extern inline int bgc_quaternion_are_close_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2); -extern inline int bgc_quaternion_are_close_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2); -extern inline int bgc_quaternion_are_close_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2); +// =============== Get Exponation =============== // + +int bgc_quaternion_get_exponation_fp32(const BgcQuaternionFP32* base, const float exponent, BgcQuaternionFP32* power) +{ + const float s0s0 = base->s0 * base->s0; + const float x1x1 = base->x1 * base->x1; + const float x2x2 = base->x2 * base->x2; + const float x3x3 = base->x3 * base->x3; + + const float square_vector = x1x1 + (x2x2 + x3x3); + const float square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3); + + // square_modulus != square_modulus means checking for NaN value at square_modulus + if (square_modulus != square_modulus) { + return 0; + } + + if (square_vector <= BGC_SQUARE_EPSYLON_FP32) { + if (base->s0 < 0.0f) { + return 0; + } + + power->s0 = powf(base->s0, exponent); + power->x1 = 0.0f; + power->x2 = 0.0f; + power->x3 = 0.0f; + + return 1; + } + + const float vector_modulus = sqrtf(square_vector); + const float power_angle = atan2f(vector_modulus, base->s0) * exponent; + const float power_modulus = powf(square_modulus, 0.5f * exponent); + const float multiplier = power_modulus * sinf(power_angle) / vector_modulus; + + power->s0 = power_modulus * cosf(power_angle); + power->x1 = base->x1 * multiplier; + power->x2 = base->x2 * multiplier; + power->x3 = base->x3 * multiplier; + + return 1; +} + +int bgc_quaternion_get_exponation_fp64(const BgcQuaternionFP64* base, const double exponent, BgcQuaternionFP64* power) +{ + const double s0s0 = base->s0 * base->s0; + const double x1x1 = base->x1 * base->x1; + const double x2x2 = base->x2 * base->x2; + const double x3x3 = base->x3 * base->x3; + + const double square_vector = x1x1 + (x2x2 + x3x3); + const double square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3); + + // square_modulus != square_modulus means checking for NaN value at square_modulus + if (square_modulus != square_modulus) { + return 0; + } + + if (square_vector <= BGC_SQUARE_EPSYLON_FP64) { + if (base->s0 < 0.0) { + return 0; + } + + power->s0 = pow(base->s0, exponent); + power->x1 = 0.0; + power->x2 = 0.0; + power->x3 = 0.0; + + return 1; + } + + const double vector_modulus = sqrt(square_vector); + const double power_angle = atan2(vector_modulus, base->s0) * exponent; + const double power_modulus = pow(square_modulus, 0.5 * exponent); + const double multiplier = power_modulus * sin(power_angle) / vector_modulus; + + power->s0 = power_modulus * cos(power_angle); + power->x1 = base->x1 * multiplier; + power->x2 = base->x2 * multiplier; + power->x3 = base->x3 * multiplier; + + return 1; +} diff --git a/basic-geometry/quaternion.h b/basic-geometry/quaternion.h index 54be3fa..ebf06db 100644 --- a/basic-geometry/quaternion.h +++ b/basic-geometry/quaternion.h @@ -473,9 +473,9 @@ inline void bgc_quaternion_divide_fp64(const BgcQuaternionFP64* dividend, const bgc_quaternion_multiply_fp64(dividend, 1.0 / divisor, quotient); } -// =================== Linear =================== // +// ============ Linear Interpolation ============ // -inline void bgc_quaternion_get_linear_interpolation_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2, const float phase, BgcQuaternionFP32* interpolation) +inline void bgc_quaternion_interpolate_linearly_fp32(const BgcQuaternionFP32* quaternion1, const BgcQuaternionFP32* quaternion2, const float phase, BgcQuaternionFP32* interpolation) { const float counterphase = 1.0f - phase; @@ -485,7 +485,7 @@ inline void bgc_quaternion_get_linear_interpolation_fp32(const BgcQuaternionFP32 interpolation->x3 = quaternion1->x3 * counterphase + quaternion2->x3 * phase; } -inline void bgc_quaternion_get_linear_interpolation_fp64(const BgcQuaternionFP64* quaternion1, const BgcQuaternionFP64* quaternion2, const double phase, BgcQuaternionFP64* interpolation) +inline void bgc_quaternion_interpolate_linearly_fp64(const BgcQuaternionFP64* quaternion1, const BgcQuaternionFP64* quaternion2, const double phase, BgcQuaternionFP64* interpolation) { const double counterphase = 1.0 - phase; @@ -495,6 +495,12 @@ inline void bgc_quaternion_get_linear_interpolation_fp64(const BgcQuaternionFP64 interpolation->x3 = quaternion1->x3 * counterphase + quaternion2->x3 * phase; } +// =============== Get Exponation =============== // + +int bgc_quaternion_get_exponation_fp32(const BgcQuaternionFP32* base, const float exponent, BgcQuaternionFP32* power); + +int bgc_quaternion_get_exponation_fp64(const BgcQuaternionFP64* base, const double exponent, BgcQuaternionFP64* power); + // ============ Get Rotation Matrix ============= // inline int bgc_quaternion_get_rotation_matrix_fp32(const BgcQuaternionFP32* quaternion, BgcMatrix3x3FP32* rotation) diff --git a/basic-geometry/slerp.c b/basic-geometry/slerp.c new file mode 100644 index 0000000..b318059 --- /dev/null +++ b/basic-geometry/slerp.c @@ -0,0 +1,129 @@ +#include "./slerp.h" + +extern inline void bgc_slerp_reset_fp32(BgcSlerpFP32* slerp); +extern inline void bgc_slerp_reset_fp64(BgcSlerpFP64* slerp); + +extern inline bgc_slerp_get_turn_for_phase_fp32(const BgcSlerpFP32* slerp, const float phase, BgcVersorFP32* result); +extern inline bgc_slerp_get_turn_for_phase_fp64(const BgcSlerpFP64* slerp, const double phase, BgcVersorFP64* result); + +void bgc_slerp_make_full_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, BgcSlerpFP32* slerp) +{ + BgcVersorFP32 delta; + + bgc_versor_exclude_fp32(end, start, &delta); + + const float square_vector = delta.x1 * delta.x1 + delta.x2 * delta.x2 + delta.x3 * delta.x3; + + if (square_vector <= BGC_SQUARE_EPSYLON_FP32 || square_vector != square_vector) { + bgc_slerp_reset_fp32(slerp); + return; + } + + const float vector_modulus = sqrtf(square_vector); + + slerp->radians = atan2f(vector_modulus, delta.s0); + + const float mutliplier = 1.0f / vector_modulus; + + slerp->s0_cos_weight = start->s0; + slerp->x1_cos_weight = start->x1; + slerp->x2_cos_weight = start->x2; + slerp->x3_cos_weight = start->x3; + + slerp->s0_sin_weight = -mutliplier * (delta.x1 * start->x1 + delta.x2 * start->x2 + delta.x3 * start->x3); + slerp->x1_sin_weight = mutliplier * (delta.x1 * start->s0 + delta.x2 * start->x3 - delta.x3 * start->x2); + slerp->x2_sin_weight = mutliplier * (delta.x2 * start->s0 - delta.x1 * start->x3 + delta.x3 * start->x1); + slerp->x3_sin_weight = mutliplier * (delta.x3 * start->s0 - delta.x2 * start->x1 + delta.x1 * start->x2); +} + +void bgc_slerp_make_full_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, BgcSlerpFP64* slerp) +{ + BgcVersorFP64 delta; + + bgc_versor_exclude_fp64(end, start, &delta); + + const double square_vector = delta.x1 * delta.x1 + delta.x2 * delta.x2 + delta.x3 * delta.x3; + + if (square_vector <= BGC_SQUARE_EPSYLON_FP64 || square_vector != square_vector) { + bgc_slerp_reset_fp64(slerp); + return; + } + + const double vector_modulus = sqrt(square_vector); + + slerp->radians = atan2(vector_modulus, delta.s0); + + const double mutliplier = 1.0 / vector_modulus; + + slerp->s0_cos_weight = start->s0; + slerp->x1_cos_weight = start->x1; + slerp->x2_cos_weight = start->x2; + slerp->x3_cos_weight = start->x3; + + slerp->s0_sin_weight = -mutliplier * (delta.x1 * start->x1 + delta.x2 * start->x2 + delta.x3 * start->x3); + slerp->x1_sin_weight = mutliplier * (delta.x1 * start->s0 + delta.x2 * start->x3 - delta.x3 * start->x2); + slerp->x2_sin_weight = mutliplier * (delta.x2 * start->s0 - delta.x1 * start->x3 + delta.x3 * start->x1); + slerp->x3_sin_weight = mutliplier * (delta.x3 * start->s0 - delta.x2 * start->x1 + delta.x1 * start->x2); +} + +void bgc_slerp_make_shortened_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, BgcSlerpFP32* slerp) +{ + BgcVersorFP32 delta; + + bgc_versor_exclude_fp32(end, start, &delta); + bgc_versor_shorten_fp32(&delta, &delta); + + const float square_vector = delta.x1 * delta.x1 + delta.x2 * delta.x2 + delta.x3 * delta.x3; + + if (square_vector <= BGC_SQUARE_EPSYLON_FP32 || square_vector != square_vector) { + bgc_slerp_reset_fp32(slerp); + return; + } + + const float vector_modulus = sqrtf(square_vector); + + slerp->radians = atan2f(vector_modulus, delta.s0); + + const float mutliplier = 1.0f / vector_modulus; + + slerp->s0_cos_weight = start->s0; + slerp->x1_cos_weight = start->x1; + slerp->x2_cos_weight = start->x2; + slerp->x3_cos_weight = start->x3; + + slerp->s0_sin_weight = -mutliplier * (delta.x1 * start->x1 + delta.x2 * start->x2 + delta.x3 * start->x3); + slerp->x1_sin_weight = mutliplier * (delta.x1 * start->s0 + delta.x2 * start->x3 - delta.x3 * start->x2); + slerp->x2_sin_weight = mutliplier * (delta.x2 * start->s0 - delta.x1 * start->x3 + delta.x3 * start->x1); + slerp->x3_sin_weight = mutliplier * (delta.x3 * start->s0 - delta.x2 * start->x1 + delta.x1 * start->x2); +} + +void bgc_slerp_make_shortened_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, BgcSlerpFP64* slerp) +{ + BgcVersorFP64 delta; + + bgc_versor_exclude_fp64(end, start, &delta); + bgc_versor_shorten_fp64(&delta, &delta); + + const double square_vector = delta.x1 * delta.x1 + delta.x2 * delta.x2 + delta.x3 * delta.x3; + + if (square_vector <= BGC_SQUARE_EPSYLON_FP64 || square_vector != square_vector) { + bgc_slerp_reset_fp64(slerp); + return; + } + + const double vector_modulus = sqrt(square_vector); + + slerp->radians = atan2(vector_modulus, delta.s0); + + const double mutliplier = 1.0 / vector_modulus; + + slerp->s0_cos_weight = start->s0; + slerp->x1_cos_weight = start->x1; + slerp->x2_cos_weight = start->x2; + slerp->x3_cos_weight = start->x3; + + slerp->s0_sin_weight = -mutliplier * (delta.x1 * start->x1 + delta.x2 * start->x2 + delta.x3 * start->x3); + slerp->x1_sin_weight = mutliplier * (delta.x1 * start->s0 + delta.x2 * start->x3 - delta.x3 * start->x2); + slerp->x2_sin_weight = mutliplier * (delta.x2 * start->s0 - delta.x1 * start->x3 + delta.x3 * start->x1); + slerp->x3_sin_weight = mutliplier * (delta.x3 * start->s0 - delta.x2 * start->x1 + delta.x1 * start->x2); +} diff --git a/basic-geometry/slerp.h b/basic-geometry/slerp.h new file mode 100644 index 0000000..9289008 --- /dev/null +++ b/basic-geometry/slerp.h @@ -0,0 +1,94 @@ +#ifndef _BGC_VERSOR_SLERP_H_ +#define _BGC_VERSOR_SLERP_H_ + +#include "./versor.h" + +typedef struct { + float s0_cos_weight, s0_sin_weight; + float x1_cos_weight, x1_sin_weight; + float x2_cos_weight, x2_sin_weight; + float x3_cos_weight, x3_sin_weight; + float radians; +} BgcSlerpFP32; + +typedef struct { + double s0_cos_weight, s0_sin_weight; + double x1_cos_weight, x1_sin_weight; + double x2_cos_weight, x2_sin_weight; + double x3_cos_weight, x3_sin_weight; + double radians; +} BgcSlerpFP64; + +inline void bgc_slerp_reset_fp32(BgcSlerpFP32* slerp) +{ + slerp->s0_cos_weight = 1.0f; + slerp->s0_sin_weight = 0.0f; + + slerp->x1_cos_weight = 0.0f; + slerp->x1_sin_weight = 0.0f; + + slerp->x2_cos_weight = 0.0f; + slerp->x2_sin_weight = 0.0f; + + slerp->x3_cos_weight = 0.0f; + slerp->x3_sin_weight = 0.0f; + + slerp->radians = 0.0f; +} + +inline void bgc_slerp_reset_fp64(BgcSlerpFP64* slerp) +{ + slerp->s0_cos_weight = 1.0; + slerp->s0_sin_weight = 0.0; + + slerp->x1_cos_weight = 0.0; + slerp->x1_sin_weight = 0.0; + + slerp->x2_cos_weight = 0.0; + slerp->x2_sin_weight = 0.0; + + slerp->x3_cos_weight = 0.0; + slerp->x3_sin_weight = 0.0; + + slerp->radians = 0.0; +} + +void bgc_slerp_make_full_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, BgcSlerpFP32* slerp); + +void bgc_slerp_make_full_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, BgcSlerpFP64* slerp); + +void bgc_slerp_make_shortened_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, BgcSlerpFP32* slerp); + +void bgc_slerp_make_shortened_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, BgcSlerpFP64* slerp); + +inline bgc_slerp_get_turn_for_phase_fp32(const BgcSlerpFP32* slerp, const float phase, BgcVersorFP32* result) +{ + const float angle = slerp->radians * phase; + const float cosine = cosf(angle); + const float sine = sinf(angle); + + bgc_versor_set_values_fp32( + slerp->s0_cos_weight * cosine + slerp->s0_sin_weight * sine, + slerp->x1_cos_weight * cosine + slerp->x1_sin_weight * sine, + slerp->x2_cos_weight * cosine + slerp->x2_sin_weight * sine, + slerp->x3_cos_weight * cosine + slerp->x3_sin_weight * sine, + result + ); +} + +inline bgc_slerp_get_turn_for_phase_fp64(const BgcSlerpFP64* slerp, const double phase, BgcVersorFP64* result) +{ + const double angle = slerp->radians * phase; + const double cosine = cos(angle); + const double sine = sin(angle); + + bgc_versor_set_values_fp64( + slerp->s0_cos_weight * cosine + slerp->s0_sin_weight * sine, + slerp->x1_cos_weight * cosine + slerp->x1_sin_weight * sine, + slerp->x2_cos_weight * cosine + slerp->x2_sin_weight * sine, + slerp->x3_cos_weight * cosine + slerp->x3_sin_weight * sine, + result + ); +} + +#endif diff --git a/basic-geometry/utilities.h b/basic-geometry/utilities.h index 09d4866..4c4d3cb 100644 --- a/basic-geometry/utilities.h +++ b/basic-geometry/utilities.h @@ -11,6 +11,8 @@ #define BGC_ONE_SEVENTH_FP32 0.142857142857f #define BGC_ONE_NINETH_FP32 0.1111111111f +#define BGC_ARCCOSINE_PRECISION_LIMIT_FP32 0.70711f + #define BGC_GOLDEN_RATIO_HIGH_FP32 1.618034f #define BGC_GOLDEN_RATIO_LOW_FP32 0.618034f diff --git a/basic-geometry/vector2.c b/basic-geometry/vector2.c index b0b7c64..637de61 100644 --- a/basic-geometry/vector2.c +++ b/basic-geometry/vector2.c @@ -57,8 +57,8 @@ extern inline void bgc_vector2_get_mean_of_two_fp64(const BgcVector2FP64* vector extern inline void bgc_vector2_get_mean_of_three_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const BgcVector2FP32* vector3, BgcVector2FP32* mean); extern inline void bgc_vector2_get_mean_of_three_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const BgcVector2FP64* vector3, BgcVector2FP64* mean); -extern inline void bgc_vector2_get_linear_interpolation_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const float phase, BgcVector2FP32* interpolation); -extern inline void bgc_vector2_get_linear_interpolation_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const double phase, BgcVector2FP64* interpolation); +extern inline void bgc_vector2_interpolate_linearly_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const float phase, BgcVector2FP32* interpolation); +extern inline void bgc_vector2_interpolate_linearly_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const double phase, BgcVector2FP64* interpolation); extern inline void bgc_vector2_minimize_fp32(const BgcVector2FP32* vector, BgcVector2FP32* minimal); extern inline void bgc_vector2_minimize_fp64(const BgcVector2FP64* vector, BgcVector2FP64* minimal); @@ -90,52 +90,44 @@ float bgc_vector2_get_angle_fp32(const BgcVector2FP32* vector1, const BgcVector2 { const float square_modulus1 = bgc_vector2_get_square_modulus_fp32(vector1); - if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP32) { + // square_modulus1 != square_modulus1 is check for NaN value at square_modulus1 + if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP32 || square_modulus1 != square_modulus1) { return 0.0f; } const float square_modulus2 = bgc_vector2_get_square_modulus_fp32(vector2); - if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP32) { + // square_modulus2 != square_modulus2 is check for NaN value at square_modulus2 + if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP32 || square_modulus2 != square_modulus2) { return 0.0f; } - const float cosine = bgc_vector2_get_scalar_product_fp32(vector1, vector2) / sqrtf(square_modulus1 * square_modulus2); + const float scalar = bgc_vector2_get_scalar_product_fp32(vector1, vector2); - if (cosine >= 1.0f - BGC_EPSYLON_FP32) { - return 0.0f; - } + const float cross = bgc_vector2_get_cross_product_fp32(vector1, vector2); - if (cosine <= -1.0f + BGC_EPSYLON_FP32) { - return bgc_angle_get_half_circle_fp32(unit); - } - - return bgc_radians_to_units_fp32(acosf(cosine), unit); + return bgc_radians_to_units_fp32(atan2f(cross >= 0 ? cross : -cross, scalar), unit); } double bgc_vector2_get_angle_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const BgcAngleUnitEnum unit) { const double square_modulus1 = bgc_vector2_get_square_modulus_fp64(vector1); - if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP64) { + // square_modulus1 != square_modulus1 is check for NaN value at square_modulus1 + if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP64 || square_modulus1 != square_modulus1) { return 0.0; } const double square_modulus2 = bgc_vector2_get_square_modulus_fp64(vector2); - if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP64) { + // square_modulus2 != square_modulus2 is check for NaN value at square_modulus2 + if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP64 || square_modulus2 != square_modulus2) { return 0.0; } - const double cosine = bgc_vector2_get_scalar_product_fp64(vector1, vector2) / sqrt(square_modulus1 * square_modulus2); + const double scalar = bgc_vector2_get_scalar_product_fp64(vector1, vector2); - if (cosine >= 1.0 - BGC_EPSYLON_FP64) { - return 0.0; - } + const double cross = bgc_vector2_get_cross_product_fp64(vector1, vector2); - if (cosine <= -1.0 + BGC_EPSYLON_FP64) { - return bgc_angle_get_half_circle_fp64(unit); - } - - return bgc_radians_to_units_fp64(acos(cosine), unit); + return bgc_radians_to_units_fp64(atan2(cross >= 0 ? cross : -cross, scalar), unit); } diff --git a/basic-geometry/vector2.h b/basic-geometry/vector2.h index 8852721..1effc5e 100644 --- a/basic-geometry/vector2.h +++ b/basic-geometry/vector2.h @@ -314,7 +314,7 @@ inline void bgc_vector2_get_mean_of_three_fp64(const BgcVector2FP64* vector1, co // =================== Linear =================== // -inline void bgc_vector2_get_linear_interpolation_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const float phase, BgcVector2FP32* interpolation) +inline void bgc_vector2_interpolate_linearly_fp32(const BgcVector2FP32* vector1, const BgcVector2FP32* vector2, const float phase, BgcVector2FP32* interpolation) { const float counterphase = 1.0f - phase; @@ -322,7 +322,7 @@ inline void bgc_vector2_get_linear_interpolation_fp32(const BgcVector2FP32* vect interpolation->x2 = vector1->x2 * counterphase + vector2->x2 * phase; } -inline void bgc_vector2_get_linear_interpolation_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const double phase, BgcVector2FP64* interpolation) +inline void bgc_vector2_interpolate_linearly_fp64(const BgcVector2FP64* vector1, const BgcVector2FP64* vector2, const double phase, BgcVector2FP64* interpolation) { const double counterphase = 1.0 - phase; diff --git a/basic-geometry/vector3.c b/basic-geometry/vector3.c index e15ac8d..8c269ed 100644 --- a/basic-geometry/vector3.c +++ b/basic-geometry/vector3.c @@ -57,8 +57,8 @@ extern inline void bgc_vector3_get_mean_of_two_fp64(const BgcVector3FP64* vector extern inline void bgc_vector3_get_mean_of_three_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const BgcVector3FP32* vector3, BgcVector3FP32* result); extern inline void bgc_vector3_get_mean_of_three_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const BgcVector3FP64* vector3, BgcVector3FP64* result); -extern inline void bgc_vector3_get_linear_interpolation_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const float phase, BgcVector3FP32* interpolation); -extern inline void bgc_vector3_get_linear_interpolation_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const double phase, BgcVector3FP64* interpolation); +extern inline void bgc_vector3_interpolate_linearly_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const float phase, BgcVector3FP32* interpolation); +extern inline void bgc_vector3_interpolate_linearly_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const double phase, BgcVector3FP64* interpolation); extern inline void bgc_vector3_minimize_fp32(const BgcVector3FP32* vector, BgcVector3FP32* minimal); extern inline void bgc_vector3_minimize_fp64(const BgcVector3FP64* vector, BgcVector3FP64* minimal); @@ -96,52 +96,52 @@ float bgc_vector3_get_angle_fp32(const BgcVector3FP32* vector1, const BgcVector3 { const float square_modulus1 = bgc_vector3_get_square_modulus_fp32(vector1); - if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP32) { + // square_modulus1 != square_modulus1 is check for NaN value at square_modulus1 + if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP32 || square_modulus1 != square_modulus1) { return 0.0f; } const float square_modulus2 = bgc_vector3_get_square_modulus_fp32(vector2); - if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP32) { + // square_modulus2 != square_modulus2 is check for NaN value at square_modulus2 + if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP32 || square_modulus2 != square_modulus2) { return 0.0f; } - const float cosine = bgc_vector3_get_scalar_product_fp32(vector1, vector2) / sqrtf(square_modulus1 * square_modulus2); + BgcVector3FP32 cross_product; - if (cosine >= 1.0f - BGC_EPSYLON_FP32) { - return 0.0f; - } + bgc_vector3_get_cross_product_fp32(vector1, vector2, &cross_product); - if (cosine <= -1.0f + BGC_EPSYLON_FP32) { - return bgc_angle_get_half_circle_fp32(unit); - } + const float scalar = bgc_vector3_get_scalar_product_fp32(vector1, vector2); - return bgc_radians_to_units_fp32(acosf(cosine), unit); + const float cross = bgc_vector3_get_modulus_fp32(&cross_product); + + return bgc_radians_to_units_fp32(atan2f(cross, scalar), unit); } double bgc_vector3_get_angle_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const BgcAngleUnitEnum unit) { const double square_modulus1 = bgc_vector3_get_square_modulus_fp64(vector1); - if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP64) { + // square_modulus1 != square_modulus1 is check for NaN value at square_modulus1 + if (square_modulus1 <= BGC_SQUARE_EPSYLON_FP64 || square_modulus1 != square_modulus1) { return 0.0; } const double square_modulus2 = bgc_vector3_get_square_modulus_fp64(vector2); - if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP64) { + // square_modulus2 != square_modulus2 is check for NaN value at square_modulus2 + if (square_modulus2 <= BGC_SQUARE_EPSYLON_FP64 || square_modulus2 != square_modulus2) { return 0.0; } - const double cosine = bgc_vector3_get_scalar_product_fp64(vector1, vector2) / sqrt(square_modulus1 * square_modulus2); + BgcVector3FP64 cross_product; - if (cosine >= 1.0 - BGC_EPSYLON_FP64) { - return 0.0; - } + bgc_vector3_get_cross_product_fp64(vector1, vector2, &cross_product); - if (cosine <= -1.0 + BGC_EPSYLON_FP64) { - return bgc_angle_get_half_circle_fp64(unit); - } + const double scalar = bgc_vector3_get_scalar_product_fp64(vector1, vector2); - return bgc_radians_to_units_fp64(acos(cosine), unit); + const double cross = bgc_vector3_get_modulus_fp64(&cross_product); + + return bgc_radians_to_units_fp64(atan2(cross, scalar), unit); } diff --git a/basic-geometry/vector3.h b/basic-geometry/vector3.h index 33451fe..52fe86c 100644 --- a/basic-geometry/vector3.h +++ b/basic-geometry/vector3.h @@ -350,7 +350,7 @@ inline void bgc_vector3_get_mean_of_three_fp64(const BgcVector3FP64* vector1, co // =================== Linear =================== // -inline void bgc_vector3_get_linear_interpolation_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const float phase, BgcVector3FP32* interpolation) +inline void bgc_vector3_interpolate_linearly_fp32(const BgcVector3FP32* vector1, const BgcVector3FP32* vector2, const float phase, BgcVector3FP32* interpolation) { const float counterphase = 1.0f - phase; @@ -359,7 +359,7 @@ inline void bgc_vector3_get_linear_interpolation_fp32(const BgcVector3FP32* vect interpolation->x3 = vector1->x3 * counterphase + vector2->x3 * phase; } -inline void bgc_vector3_get_linear_interpolation_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const double phase, BgcVector3FP64* interpolation) +inline void bgc_vector3_interpolate_linearly_fp64(const BgcVector3FP64* vector1, const BgcVector3FP64* vector2, const double phase, BgcVector3FP64* interpolation) { const double counterphase = 1.0 - phase; diff --git a/basic-geometry/versor.c b/basic-geometry/versor.c index 8405012..842fc40 100644 --- a/basic-geometry/versor.c +++ b/basic-geometry/versor.c @@ -81,7 +81,6 @@ void _bgc_versor_normalize_fp32(const float square_modulus, _BgcDarkTwinVersorFP twin->x1 *= multiplier; twin->x2 *= multiplier; twin->x3 *= multiplier; - } void _bgc_versor_normalize_fp64(const double square_modulus, _BgcDarkTwinVersorFP64* twin) @@ -152,54 +151,22 @@ void bgc_versor_set_turn_fp64(const double x1, const double x2, const double x3, bgc_versor_set_values_fp64(cos(half_angle), x1 * multiplier, x2 * multiplier, x3 * multiplier, result); } -// ================ Get Rotation ================ // - -void bgc_versor_get_rotation_fp32(const BgcVersorFP32* versor, BgcRotation3FP32* result) -{ - if (versor->s0 <= -(1.0f - BGC_EPSYLON_FP32) || 1.0f - BGC_EPSYLON_FP32 <= versor->s0) { - bgc_rotation3_reset_fp32(result); - return; - } - - const float multiplier = sqrtf(1.0f / (versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3)); - - result->radians = 2.0f * acosf(versor->s0); - - result->axis.x1 = versor->x1 * multiplier; - result->axis.x2 = versor->x2 * multiplier; - result->axis.x3 = versor->x3 * multiplier; -} - -void bgc_versor_get_rotation_fp64(const BgcVersorFP64* versor, BgcRotation3FP64* result) -{ - if (versor->s0 <= -(1.0 - BGC_EPSYLON_FP64) || 1.0 - BGC_EPSYLON_FP64 <= versor->s0) { - bgc_rotation3_reset_fp64(result); - return; - } - - const double multiplier = sqrt(1.0 / (versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3)); - - result->radians = 2.0 * acos(versor->s0); - - result->axis.x1 = versor->x1 * multiplier; - result->axis.x2 = versor->x2 * multiplier; - result->axis.x3 = versor->x3 * multiplier; -} - // =============== Get Exponation =============== // void bgc_versor_get_exponation_fp32(const BgcVersorFP32* base, const float exponent, BgcVersorFP32* power) { const float square_vector = base->x1 * base->x1 + base->x2 * base->x2 + base->x3 * base->x3; - if (square_vector <= BGC_SQUARE_EPSYLON_FP32) { + if (square_vector <= BGC_SQUARE_EPSYLON_FP32 || square_vector != square_vector) { bgc_versor_reset_fp32(power); return; } - const float angle = acosf(base->s0) * exponent; + const float vector_modulus = sqrtf(square_vector); - const float multiplier = sinf(angle) / sqrtf(square_vector); + const float angle = atan2f(vector_modulus, base->s0) * exponent; + + const float multiplier = sinf(angle) / vector_modulus; bgc_versor_set_values_fp32(cosf(angle), base->x1 * multiplier, base->x2 * multiplier, base->x3 * multiplier, power); } @@ -208,14 +175,130 @@ void bgc_versor_get_exponation_fp64(const BgcVersorFP64* base, const double expo { const double square_vector = base->x1 * base->x1 + base->x2 * base->x2 + base->x3 * base->x3; - if (square_vector <= BGC_SQUARE_EPSYLON_FP64) { + if (square_vector <= BGC_SQUARE_EPSYLON_FP64 || square_vector != square_vector) { bgc_versor_reset_fp64(power); return; } - const double angle = acos(base->s0) * exponent; + const double vector_modulus = sqrt(square_vector); - const double multiplier = sin(angle) / sqrt(square_vector); + const double angle = atan2(vector_modulus, base->s0) * exponent; + + const double multiplier = sin(angle) / vector_modulus; bgc_versor_set_values_fp64(cos(angle), base->x1 * multiplier, base->x2 * multiplier, base->x3 * multiplier, power); } + +// ============ Sphere Interpolation ============ // + +void bgc_versor_spherically_interpolate_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, const float phase, BgcVersorFP32* result) +{ + const float delta_s0 = (end->s0 * start->s0 + end->x1 * start->x1) + (end->x2 * start->x2 + end->x3 * start->x3); + const float delta_x1 = (end->x1 * start->s0 + end->x3 * start->x2) - (end->s0 * start->x1 + end->x2 * start->x3); + const float delta_x2 = (end->x2 * start->s0 + end->x1 * start->x3) - (end->s0 * start->x2 + end->x3 * start->x1); + const float delta_x3 = (end->x3 * start->s0 + end->x2 * start->x1) - (end->s0 * start->x3 + end->x1 * start->x2); + + const float square_vector = delta_x1 * delta_x1 + delta_x2 * delta_x2 + delta_x3 * delta_x3; + + // square_vector != square_vector means checking for NaN value at square_vector + if (square_vector <= BGC_SQUARE_EPSYLON_FP32 || square_vector != square_vector) { + bgc_versor_copy_fp32(end, result); + return; + } + + // Calculating of the turning which fits the phase: + const float vector_modulus = sqrtf(square_vector); + const float angle = atan2f(vector_modulus, delta_s0) * phase; + const float multiplier = sinf(angle) / vector_modulus; + + const float turn_s0 = cosf(angle); + const float turn_x1 = delta_x1 * multiplier; + const float turn_x2 = delta_x2 * multiplier; + const float turn_x3 = delta_x3 * multiplier; + + // Combining of starting orientation with the turning + bgc_versor_set_values_fp32( + (turn_s0 * start->s0 - turn_x1 * start->x1) - (turn_x2 * start->x2 + turn_x3 * start->x3), + (turn_x1 * start->s0 + turn_s0 * start->x1) - (turn_x3 * start->x2 - turn_x2 * start->x3), + (turn_x2 * start->s0 + turn_s0 * start->x2) - (turn_x1 * start->x3 - turn_x3 * start->x1), + (turn_x3 * start->s0 + turn_s0 * start->x3) - (turn_x2 * start->x1 - turn_x1 * start->x2), + result + ); +} + +void bgc_versor_spherically_interpolate_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, const double phase, BgcVersorFP64* result) +{ + const double delta_s0 = (end->s0 * start->s0 + end->x1 * start->x1) + (end->x2 * start->x2 + end->x3 * start->x3); + const double delta_x1 = (end->x1 * start->s0 + end->x3 * start->x2) - (end->s0 * start->x1 + end->x2 * start->x3); + const double delta_x2 = (end->x2 * start->s0 + end->x1 * start->x3) - (end->s0 * start->x2 + end->x3 * start->x1); + const double delta_x3 = (end->x3 * start->s0 + end->x2 * start->x1) - (end->s0 * start->x3 + end->x1 * start->x2); + + const double square_vector = delta_x1 * delta_x1 + delta_x2 * delta_x2 + delta_x3 * delta_x3; + + // square_vector != square_vector means checking for NaN value at square_vector + if (square_vector <= BGC_SQUARE_EPSYLON_FP64 || square_vector != square_vector) { + bgc_versor_copy_fp64(end, result); + return; + } + + // Calculating of the turning which fits the phase: + const double vector_modulus = sqrt(square_vector); + const double angle = atan2(vector_modulus, delta_s0) * phase; + const double multiplier = sin(angle) / vector_modulus; + + const double turn_s0 = cos(angle); + const double turn_x1 = delta_x1 * multiplier; + const double turn_x2 = delta_x2 * multiplier; + const double turn_x3 = delta_x3 * multiplier; + + // Combining of starting orientation with the turning + bgc_versor_set_values_fp64( + (turn_s0 * start->s0 - turn_x1 * start->x1) - (turn_x2 * start->x2 + turn_x3 * start->x3), + (turn_x1 * start->s0 + turn_s0 * start->x1) - (turn_x3 * start->x2 - turn_x2 * start->x3), + (turn_x2 * start->s0 + turn_s0 * start->x2) - (turn_x1 * start->x3 - turn_x3 * start->x1), + (turn_x3 * start->s0 + turn_s0 * start->x3) - (turn_x2 * start->x1 - turn_x1 * start->x2), + result + ); +} + +// ================ Get Rotation ================ // + +void bgc_versor_get_rotation_fp32(const BgcVersorFP32* versor, BgcRotation3FP32* result) +{ + const float square_modulus = versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3; + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP32) { + bgc_rotation3_reset_fp32(result); + return; + } + + const float vector_modulus = sqrtf(square_modulus); + + const float multiplier = 1.0f / vector_modulus; + + result->radians = 2.0f * atan2f(vector_modulus, versor->s0); + + result->axis.x1 = versor->x1 * multiplier; + result->axis.x2 = versor->x2 * multiplier; + result->axis.x3 = versor->x3 * multiplier; +} + +void bgc_versor_get_rotation_fp64(const BgcVersorFP64* versor, BgcRotation3FP64* result) +{ + const double square_modulus = versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3; + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP64) { + bgc_rotation3_reset_fp64(result); + return; + } + + const double vector_modulus = sqrt(square_modulus); + + const double multiplier = 1.0 / vector_modulus; + + result->radians = 2.0 * atan2(vector_modulus, versor->s0); + + result->axis.x1 = versor->x1 * multiplier; + result->axis.x2 = versor->x2 * multiplier; + result->axis.x3 = versor->x3 * multiplier; +} diff --git a/basic-geometry/versor.h b/basic-geometry/versor.h index 6240b0f..c516c8d 100644 --- a/basic-geometry/versor.h +++ b/basic-geometry/versor.h @@ -223,14 +223,14 @@ inline void bgc_versor_shorten_fp32(const BgcVersorFP32* versor, BgcVersorFP32* _BgcDarkTwinVersorFP32* twin = (_BgcDarkTwinVersorFP32*)shortened; if (versor->s0 >= 0.0f) { - twin->x1 = versor->s0; + twin->s0 = versor->s0; twin->x1 = versor->x1; twin->x2 = versor->x2; twin->x3 = versor->x3; return; } - twin->x1 = -versor->s0; + twin->s0 = -versor->s0; twin->x1 = -versor->x1; twin->x2 = -versor->x2; twin->x3 = -versor->x3; @@ -241,14 +241,14 @@ inline void bgc_versor_shorten_fp64(const BgcVersorFP64* versor, BgcVersorFP64* _BgcDarkTwinVersorFP64* twin = (_BgcDarkTwinVersorFP64*)shortened; if (versor->s0 >= 0.0) { - twin->x1 = versor->s0; + twin->s0 = versor->s0; twin->x1 = versor->x1; twin->x2 = versor->x2; twin->x3 = versor->x3; return; } - twin->x1 = -versor->s0; + twin->s0 = -versor->s0; twin->x1 = -versor->x1; twin->x2 = -versor->x2; twin->x3 = -versor->x3; @@ -362,6 +362,12 @@ inline void bgc_versor_exclude_fp64(const BgcVersorFP64* base, const BgcVersorFP ); } +// ============ Sphere Interpolation ============ // + +void bgc_versor_spherically_interpolate_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, const float phase, BgcVersorFP32* result); + +void bgc_versor_spherically_interpolate_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, const double phase, BgcVersorFP64* result); + // ================ Get Rotation ================ // void bgc_versor_get_rotation_fp32(const BgcVersorFP32* versor, BgcRotation3FP32* result);