From d2a25823a5860a43cec26aac95637c0d9fa3dfeb Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Wed, 25 Dec 2024 13:46:31 +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=B0=20=D0=BA=D0=BE=D0=BC=D0=B1=D0=B8=D0=BD=D0=B0=D1=86?= =?UTF-8?q?=D0=B8=D1=8F=20=D1=82=D1=80=D1=91=D1=85=20=D0=B2=D0=B5=D1=80?= =?UTF-8?q?=D1=81=D0=BE=D1=80=D0=BE=D0=B2=20=D0=BE=D0=B4=D0=BD=D0=BE=D0=B9?= =?UTF-8?q?=20=D0=BE=D0=BF=D0=B5=D1=80=D0=B0=D1=86=D0=B8=D0=B5=D0=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- basic-geometry/versor.h | 68 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 68 insertions(+) diff --git a/basic-geometry/versor.h b/basic-geometry/versor.h index 6eff522..dca3da6 100644 --- a/basic-geometry/versor.h +++ b/basic-geometry/versor.h @@ -440,6 +440,74 @@ static inline void bg_fp64_versor_combine(const BgFP64Versor* second, const BgFP twin->x3 *= multiplier; } +// ============ Combination of three ============ // + +static inline void bg_fp32_versor_combine3(const BgFP32Versor* third, const BgFP32Versor* second, const BgFP32Versor* first, BgFP32Versor* 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 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); + + __BgFP32DarkTwinVersor* twin = (__BgFP32DarkTwinVersor*)result; + + twin->s0 = s0b; + twin->x1 = x1b; + twin->x2 = x2b; + twin->x3 = x3b; + + if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) { + return; + } + + const float multiplier = sqrtf(1.0f / square_modulus); + + twin->s0 *= multiplier; + twin->x1 *= multiplier; + twin->x2 *= multiplier; + twin->x3 *= multiplier; +} + +static inline void bg_fp64_versor_combine3(const BgFP64Versor* third, const BgFP64Versor* second, const BgFP64Versor* first, BgFP64Versor* 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 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); + + __BgFP64DarkTwinVersor* twin = (__BgFP64DarkTwinVersor*)result; + + twin->s0 = s0b; + twin->x1 = x1b; + twin->x2 = x2b; + twin->x3 = x3b; + + if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) { + return; + } + + const double multiplier = sqrt(1.0 / square_modulus); + + twin->s0 *= multiplier; + twin->x1 *= multiplier; + twin->x2 *= multiplier; + twin->x3 *= multiplier; +} + // ================= Exclusion ================== // static inline void bg_fp32_versor_exclude(const BgFP32Versor* basic, const BgFP32Versor* exclusion, BgFP32Versor* result)