From add4d89c802d9818fb8972064da9c5072551d91d Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Thu, 26 Feb 2026 02:36:03 +0700 Subject: [PATCH] =?UTF-8?q?=D0=A0=D0=B0=D0=B7=D0=B2=D0=B8=D1=82=D0=B8?= =?UTF-8?q?=D0=B5=20=D1=81=D1=82=D1=80=D1=83=D0=BA=D1=82=D1=83=D1=80=D1=8B?= =?UTF-8?q?=20Posture3=20=D0=B4=D0=BB=D1=8F=20=D1=85=D1=80=D0=B0=D0=BD?= =?UTF-8?q?=D0=B5=D0=BD=D0=B8=D1=8F=20=D0=BF=D0=BB=D0=BE=D0=B6=D0=B5=D0=BD?= =?UTF-8?q?=D0=B8=D1=8F=20=D1=82=D0=B5=D0=BB=D0=B0=20=D0=B2=20=D0=B2=D0=B8?= =?UTF-8?q?=D0=B4=D0=B5=20=D0=BD=D0=BE=D1=80=D0=BC=D0=B0=D0=BB=D0=B8=D0=B7?= =?UTF-8?q?=D0=BE=D0=B2=D0=B0=D0=BD=D0=BD=D0=BE=D0=B3=D0=BE=20=D0=B4=D1=83?= =?UTF-8?q?=D0=B0=D0=BB=D1=8C=D0=BD=D0=BE=D0=B3=D0=BE=20=D0=BA=D0=B2=D0=B0?= =?UTF-8?q?=D1=82=D0=B5=D1=80=D0=BD=D0=B8=D0=BE=D0=BD=D0=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- basic-geometry/posture3.c | 31 +++++++++++-- basic-geometry/posture3.h | 98 ++++++++++++++++++++++++++++++++------- basic-geometry/types.h | 4 +- 3 files changed, 109 insertions(+), 24 deletions(-) diff --git a/basic-geometry/posture3.c b/basic-geometry/posture3.c index 8c248f0..f3e76b2 100644 --- a/basic-geometry/posture3.c +++ b/basic-geometry/posture3.c @@ -3,10 +3,18 @@ extern inline void bgc_fp32_posture3_reset(BGC_FP32_Posture3* posture); extern inline void bgc_fp64_posture3_reset(BGC_FP64_Posture3* posture); -void _bgc_fp32_posture3_normalize(BGC_FP32_Posture3* posture) -{ - const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(&posture->_dual_versor.real_part); +extern inline void bgc_fp32_posture3_copy(BGC_FP32_Posture3* destination, const BGC_FP32_Posture3* source); +extern inline void bgc_fp64_posture3_copy(BGC_FP64_Posture3* destination, const BGC_FP64_Posture3* source); +extern inline void bgc_fp32_posture3_swap(BGC_FP32_Posture3* posture1, BGC_FP32_Posture3* posture2); +extern inline void bgc_fp64_posture3_swap(BGC_FP64_Posture3* posture1, BGC_FP64_Posture3* posture2); + +extern inline void bgc_fp32_posture3_convert_to_fp64(BGC_FP64_Posture3* destination, const BGC_FP32_Posture3* source); +extern inline void bgc_fp64_posture3_convert_to_fp32(BGC_FP32_Posture3* destination, const BGC_FP64_Posture3* source); + + +void _bgc_fp32_posture3_normalize(BGC_FP32_Posture3* posture, const float square_magnitude) +{ if (square_magnitude <= BGC_FP32_SQUARE_EPSILON || isnan(square_magnitude)) { bgc_fp32_posture3_reset(posture); return; @@ -14,6 +22,19 @@ void _bgc_fp32_posture3_normalize(BGC_FP32_Posture3* posture) const float multiplier = sqrtf(1.0f / square_magnitude); - bgc_fp32_quaternion_multiply_by_real(&posture->_dual_versor.real_part, &posture->_dual_versor.real_part, multiplier); - bgc_fp32_quaternion_multiply_by_real(&posture->_dual_versor.dual_part, &posture->_dual_versor.dual_part, multiplier); + bgc_fp32_quaternion_multiply_by_real(&posture->_real_part, &posture->_real_part, multiplier); + bgc_fp32_quaternion_multiply_by_real(&posture->_dual_part, &posture->_dual_part, multiplier); +} + +void _bgc_fp64_posture3_normalize(BGC_FP64_Posture3* posture, const double square_magnitude) +{ + if (square_magnitude <= BGC_FP64_SQUARE_EPSILON || isnan(square_magnitude)) { + bgc_fp64_posture3_reset(posture); + return; + } + + const double multiplier = sqrt(1.0 / square_magnitude); + + bgc_fp64_quaternion_multiply_by_real(&posture->_real_part, &posture->_real_part, multiplier); + bgc_fp64_quaternion_multiply_by_real(&posture->_dual_part, &posture->_dual_part, multiplier); } diff --git a/basic-geometry/posture3.h b/basic-geometry/posture3.h index e89c642..014ea2e 100644 --- a/basic-geometry/posture3.h +++ b/basic-geometry/posture3.h @@ -5,35 +5,99 @@ #include "quaternion.h" #include "dual-quaternion.h" +// ================= Normalize ================== // + +void _bgc_fp32_posture3_normalize(BGC_FP32_Posture3* posture, const float square_magnitude); + +void _bgc_fp64_posture3_normalize(BGC_FP64_Posture3* posture, const double square_magnitude); + // ==================== Reset =================== // inline void bgc_fp32_posture3_reset(BGC_FP32_Posture3* posture) { - posture->_dual_versor.real_part.s0 = 1.0f; - posture->_dual_versor.real_part.x1 = 0.0f; - posture->_dual_versor.real_part.x2 = 0.0f; - posture->_dual_versor.real_part.x3 = 0.0f; + posture->_real_part.s0 = 1.0f; + posture->_real_part.x1 = 0.0f; + posture->_real_part.x2 = 0.0f; + posture->_real_part.x3 = 0.0f; - posture->_dual_versor.dual_part.s0 = 0.0f; - posture->_dual_versor.dual_part.x1 = 0.0f; - posture->_dual_versor.dual_part.x2 = 0.0f; - posture->_dual_versor.dual_part.x3 = 0.0f; + posture->_dual_part.s0 = 0.0f; + posture->_dual_part.x1 = 0.0f; + posture->_dual_part.x2 = 0.0f; + posture->_dual_part.x3 = 0.0f; } inline void bgc_fp64_posture3_reset(BGC_FP64_Posture3* posture) { - posture->_dual_versor.real_part.s0 = 1.0; - posture->_dual_versor.real_part.x1 = 0.0; - posture->_dual_versor.real_part.x2 = 0.0; - posture->_dual_versor.real_part.x3 = 0.0; + posture->_real_part.s0 = 1.0; + posture->_real_part.x1 = 0.0; + posture->_real_part.x2 = 0.0; + posture->_real_part.x3 = 0.0; - posture->_dual_versor.dual_part.s0 = 0.0; - posture->_dual_versor.dual_part.x1 = 0.0; - posture->_dual_versor.dual_part.x2 = 0.0; - posture->_dual_versor.dual_part.x3 = 0.0; + posture->_dual_part.s0 = 0.0; + posture->_dual_part.x1 = 0.0; + posture->_dual_part.x2 = 0.0; + posture->_dual_part.x3 = 0.0; } -void _bgc_fp32_posture3_normalize(BGC_FP32_Posture3* posture); +// ==================== Copy ==================== // +inline void bgc_fp32_posture3_copy(BGC_FP32_Posture3* destination, const BGC_FP32_Posture3* source) +{ + bgc_fp32_quaternion_copy(&destination->_real_part, &source->_real_part); + bgc_fp32_quaternion_copy(&destination->_dual_part, &source->_dual_part); +} + +inline void bgc_fp64_posture3_copy(BGC_FP64_Posture3* destination, const BGC_FP64_Posture3* source) +{ + bgc_fp64_quaternion_copy(&destination->_real_part, &source->_real_part); + bgc_fp64_quaternion_copy(&destination->_dual_part, &source->_dual_part); +} + +// ==================== Swap ==================== // + +inline void bgc_fp32_posture3_swap(BGC_FP32_Posture3* posture1, BGC_FP32_Posture3* posture2) +{ + bgc_fp32_quaternion_swap(&posture1->_real_part, &posture2->_real_part); + bgc_fp32_quaternion_swap(&posture1->_dual_part, &posture2->_dual_part); +} + +inline void bgc_fp64_posture3_swap(BGC_FP64_Posture3* posture1, BGC_FP64_Posture3* posture2) +{ + bgc_fp64_quaternion_swap(&posture1->_real_part, &posture2->_real_part); + bgc_fp64_quaternion_swap(&posture1->_dual_part, &posture2->_dual_part); +} + +// ================== Convert =================== // + +inline void bgc_fp32_posture3_convert_to_fp64(BGC_FP64_Posture3* destination, const BGC_FP32_Posture3* source) +{ + bgc_fp32_quaternion_convert_to_fp64(&destination->_real_part, &source->_real_part); + bgc_fp32_quaternion_convert_to_fp64(&destination->_dual_part, &source->_dual_part); + + const double square_magnitude = bgc_fp64_quaternion_get_square_magnitude(&destination->_real_part); + + if (!bgc_fp64_is_square_unit(square_magnitude)) { + _bgc_fp64_posture3_normalize(destination, square_magnitude); + } +} + +inline void bgc_fp64_posture3_convert_to_fp32(BGC_FP32_Posture3* destination, const BGC_FP64_Posture3* source) +{ + bgc_fp64_quaternion_convert_to_fp32(&destination->_real_part, &source->_real_part); + bgc_fp64_quaternion_convert_to_fp32(&destination->_dual_part, &source->_dual_part); + + const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(&destination->_real_part); + + if (!bgc_fp32_is_square_unit(square_magnitude)) { + _bgc_fp32_posture3_normalize(destination, square_magnitude); + } +} + +// ================== Combine =================== // + +inline void bgc_fp32_posture3_combine(BGC_FP32_Posture3* combination, const BGC_FP64_Posture3* first, const BGC_FP64_Posture3* second) +{ + +} #endif diff --git a/basic-geometry/types.h b/basic-geometry/types.h index ce6c8d4..430709f 100644 --- a/basic-geometry/types.h +++ b/basic-geometry/types.h @@ -214,11 +214,11 @@ typedef struct { // ================== Posture3 ================== // typedef struct { - BGC_FP32_DualQuaternion _dual_versor; + BGC_FP32_Quaternion _real_part, _dual_part; } BGC_FP32_Posture3; typedef struct { - BGC_FP64_DualQuaternion _dual_versor; + BGC_FP64_Quaternion _real_part, _dual_part; } BGC_FP64_Posture3; #endif