From e39765b7331cd8ca061fb5ecdeee0b04cbc6ed53 Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Thu, 30 Jan 2025 01:10:41 +0700 Subject: [PATCH] =?UTF-8?q?=D0=A0=D0=B5=D0=BE=D1=80=D0=B3=D0=B0=D0=BD?= =?UTF-8?q?=D0=B8=D0=B7=D0=B0=D1=86=D0=B8=D1=8F=20=D0=BC=D0=B5=D1=82=D0=BE?= =?UTF-8?q?=D0=B4=D0=BE=D0=B2=20=D0=B4=D0=BB=D1=8F=20=D0=B2=D0=B5=D1=80?= =?UTF-8?q?=D1=81=D0=BE=D1=80=D0=BE=D0=B2=20=D0=B8=20=D1=82=D0=B0=D0=BD?= =?UTF-8?q?=D0=B3=D0=B5=D0=BD=D1=82=D0=BE=D0=B2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- basic-geometry-dev/main.c | 53 ++++----- basic-geometry/tangent.c | 32 ++++++ basic-geometry/tangent.h | 34 ++---- basic-geometry/versor.c | 43 +++++++ basic-geometry/versor.h | 232 +++++++------------------------------- 5 files changed, 147 insertions(+), 247 deletions(-) diff --git a/basic-geometry-dev/main.c b/basic-geometry-dev/main.c index 3ec1f28..c5412f9 100644 --- a/basic-geometry-dev/main.c +++ b/basic-geometry-dev/main.c @@ -12,7 +12,7 @@ typedef struct { BgcVersorFP32 versor1, versor2, result; //BgcMatrix3x3FP32 matrix; - BgcVector3FP32 vector1, vector2; + //BgcVector3FP32 vector1, vector2; } structure_fp32_t; structure_fp32_t* allocate_structures(const unsigned int amount) @@ -50,7 +50,7 @@ structure_fp32_t* make_structures(const unsigned int amount) bgc_versor_reset_fp32(&list[i].result); //bgc_matrix3x3_set_to_identity_fp32(&list[i].matrix); - + /* bgc_vector3_set_values_fp32( rand() * multiplier - 1.0f, rand() * multiplier - 1.0f, @@ -59,6 +59,7 @@ structure_fp32_t* make_structures(const unsigned int amount) ); bgc_vector3_reset_fp32(&list[i].vector2); + */ } return list; @@ -84,51 +85,37 @@ void print_vector_fp64(const BgcVector3FP64* vector) printf("(%lf, %lf, %lf) / %lf\n", vector->x1, vector->x2, vector->x3, bgc_vector3_get_modulus_fp64(vector)); } -void item_work(structure_fp32_t* item) +void list_work(const uint_fast32_t amount, structure_fp32_t* list) { - for (unsigned int j = 0; j < 1000; j++) { - bgc_versor_combine_fp32(&item->versor1, &item->versor2, &item->result); - //bgc_versor_turn_vector_fp32(&item->result, &item->vector1, &item->vector2); + for (uint_fast32_t j = 0; j < 1000; j++) { + for (uint_fast32_t i = 0; i < amount; i++) { + bgc_versor_combine_fp32(&list[i].versor1, &list[i].versor1, &list[i].result); + } } } int main() { const unsigned int amount = 1000000; - structure_fp32_t* list; + structure_fp32_t* list = make_structures(amount); #ifdef _WIN64 - ULONGLONG now, start, end; - now = GetTickCount64(); - srand((unsigned int)(now & 0xfffffff)); + ULONGLONG start, end; + + start = GetTickCount64(); + + srand((unsigned int)(start & 0xfffffff)); + + start = GetTickCount64(); #else struct timespec start, end; clock_gettime(0, &start); srand((unsigned int)(start.tv_nsec & 0xfffffff)); -#endif // _WIN64 - - list = make_structures(amount); - -#ifdef _WIN64 - end = GetTickCount64(); - printf("Setup time: %lld\n", end - now); - - start = GetTickCount64(); -#else - clock_gettime(CLOCK_REALTIME, &end); - printf("Time: %lf\n", (end.tv_sec - start.tv_sec) * 1000.0 + (end.tv_nsec - start.tv_nsec) * 0.000001); clock_gettime(CLOCK_REALTIME, &start); #endif // _WIN64 - for (unsigned int i = 0; i < amount; i++) { - //for (int j = 0; j < 1000; j++) { - item_work(list + i); - //structure_fp32_t* item = list + i; - //bgc_versor_combine_fp32(&item->versor1, &item->versor2, &item->result); - //bgc_versor_turn_vector_fp32(&item->result, &item->vector1, &item->vector2); - //} - } + list_work(amount, list); #ifdef _WIN64 end = GetTickCount64(); @@ -140,9 +127,9 @@ int main() printf("Time: %lf\n", (end.tv_sec - start.tv_sec) * 1000.0 + (end.tv_nsec - start.tv_nsec) * 0.000001); #endif // _WIN64 - //print_versor_fp32(&list[10].versor1); - //print_versor_fp32(&list[10].versor2); - //print_versor_fp32(&list[10].result); + print_versor_fp32(&list[10].versor1); + print_versor_fp32(&list[10].versor2); + print_versor_fp32(&list[10].result); free(list); diff --git a/basic-geometry/tangent.c b/basic-geometry/tangent.c index c4e31c6..a4f95d6 100644 --- a/basic-geometry/tangent.c +++ b/basic-geometry/tangent.c @@ -3,3 +3,35 @@ const BgcTangentFP32 BGC_IDLE_TANGENT_FP32 = { 1.0f, 0.0f }; const BgcTangentFP64 BGC_IDLE_TANGENT_FP64 = { 1.0, 0.0 }; + +void _bgc_tangent_normalize_fp32(const float square_modulus, _BgcDarkTwinTangentFP32* twin) +{ + // (square_modulus != square_modulus) is true when square_modulus is NaN + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { + twin->cos = 1.0f; + twin->sin = 0.0f; + return; + } + + const float multiplier = sqrtf(1.0f / square_modulus); + + twin->cos *= multiplier; + twin->sin *= multiplier; +} + +void _bgc_tangent_normalize_fp64(const double square_modulus, _BgcDarkTwinTangentFP64* twin) +{ + // (square_modulus != square_modulus) is true when square_modulus is NaN + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { + twin->cos = 1.0; + twin->sin = 0.0; + return; + } + + const double multiplier = sqrt(1.0 / square_modulus); + + twin->cos *= multiplier; + twin->sin *= multiplier; +} diff --git a/basic-geometry/tangent.h b/basic-geometry/tangent.h index f0e60cf..ea49f79 100644 --- a/basic-geometry/tangent.h +++ b/basic-geometry/tangent.h @@ -55,54 +55,40 @@ inline void bgc_tangent_reset_fp64(BgcTangentFP64* tangent) // ==================== Set ===================== // +void _bgc_tangent_normalize_fp32(const float square_modulus, _BgcDarkTwinTangentFP32* twin); + +void _bgc_tangent_normalize_fp64(const double square_modulus, _BgcDarkTwinTangentFP64* twin); + inline void bgc_tangent_set_values_fp32(const float x1, const float x2, BgcTangentFP32* tangent) { - const float square_module = x1 * x1 + x2 * x2; + const float square_modulus = x1 * x1 + x2 * x2; _BgcDarkTwinTangentFP32* twin = (_BgcDarkTwinTangentFP32*)tangent; twin->cos = x1; twin->sin = x2; - if (1.0f - BGC_TWO_EPSYLON_FP32 <= square_module && square_module <= 1.0f + BGC_TWO_EPSYLON_FP32) { + if (1.0f - BGC_TWO_EPSYLON_FP32 <= square_modulus && square_modulus <= 1.0f + BGC_TWO_EPSYLON_FP32) { return; } - if (square_module <= BGC_SQUARE_EPSYLON_FP32) { - twin->cos = 1.0f; - twin->sin = 0.0f; - return; - } - - const float multiplier = sqrtf(1.0f / square_module); - - twin->cos = x1 * multiplier; - twin->sin = x2 * multiplier; + _bgc_tangent_normalize_fp32(square_modulus, twin); } inline void bgc_tangent_set_values_fp64(const double x1, const double x2, BgcTangentFP64* tangent) { - const double square_module = x1 * x1 + x2 * x2; + const double square_modulus = x1 * x1 + x2 * x2; _BgcDarkTwinTangentFP64* twin = (_BgcDarkTwinTangentFP64*)tangent; twin->cos = x1; twin->sin = x2; - if (1.0 - BGC_TWO_EPSYLON_FP64 <= square_module && square_module <= 1.0 + BGC_TWO_EPSYLON_FP64) { + if (1.0 - BGC_TWO_EPSYLON_FP64 <= square_modulus && square_modulus <= 1.0 + BGC_TWO_EPSYLON_FP64) { return; } - if (square_module <= BGC_SQUARE_EPSYLON_FP64) { - twin->cos = 1.0; - twin->sin = 0.0; - return; - } - - const double multiplier = sqrt(1.0 / square_module); - - twin->cos = x1 * multiplier; - twin->sin = x2 * multiplier; + _bgc_tangent_normalize_fp64(square_modulus, twin); } // ==================== Copy ==================== // diff --git a/basic-geometry/versor.c b/basic-geometry/versor.c index 5f9d45e..19e0143 100644 --- a/basic-geometry/versor.c +++ b/basic-geometry/versor.c @@ -7,6 +7,49 @@ const BgcVersorFP32 BGC_IDLE_VERSOR_FP32 = { 1.0f, 0.0f, 0.0f, 0.0f }; const BgcVersorFP64 BGC_IDLE_VERSOR_FP64 = { 1.0, 0.0, 0.0, 0.0 }; +// =============== Normalization ================ // + +void _bgc_versor_normalize_fp32(const float square_modulus, _BgcDarkTwinVersorFP32* twin) +{ + // (square_modulus != square_modulus) is true when square_modulus is NaN + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) { + twin->s0 = 1.0f; + twin->x1 = 0.0f; + twin->x2 = 0.0f; + twin->x3 = 0.0f; + return; + } + + const float multiplier = sqrtf(1.0f / square_modulus); + + twin->s0 *= multiplier; + twin->x1 *= multiplier; + twin->x2 *= multiplier; + twin->x3 *= multiplier; + +} + +void _bgc_versor_normalize_fp64(const double square_modulus, _BgcDarkTwinVersorFP64* twin) +{ + // (square_modulus != square_modulus) is true when square_modulus is NaN + + if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) { + twin->s0 = 1.0; + twin->x1 = 0.0; + twin->x2 = 0.0; + twin->x3 = 0.0; + return; + } + + const double multiplier = sqrt(1.0 / square_modulus); + + twin->s0 *= multiplier; + twin->x1 *= multiplier; + twin->x2 *= multiplier; + twin->x3 *= multiplier; +} + // =============== Set Crude Turn =============== // void bgc_versor_set_crude_turn_fp32(const float x1, const float x2, const float x3, const float angle, const BgcAngleUnitEnum unit, BgcVersorFP32* result) diff --git a/basic-geometry/versor.h b/basic-geometry/versor.h index ffa4d49..7b815ea 100644 --- a/basic-geometry/versor.h +++ b/basic-geometry/versor.h @@ -58,6 +58,10 @@ inline void bgc_versor_reset_fp64(BgcVersorFP64* versor) // ==================== Set ===================== // +void _bgc_versor_normalize_fp32(const float square_modulus, _BgcDarkTwinVersorFP32* twin); + +void _bgc_versor_normalize_fp64(const double square_modulus, _BgcDarkTwinVersorFP64* twin); + inline void bgc_versor_set_values_fp32(const float s0, const float x1, const float x2, const float x3, BgcVersorFP32* versor) { _BgcDarkTwinVersorFP32* twin = (_BgcDarkTwinVersorFP32*)versor; @@ -73,20 +77,7 @@ inline void bgc_versor_set_values_fp32(const float s0, const float x1, const flo return; } - if (square_modulus <= BGC_SQUARE_EPSYLON_FP32) { - twin->s0 = 1.0f; - twin->x1 = 0.0f; - twin->x2 = 0.0f; - twin->x3 = 0.0f; - return; - } - - const float multiplier = sqrtf(1.0f / square_modulus); - - twin->s0 *= multiplier; - twin->x1 *= multiplier; - twin->x2 *= multiplier; - twin->x3 *= multiplier; + _bgc_versor_normalize_fp32(square_modulus, twin); } inline void bgc_versor_set_values_fp64(const double s0, const double x1, const double x2, const double x3, BgcVersorFP64* versor) @@ -104,20 +95,7 @@ inline void bgc_versor_set_values_fp64(const double s0, const double x1, const d return; } - if (square_modulus <= BGC_SQUARE_EPSYLON_FP64) { - twin->s0 = 1.0; - twin->x1 = 0.0; - twin->x2 = 0.0; - twin->x3 = 0.0; - return; - } - - const double multiplier = sqrt(1.0 / square_modulus); - - twin->s0 *= multiplier; - twin->x1 *= multiplier; - twin->x2 *= multiplier; - twin->x3 *= multiplier; + _bgc_versor_normalize_fp64(square_modulus, twin); } // ==================== Copy ==================== // @@ -386,184 +364,58 @@ inline void bgc_versor_set_inverted_fp32_to_fp64(const BgcVersorFP32* versor, Bg inline void bgc_versor_combine_fp32(const BgcVersorFP32* second, const BgcVersorFP32* first, BgcVersorFP32* result) { - const float s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3); - const float x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3); - const float x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1); - const float x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2); - - const float square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - _BgcDarkTwinVersorFP32* twin = (_BgcDarkTwinVersorFP32*)result; - - twin->s0 = s0; - twin->x1 = x1; - twin->x2 = x2; - twin->x3 = x3; - - if (1.0f - BGC_TWO_EPSYLON_FP32 <= square_modulus && square_modulus <= 1.0f + BGC_TWO_EPSYLON_FP32) { - return; - } - - const float multiplier = sqrtf(1.0f / square_modulus); - - twin->s0 *= multiplier; - twin->x1 *= multiplier; - twin->x2 *= multiplier; - twin->x3 *= multiplier; + bgc_versor_set_values_fp32( + (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3), + (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3), + (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1), + (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2), + result + ); } inline void bgc_versor_combine_fp64(const BgcVersorFP64* second, const BgcVersorFP64* first, BgcVersorFP64* result) { - const double s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3); - const double x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3); - const double x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1); - const double x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2); - - const double square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - _BgcDarkTwinVersorFP64* twin = (_BgcDarkTwinVersorFP64*)result; - - twin->s0 = s0; - twin->x1 = x1; - twin->x2 = x2; - twin->x3 = x3; - - if (1.0 - BGC_TWO_EPSYLON_FP64 <= square_modulus && square_modulus <= 1.0 + BGC_TWO_EPSYLON_FP64) { - return; - } - - const double multiplier = sqrt(1.0 / square_modulus); - - twin->s0 *= multiplier; - twin->x1 *= multiplier; - twin->x2 *= multiplier; - twin->x3 *= multiplier; + bgc_versor_set_values_fp64( + (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3), + (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3), + (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1), + (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2), + result + ); } // ============ Combination of three ============ // inline void bgc_versor_combine3_fp32(const BgcVersorFP32* third, const BgcVersorFP32* second, const BgcVersorFP32* first, BgcVersorFP32* result) { - const float s0a = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3); - const float x1a = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3); - const float x2a = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1); - const float x3a = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2); + const float s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3); + const float x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3); + const float x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1); + const float x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2); - const float s0b = (third->s0 * s0a - third->x1 * x1a) - (third->x2 * x2a + third->x3 * x3a); - const float x1b = (third->x1 * s0a + third->s0 * x1a) - (third->x3 * x2a - third->x2 * x3a); - const float x2b = (third->x2 * s0a + third->s0 * x2a) - (third->x1 * x3a - third->x3 * x1a); - const float x3b = (third->x3 * s0a + third->s0 * x3a) - (third->x2 * x1a - third->x1 * x2a); - - const float square_modulus = (s0b * s0b + x1b * x1b) + (x2b * x2b + x3b * x3b); - - _BgcDarkTwinVersorFP32* twin = (_BgcDarkTwinVersorFP32*)result; - - twin->s0 = s0b; - twin->x1 = x1b; - twin->x2 = x2b; - twin->x3 = x3b; - - if (1.0f - BGC_TWO_EPSYLON_FP32 <= square_modulus && square_modulus <= 1.0f + BGC_TWO_EPSYLON_FP32) { - return; - } - - const float multiplier = sqrtf(1.0f / square_modulus); - - twin->s0 *= multiplier; - twin->x1 *= multiplier; - twin->x2 *= multiplier; - twin->x3 *= multiplier; + bgc_versor_set_values_fp32( + (third->s0 * s0 - third->x1 * x1) - (third->x2 * x2 + third->x3 * x3), + (third->x1 * s0 + third->s0 * x1) - (third->x3 * x2 - third->x2 * x3), + (third->x2 * s0 + third->s0 * x2) - (third->x1 * x3 - third->x3 * x1), + (third->x3 * s0 + third->s0 * x3) - (third->x2 * x1 - third->x1 * x2), + result + ); } inline void bgc_versor_combine3_fp64(const BgcVersorFP64* third, const BgcVersorFP64* second, const BgcVersorFP64* first, BgcVersorFP64* result) { - const double s0a = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3); - const double x1a = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3); - const double x2a = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1); - const double x3a = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2); + const double s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3); + const double x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3); + const double x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1); + const double x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2); - const double s0b = (third->s0 * s0a - third->x1 * x1a) - (third->x2 * x2a + third->x3 * x3a); - const double x1b = (third->x1 * s0a + third->s0 * x1a) - (third->x3 * x2a - third->x2 * x3a); - const double x2b = (third->x2 * s0a + third->s0 * x2a) - (third->x1 * x3a - third->x3 * x1a); - const double x3b = (third->x3 * s0a + third->s0 * x3a) - (third->x2 * x1a - third->x1 * x2a); - - const double square_modulus = (s0b * s0b + x1b * x1b) + (x2b * x2b + x3b * x3b); - - _BgcDarkTwinVersorFP64* twin = (_BgcDarkTwinVersorFP64*)result; - - twin->s0 = s0b; - twin->x1 = x1b; - twin->x2 = x2b; - twin->x3 = x3b; - - if (1.0 - BGC_TWO_EPSYLON_FP64 <= square_modulus && square_modulus <= 1.0 + BGC_TWO_EPSYLON_FP64) { - return; - } - - const double multiplier = sqrt(1.0 / square_modulus); - - twin->s0 *= multiplier; - twin->x1 *= multiplier; - twin->x2 *= multiplier; - twin->x3 *= multiplier; -} - -// ================= Exclusion ================== // - -inline void bgc_versor_exclude_fp32(const BgcVersorFP32* basic, const BgcVersorFP32* exclusion, BgcVersorFP32* result) -{ - const float s0 = (basic->s0 * exclusion->s0 + basic->x1 * exclusion->x1) + (basic->x2 * exclusion->x2 + basic->x3 * exclusion->x3); - const float x1 = (basic->x1 * exclusion->s0 - basic->s0 * exclusion->x1) + (basic->x3 * exclusion->x2 - basic->x2 * exclusion->x3); - const float x2 = (basic->x2 * exclusion->s0 - basic->s0 * exclusion->x2) + (basic->x1 * exclusion->x3 - basic->x3 * exclusion->x1); - const float x3 = (basic->x3 * exclusion->s0 - basic->s0 * exclusion->x3) + (basic->x2 * exclusion->x1 - basic->x1 * exclusion->x2); - - const float square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - _BgcDarkTwinVersorFP32* twin = (_BgcDarkTwinVersorFP32*)result; - - twin->s0 = s0; - twin->x1 = x1; - twin->x2 = x2; - twin->x3 = x3; - - if (1.0f - BGC_TWO_EPSYLON_FP32 <= square_modulus && square_modulus <= 1.0f + BGC_TWO_EPSYLON_FP32) { - return; - } - - const float multiplier = sqrtf(1.0f / square_modulus); - - twin->s0 *= multiplier; - twin->x1 *= multiplier; - twin->x2 *= multiplier; - twin->x3 *= multiplier; -} - -inline void bgc_versor_exclude_fp64(const BgcVersorFP64* basic, const BgcVersorFP64* exclusion, BgcVersorFP64* result) -{ - const double s0 = (basic->s0 * exclusion->s0 + basic->x1 * exclusion->x1) + (basic->x2 * exclusion->x2 + basic->x3 * exclusion->x3); - const double x1 = (basic->x1 * exclusion->s0 - basic->s0 * exclusion->x1) + (basic->x3 * exclusion->x2 - basic->x2 * exclusion->x3); - const double x2 = (basic->x2 * exclusion->s0 - basic->s0 * exclusion->x2) + (basic->x1 * exclusion->x3 - basic->x3 * exclusion->x1); - const double x3 = (basic->x3 * exclusion->s0 - basic->s0 * exclusion->x3) + (basic->x2 * exclusion->x1 - basic->x1 * exclusion->x2); - - const double square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - _BgcDarkTwinVersorFP64* twin = (_BgcDarkTwinVersorFP64*)result; - - twin->s0 = s0; - twin->x1 = x1; - twin->x2 = x2; - twin->x3 = x3; - - if (1.0 - BGC_TWO_EPSYLON_FP64 <= square_modulus && square_modulus <= 1.0 + BGC_TWO_EPSYLON_FP64) { - return; - } - - const double multiplier = sqrt(1.0 / square_modulus); - - twin->s0 *= multiplier; - twin->x1 *= multiplier; - twin->x2 *= multiplier; - twin->x3 *= multiplier; + bgc_versor_set_values_fp64( + (third->s0 * s0 - third->x1 * x1) - (third->x2 * x2 + third->x3 * x3), + (third->x1 * s0 + third->s0 * x1) - (third->x3 * x2 - third->x2 * x3), + (third->x2 * s0 + third->s0 * x2) - (third->x1 * x3 - third->x3 * x1), + (third->x3 * s0 + third->s0 * x3) - (third->x2 * x1 - third->x1 * x2), + result + ); } // ================= Rotation3 ================== //