From a57f13f4a9a9072f01a491211b420e10312ac452 Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Wed, 18 Mar 2026 20:23:50 +0700 Subject: [PATCH] =?UTF-8?q?=D0=98=D1=81=D0=BF=D1=80=D0=B0=D0=B2=D0=BB?= =?UTF-8?q?=D0=B5=D0=BD=D0=B8=D1=8F=20=D0=B2=20=D0=BD=D0=BE=D1=80=D0=BC?= =?UTF-8?q?=D0=B0=D0=BB=D0=B8=D0=B7=D0=B0=D1=86=D0=B8=D0=B8=20=D0=B4=D1=83?= =?UTF-8?q?=D0=B0=D0=BB=D1=8C=D0=BD=D1=8B=D1=85=20=D0=BA=D0=B2=D0=B0=D1=82?= =?UTF-8?q?=D0=B5=D1=80=D0=BD=D0=B8=D0=BE=D0=BD=D0=BE=D0=B2=20=D0=B8=20?= =?UTF-8?q?=D0=BF=D0=BE=D0=B7=D0=B8=D1=86=D0=B8=D0=BE=D0=BD=D0=B8=D1=80?= =?UTF-8?q?=D0=BE=D0=B2=D0=B0=D0=BD=D0=B8=D1=8F=20=D1=81=20=D0=BF=D0=BE?= =?UTF-8?q?=D0=BC=D0=BE=D1=89=D1=8C=D1=8E=20=D0=B4=D1=83=D0=B0=D0=BB=D1=8C?= =?UTF-8?q?=D0=BD=D1=8B=D1=85=20=D0=BA=D0=B2=D0=B0=D1=82=D0=B5=D1=80=D0=BD?= =?UTF-8?q?=D0=B8=D0=BE=D0=BD=D0=BE=D0=B2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- basic-geometry/dual-quaternion.c | 6 ++ basic-geometry/dual-quaternion.h | 138 +++++++++++++++++++++-------- basic-geometry/quaternion.c | 4 +- basic-geometry/quaternion.h | 28 +++--- basic-geometry/rigid-pose3.c | 29 ------ basic-geometry/rigid-pose3.h | 147 +++++++++++++++---------------- basic-geometry/slerp3.h | 16 ++-- basic-geometry/turn3.c | 4 +- basic-geometry/types.h | 4 +- 9 files changed, 206 insertions(+), 170 deletions(-) diff --git a/basic-geometry/dual-quaternion.c b/basic-geometry/dual-quaternion.c index 659394e..112333d 100644 --- a/basic-geometry/dual-quaternion.c +++ b/basic-geometry/dual-quaternion.c @@ -42,6 +42,12 @@ extern inline void bgc_fp64_dual_quaternion_fully_conjugate(BGC_FP64_DualQuatern extern inline void bgc_fp32_dual_quaternion_get_fully_conjugate(BGC_FP32_DualQuaternion* conjugate, const BGC_FP32_DualQuaternion* quaternion); extern inline void bgc_fp64_dual_quaternion_get_fully_conjugate(BGC_FP64_DualQuaternion* conjugate, const BGC_FP64_DualQuaternion* quaternion); +extern inline int bgc_fp32_dual_quaternion_normalize(BGC_FP32_DualQuaternion* quaternion); +extern inline int bgc_fp64_dual_quaternion_normalize(BGC_FP64_DualQuaternion* quaternion); + +extern inline int bgc_fp32_dual_quaternion_get_normalized(BGC_FP32_DualQuaternion* normalized, const BGC_FP32_DualQuaternion* quaternion); +extern inline int bgc_fp64_dual_quaternion_get_normalized(BGC_FP64_DualQuaternion* normalized, const BGC_FP64_DualQuaternion* quaternion); + extern inline void bgc_fp32_dual_quaternion_add(BGC_FP32_DualQuaternion* sum, const BGC_FP32_DualQuaternion* first, const BGC_FP32_DualQuaternion* second); extern inline void bgc_fp64_dual_quaternion_add(BGC_FP64_DualQuaternion* sum, const BGC_FP64_DualQuaternion* first, const BGC_FP64_DualQuaternion* second); diff --git a/basic-geometry/dual-quaternion.h b/basic-geometry/dual-quaternion.h index 4d39543..f0590b2 100644 --- a/basic-geometry/dual-quaternion.h +++ b/basic-geometry/dual-quaternion.h @@ -260,34 +260,102 @@ inline void bgc_fp64_dual_quaternion_get_fully_conjugate(BGC_FP64_DualQuaternion // ================= Normalize ================== // -void _bgc_fp32_dual_quaternion_normalize_parts(BGC_FP32_DualQuaternion* quaternion, const float square_magnitude); - -void _bgc_fp64_dual_quaternion_normalize_parts(BGC_FP64_DualQuaternion* quaternion, const double square_magnitude); - -inline void bgc_fp32_dual_quaternion_normalize(BGC_FP32_DualQuaternion* quaternion) +inline int bgc_fp32_dual_quaternion_normalize(BGC_FP32_DualQuaternion* quaternion) { const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(&quaternion->real_part); + if (square_magnitude <= BGC_FP32_SQUARE_EPSILON || isnan(square_magnitude)) { + return BGC_FAILURE; + } + if (!bgc_fp32_is_square_unit(square_magnitude)) { - _bgc_fp32_quaternion_normalize_normalize_parts(quaternion, square_magnitude); + const float multiplier = sqrtf(1.0f / square_magnitude); + + bgc_fp32_quaternion_multiply_by_real_number(&quaternion->real_part, &quaternion->real_part, multiplier); + bgc_fp32_quaternion_multiply_by_real_number(&quaternion->dual_part, &quaternion->dual_part, multiplier); } const float dot_product = bgc_fp32_quaternion_get_dot_product(&quaternion->real_part, &quaternion->dual_part); bgc_fp32_quaternion_subtract_scaled(&quaternion->dual_part, &quaternion->dual_part, &quaternion->real_part, dot_product); + + return BGC_SUCCESS; } -inline void bgc_fp64_dual_quaternion_normalize(BGC_FP64_DualQuaternion* quaternion) +inline int bgc_fp64_dual_quaternion_normalize(BGC_FP64_DualQuaternion* quaternion) { const double square_magnitude = bgc_fp64_quaternion_get_square_magnitude(&quaternion->real_part); - if (!bgc_fp64_is_square_unit(square_magnitude)) { - _bgc_fp64_quaternion_normalize_normalize_parts(quaternion, square_magnitude); + if (square_magnitude <= BGC_FP64_SQUARE_EPSILON || isnan(square_magnitude)) { + return BGC_FAILURE; } - const double dot_product = bgc_fp64_quaternion_get_dot_product(&pose->_real_part, &pose->_dual_part); + if (!bgc_fp64_is_square_unit(square_magnitude)) { + const double multiplier = sqrt(1.0 / square_magnitude); - bgc_fp64_quaternion_subtract_scaled(&pose->_dual_part, &pose->_dual_part, &pose->_real_part, dot_product); + bgc_fp64_quaternion_multiply_by_real_number(&quaternion->real_part, &quaternion->real_part, multiplier); + bgc_fp64_quaternion_multiply_by_real_number(&quaternion->dual_part, &quaternion->dual_part, multiplier); + } + + const double dot_product = bgc_fp64_quaternion_get_dot_product(&quaternion->real_part, &quaternion->dual_part); + + bgc_fp64_quaternion_subtract_scaled(&quaternion->dual_part, &quaternion->dual_part, &quaternion->real_part, dot_product); + + return BGC_SUCCESS; +} + +// =============== Get Normalized =============== // + +inline int bgc_fp32_dual_quaternion_get_normalized(BGC_FP32_DualQuaternion* normalized, const BGC_FP32_DualQuaternion* quaternion) +{ + const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(&quaternion->real_part); + + if (square_magnitude <= BGC_FP32_SQUARE_EPSILON || isnan(square_magnitude)) { + return BGC_FAILURE; + } + + if (bgc_fp32_is_square_unit(square_magnitude)) { + bgc_fp32_quaternion_copy(&normalized->real_part, &quaternion->real_part); + bgc_fp32_quaternion_copy(&normalized->dual_part, &quaternion->dual_part); + } + else { + const float multiplier = sqrtf(1.0f / square_magnitude); + + bgc_fp32_quaternion_multiply_by_real_number(&normalized->real_part, &quaternion->real_part, multiplier); + bgc_fp32_quaternion_multiply_by_real_number(&normalized->dual_part, &quaternion->dual_part, multiplier); + } + + const float dot_product = bgc_fp32_quaternion_get_dot_product(&normalized->real_part, &normalized->dual_part); + + bgc_fp32_quaternion_subtract_scaled(&normalized->dual_part, &normalized->dual_part, &normalized->real_part, dot_product); + + return BGC_SUCCESS; +} + +inline int bgc_fp64_dual_quaternion_get_normalized(BGC_FP64_DualQuaternion* normalized, const BGC_FP64_DualQuaternion* quaternion) +{ + const double square_magnitude = bgc_fp64_quaternion_get_square_magnitude(&quaternion->real_part); + + if (square_magnitude <= BGC_FP64_SQUARE_EPSILON || isnan(square_magnitude)) { + return BGC_FAILURE; + } + + if (bgc_fp64_is_square_unit(square_magnitude)) { + bgc_fp64_quaternion_copy(&normalized->real_part, &quaternion->real_part); + bgc_fp64_quaternion_copy(&normalized->dual_part, &quaternion->dual_part); + } + else { + const double multiplier = sqrt(1.0 / square_magnitude); + + bgc_fp64_quaternion_multiply_by_real_number(&normalized->real_part, &quaternion->real_part, multiplier); + bgc_fp64_quaternion_multiply_by_real_number(&normalized->dual_part, &quaternion->dual_part, multiplier); + } + + const double dot_product = bgc_fp64_quaternion_get_dot_product(&normalized->real_part, &normalized->dual_part); + + bgc_fp64_quaternion_subtract_scaled(&normalized->dual_part, &normalized->dual_part, &normalized->real_part, dot_product); + + return BGC_SUCCESS; } // ==================== Add ===================== // @@ -350,14 +418,14 @@ inline void bgc_fp64_dual_quaternion_subtract_scaled(BGC_FP64_DualQuaternion* di inline void bgc_fp32_dual_quaternion_multiply_by_real_number(BGC_FP32_DualQuaternion* product, const BGC_FP32_DualQuaternion* multiplicand, const float multiplier) { - bgc_fp32_quaternion_multiply_by_real(&product->real_part, &multiplicand->real_part, multiplier); - bgc_fp32_quaternion_multiply_by_real(&product->dual_part, &multiplicand->dual_part, multiplier); + bgc_fp32_quaternion_multiply_by_real_number(&product->real_part, &multiplicand->real_part, multiplier); + bgc_fp32_quaternion_multiply_by_real_number(&product->dual_part, &multiplicand->dual_part, multiplier); } inline void bgc_fp64_dual_quaternion_multiply_by_real_number(BGC_FP64_DualQuaternion* product, const BGC_FP64_DualQuaternion* multiplicand, const double multiplier) { - bgc_fp64_quaternion_multiply_by_real(&product->real_part, &multiplicand->real_part, multiplier); - bgc_fp64_quaternion_multiply_by_real(&product->dual_part, &multiplicand->dual_part, multiplier); + bgc_fp64_quaternion_multiply_by_real_number(&product->real_part, &multiplicand->real_part, multiplier); + bgc_fp64_quaternion_multiply_by_real_number(&product->dual_part, &multiplicand->dual_part, multiplier); } // ========== Multiply by Dual Number =========== // @@ -366,10 +434,10 @@ inline void bgc_fp32_dual_quaternion_multiply_by_dual_number(BGC_FP32_DualQuater { BGC_FP32_Quaternion dual_part; - bgc_fp32_quaternion_multiply_by_real(&dual_part, &multiplicand->dual_part, multiplier->real_part); + bgc_fp32_quaternion_multiply_by_real_number(&dual_part, &multiplicand->dual_part, multiplier->real_part); bgc_fp32_quaternion_add_scaled(&dual_part, &dual_part, &multiplicand->real_part, multiplier->dual_part); - bgc_fp32_quaternion_multiply_by_real(&product->real_part, &multiplicand->real_part, multiplier->real_part); + bgc_fp32_quaternion_multiply_by_real_number(&product->real_part, &multiplicand->real_part, multiplier->real_part); bgc_fp32_quaternion_copy(&product->dual_part, &dual_part); } @@ -377,10 +445,10 @@ inline void bgc_fp64_dual_quaternion_multiply_by_dual_number(BGC_FP64_DualQuater { BGC_FP64_Quaternion dual_part; - bgc_fp64_quaternion_multiply_by_real(&dual_part, &multiplicand->dual_part, multiplier->real_part); + bgc_fp64_quaternion_multiply_by_real_number(&dual_part, &multiplicand->dual_part, multiplier->real_part); bgc_fp64_quaternion_add_scaled(&dual_part, &dual_part, &multiplicand->real_part, multiplier->dual_part); - bgc_fp64_quaternion_multiply_by_real(&product->real_part, &multiplicand->real_part, multiplier->real_part); + bgc_fp64_quaternion_multiply_by_real_number(&product->real_part, &multiplicand->real_part, multiplier->real_part); bgc_fp64_quaternion_copy(&product->dual_part, &dual_part); } @@ -390,10 +458,10 @@ inline void bgc_fp32_dual_quaternion_multiply_by_conjugate_dual_number(BGC_FP32_ { BGC_FP32_Quaternion dual_part; - bgc_fp32_quaternion_multiply_by_real(&dual_part, &multiplicand->dual_part, multiplier_to_conjugate->real_part); + bgc_fp32_quaternion_multiply_by_real_number(&dual_part, &multiplicand->dual_part, multiplier_to_conjugate->real_part); bgc_fp32_quaternion_subtract_scaled(&dual_part, &dual_part, &multiplicand->real_part, multiplier_to_conjugate->dual_part); - bgc_fp32_quaternion_multiply_by_real(&product->real_part, &multiplicand->real_part, multiplier_to_conjugate->real_part); + bgc_fp32_quaternion_multiply_by_real_number(&product->real_part, &multiplicand->real_part, multiplier_to_conjugate->real_part); bgc_fp32_quaternion_copy(&product->dual_part, &dual_part); } @@ -401,10 +469,10 @@ inline void bgc_fp64_dual_quaternion_multiply_by_conjugate_dual_number(BGC_FP64_ { BGC_FP64_Quaternion dual_part; - bgc_fp64_quaternion_multiply_by_real(&dual_part, &multiplicand->dual_part, multiplier_to_conjugate->real_part); + bgc_fp64_quaternion_multiply_by_real_number(&dual_part, &multiplicand->dual_part, multiplier_to_conjugate->real_part); bgc_fp64_quaternion_subtract_scaled(&dual_part, &dual_part, &multiplicand->real_part, multiplier_to_conjugate->dual_part); - bgc_fp64_quaternion_multiply_by_real(&product->real_part, &multiplicand->real_part, multiplier_to_conjugate->real_part); + bgc_fp64_quaternion_multiply_by_real_number(&product->real_part, &multiplicand->real_part, multiplier_to_conjugate->real_part); bgc_fp64_quaternion_copy(&product->dual_part, &dual_part); } @@ -470,8 +538,8 @@ inline int bgc_fp32_dual_quaternion_divide_by_real_number(BGC_FP32_DualQuaternio const float multiplier = 1.0f / divisor; - bgc_fp32_quaternion_multiply_by_real("ient->real_part, ÷nd->real_part, multiplier); - bgc_fp32_quaternion_multiply_by_real("ient->dual_part, ÷nd->dual_part, multiplier); + bgc_fp32_quaternion_multiply_by_real_number("ient->real_part, ÷nd->real_part, multiplier); + bgc_fp32_quaternion_multiply_by_real_number("ient->dual_part, ÷nd->dual_part, multiplier); return BGC_SUCCESS; } @@ -484,8 +552,8 @@ inline int bgc_fp64_dual_quaternion_divide_by_real_number(BGC_FP64_DualQuaternio const double multiplier = 1.0 / divisor; - bgc_fp64_quaternion_multiply_by_real("ient->real_part, ÷nd->real_part, multiplier); - bgc_fp64_quaternion_multiply_by_real("ient->dual_part, ÷nd->dual_part, multiplier); + bgc_fp64_quaternion_multiply_by_real_number("ient->real_part, ÷nd->real_part, multiplier); + bgc_fp64_quaternion_multiply_by_real_number("ient->dual_part, ÷nd->dual_part, multiplier); return BGC_SUCCESS; } @@ -565,8 +633,8 @@ inline int bgc_fp32_dual_quaternion_divide_by_quaternion(BGC_FP32_DualQuaternion bgc_fp32_quaternion_multiply_by_conjugate("ient->real_part, ÷nd->real_part, divisor); bgc_fp32_quaternion_multiply_by_conjugate("ient->dual_part, ÷nd->dual_part, divisor); - bgc_fp32_quaternion_multiply_by_real("ient->real_part, "ient->real_part, multiplier); - bgc_fp32_quaternion_multiply_by_real("ient->dual_part, "ient->dual_part, multiplier); + bgc_fp32_quaternion_multiply_by_real_number("ient->real_part, "ient->real_part, multiplier); + bgc_fp32_quaternion_multiply_by_real_number("ient->dual_part, "ient->dual_part, multiplier); return BGC_SUCCESS; } @@ -584,8 +652,8 @@ inline int bgc_fp64_dual_quaternion_divide_by_quaternion(BGC_FP64_DualQuaternion bgc_fp64_quaternion_multiply_by_conjugate("ient->real_part, ÷nd->real_part, divisor); bgc_fp64_quaternion_multiply_by_conjugate("ient->dual_part, ÷nd->dual_part, divisor); - bgc_fp64_quaternion_multiply_by_real("ient->real_part, "ient->real_part, multiplier); - bgc_fp64_quaternion_multiply_by_real("ient->dual_part, "ient->dual_part, multiplier); + bgc_fp64_quaternion_multiply_by_real_number("ient->real_part, "ient->real_part, multiplier); + bgc_fp64_quaternion_multiply_by_real_number("ient->dual_part, "ient->dual_part, multiplier); return BGC_SUCCESS; } @@ -605,8 +673,8 @@ inline int bgc_fp32_dual_quaternion_divide_by_conjugate_quaternion(BGC_FP32_Dual bgc_fp32_quaternion_multiply_by_quaternion("ient->real_part, ÷nd->real_part, divisor_to_conjugate); bgc_fp32_quaternion_multiply_by_quaternion("ient->dual_part, ÷nd->dual_part, divisor_to_conjugate); - bgc_fp32_quaternion_multiply_by_real("ient->real_part, "ient->real_part, multiplier); - bgc_fp32_quaternion_multiply_by_real("ient->dual_part, "ient->dual_part, multiplier); + bgc_fp32_quaternion_multiply_by_real_number("ient->real_part, "ient->real_part, multiplier); + bgc_fp32_quaternion_multiply_by_real_number("ient->dual_part, "ient->dual_part, multiplier); return BGC_SUCCESS; } @@ -624,8 +692,8 @@ inline int bgc_fp64_dual_quaternion_divide_by_conjugate_quaternion(BGC_FP64_Dual bgc_fp64_quaternion_multiply_by_quaternion("ient->real_part, ÷nd->real_part, divisor_to_conjugate); bgc_fp64_quaternion_multiply_by_quaternion("ient->dual_part, ÷nd->dual_part, divisor_to_conjugate); - bgc_fp64_quaternion_multiply_by_real("ient->real_part, "ient->real_part, multiplier); - bgc_fp64_quaternion_multiply_by_real("ient->dual_part, "ient->dual_part, multiplier); + bgc_fp64_quaternion_multiply_by_real_number("ient->real_part, "ient->real_part, multiplier); + bgc_fp64_quaternion_multiply_by_real_number("ient->dual_part, "ient->dual_part, multiplier); return BGC_SUCCESS; } diff --git a/basic-geometry/quaternion.c b/basic-geometry/quaternion.c index 5c76579..be269a9 100644 --- a/basic-geometry/quaternion.c +++ b/basic-geometry/quaternion.c @@ -43,8 +43,8 @@ extern inline void bgc_fp64_quaternion_subtract(BGC_FP64_Quaternion* difference, extern inline void bgc_fp32_quaternion_subtract_scaled(BGC_FP32_Quaternion* difference, const BGC_FP32_Quaternion* basic_quaternion, const BGC_FP32_Quaternion* scalable_quaternion, const float scale); extern inline void bgc_fp64_quaternion_subtract_scaled(BGC_FP64_Quaternion* difference, const BGC_FP64_Quaternion* basic_quaternion, const BGC_FP64_Quaternion* scalable_quaternion, const double scale); -extern inline void bgc_fp32_quaternion_multiply_by_real(BGC_FP32_Quaternion* product, const BGC_FP32_Quaternion* multiplicand, const float multiplier); -extern inline void bgc_fp64_quaternion_multiply_by_real(BGC_FP64_Quaternion* product, const BGC_FP64_Quaternion* multiplicand, const double multiplier); +extern inline void bgc_fp32_quaternion_multiply_by_real_number(BGC_FP32_Quaternion* product, const BGC_FP32_Quaternion* multiplicand, const float multiplier); +extern inline void bgc_fp64_quaternion_multiply_by_real_number(BGC_FP64_Quaternion* product, const BGC_FP64_Quaternion* multiplicand, const double multiplier); extern inline void bgc_fp32_quaternion_multiply_by_quaternion(BGC_FP32_Quaternion* product, const BGC_FP32_Quaternion* left, const BGC_FP32_Quaternion* right); extern inline void bgc_fp64_quaternion_multiply_by_quaternion(BGC_FP64_Quaternion* product, const BGC_FP64_Quaternion* left, const BGC_FP64_Quaternion* right); diff --git a/basic-geometry/quaternion.h b/basic-geometry/quaternion.h index 5e513ac..a801394 100644 --- a/basic-geometry/quaternion.h +++ b/basic-geometry/quaternion.h @@ -258,7 +258,7 @@ inline void bgc_fp64_quaternion_subtract_scaled(BGC_FP64_Quaternion* difference, // ============= Multiply By Number ============= // -inline void bgc_fp32_quaternion_multiply_by_real(BGC_FP32_Quaternion* product, const BGC_FP32_Quaternion* multiplicand, const float multiplier) +inline void bgc_fp32_quaternion_multiply_by_real_number(BGC_FP32_Quaternion* product, const BGC_FP32_Quaternion* multiplicand, const float multiplier) { product->s0 = multiplicand->s0 * multiplier; product->x1 = multiplicand->x1 * multiplier; @@ -266,7 +266,7 @@ inline void bgc_fp32_quaternion_multiply_by_real(BGC_FP32_Quaternion* product, c product->x3 = multiplicand->x3 * multiplier; } -inline void bgc_fp64_quaternion_multiply_by_real(BGC_FP64_Quaternion* product, const BGC_FP64_Quaternion* multiplicand, const double multiplier) +inline void bgc_fp64_quaternion_multiply_by_real_number(BGC_FP64_Quaternion* product, const BGC_FP64_Quaternion* multiplicand, const double multiplier) { product->s0 = multiplicand->s0 * multiplier; product->x1 = multiplicand->x1 * multiplier; @@ -370,14 +370,14 @@ inline void _bgc_fp64_restrict_quaternion_multiply_by_conjugate(BGC_FP64_Quatern inline void bgc_fp32_quaternion_multiply_by_dual_number(BGC_FP32_DualQuaternion* product, const BGC_FP32_Quaternion* multiplicand, const BGC_FP32_DualNumber* multiplier) { - bgc_fp32_quaternion_multiply_by_real(&product->real_part, multiplicand, multiplier->real_part); - bgc_fp32_quaternion_multiply_by_real(&product->dual_part, multiplicand, multiplier->dual_part); + bgc_fp32_quaternion_multiply_by_real_number(&product->real_part, multiplicand, multiplier->real_part); + bgc_fp32_quaternion_multiply_by_real_number(&product->dual_part, multiplicand, multiplier->dual_part); } inline void bgc_fp64_quaternion_multiply_by_dual_number(BGC_FP64_DualQuaternion* product, const BGC_FP64_Quaternion* multiplicand, const BGC_FP64_DualNumber* multiplier) { - bgc_fp64_quaternion_multiply_by_real(&product->real_part, multiplicand, multiplier->real_part); - bgc_fp64_quaternion_multiply_by_real(&product->dual_part, multiplicand, multiplier->dual_part); + bgc_fp64_quaternion_multiply_by_real_number(&product->real_part, multiplicand, multiplier->real_part); + bgc_fp64_quaternion_multiply_by_real_number(&product->dual_part, multiplicand, multiplier->dual_part); } // ======== Multiply By Dual Quaternion ========= // @@ -402,7 +402,7 @@ inline int bgc_fp32_quaternion_divide_by_real(BGC_FP32_Quaternion* quotient, con return BGC_FAILURE; } - bgc_fp32_quaternion_multiply_by_real(quotient, dividend, 1.0f / divisor); + bgc_fp32_quaternion_multiply_by_real_number(quotient, dividend, 1.0f / divisor); return BGC_SUCCESS; } @@ -413,7 +413,7 @@ inline int bgc_fp64_quaternion_divide_by_real(BGC_FP64_Quaternion* quotient, con return BGC_FAILURE; } - bgc_fp64_quaternion_multiply_by_real(quotient, dividend, 1.0 / divisor); + bgc_fp64_quaternion_multiply_by_real_number(quotient, dividend, 1.0 / divisor); return BGC_SUCCESS; } @@ -429,7 +429,7 @@ inline int bgc_fp32_quaternion_divide_by_quaternion(BGC_FP32_Quaternion* quotien } bgc_fp32_quaternion_multiply_by_conjugate(quotient, divident, divisor); - bgc_fp32_quaternion_multiply_by_real(quotient, quotient, 1.0f / square_modulus); + bgc_fp32_quaternion_multiply_by_real_number(quotient, quotient, 1.0f / square_modulus); return BGC_SUCCESS; } @@ -443,7 +443,7 @@ inline int bgc_fp64_quaternion_divide_by_quaternion(BGC_FP64_Quaternion* quotien } bgc_fp64_quaternion_multiply_by_conjugate(quotient, divident, divisor); - bgc_fp64_quaternion_multiply_by_real(quotient, quotient, 1.0 / square_modulus); + bgc_fp64_quaternion_multiply_by_real_number(quotient, quotient, 1.0 / square_modulus); return BGC_SUCCESS; } @@ -459,7 +459,7 @@ inline int bgc_fp32_quaternion_divide_by_conjugate(BGC_FP32_Quaternion* quotient } bgc_fp32_quaternion_multiply_by_quaternion(quotient, divisor_to_conjugate, divisor_to_conjugate); - bgc_fp32_quaternion_multiply_by_real(quotient, quotient, 1.0f / square_modulus); + bgc_fp32_quaternion_multiply_by_real_number(quotient, quotient, 1.0f / square_modulus); return BGC_SUCCESS; } @@ -473,7 +473,7 @@ inline int bgc_fp64_quaternion_divide_by_conjugate(BGC_FP64_Quaternion* quotient } bgc_fp64_quaternion_multiply_by_quaternion(quotient, divisor_to_conjugate, divisor_to_conjugate); - bgc_fp64_quaternion_multiply_by_real(quotient, quotient, 1.0 / square_modulus); + bgc_fp64_quaternion_multiply_by_real_number(quotient, quotient, 1.0 / square_modulus); return BGC_SUCCESS; } @@ -722,7 +722,7 @@ inline int bgc_fp32_quaternion_get_normalized(BGC_FP32_Quaternion* normalized, c return BGC_SUCCESS; } - bgc_fp32_quaternion_multiply_by_real(normalized, quaternion, sqrtf(1.0f / square_modulus)); + bgc_fp32_quaternion_multiply_by_real_number(normalized, quaternion, sqrtf(1.0f / square_modulus)); return BGC_SUCCESS; } @@ -740,7 +740,7 @@ inline int bgc_fp64_quaternion_get_normalized(BGC_FP64_Quaternion* normalized, c return BGC_SUCCESS; } - bgc_fp64_quaternion_multiply_by_real(normalized, quaternion, sqrt(1.0 / square_modulus)); + bgc_fp64_quaternion_multiply_by_real_number(normalized, quaternion, sqrt(1.0 / square_modulus)); return BGC_SUCCESS; } diff --git a/basic-geometry/rigid-pose3.c b/basic-geometry/rigid-pose3.c index 973e847..72cb97b 100644 --- a/basic-geometry/rigid-pose3.c +++ b/basic-geometry/rigid-pose3.c @@ -1,34 +1,5 @@ #include "rigid-pose3.h" -void _bgc_fp32_rigid_pose3_normalize_parts(BGC_FP32_RigidPose3* pose, const float square_magnitude) -{ - if (square_magnitude <= BGC_FP32_SQUARE_EPSILON || isnan(square_magnitude)) { - bgc_fp32_rigid_pose3_reset(pose); - return; - } - - const float multiplier = sqrtf(1.0f / square_magnitude); - - bgc_fp32_quaternion_multiply_by_real(&pose->_real_part, &pose->_real_part, multiplier); - bgc_fp32_quaternion_multiply_by_real(&pose->_dual_part, &pose->_dual_part, multiplier); -} - -void _bgc_fp64_rigid_pose3_normalize_parts(BGC_FP64_RigidPose3* pose, const double square_magnitude) -{ - if (square_magnitude <= BGC_FP64_SQUARE_EPSILON || isnan(square_magnitude)) { - bgc_fp64_rigid_pose3_reset(pose); - return; - } - - const double multiplier = sqrt(1.0 / square_magnitude); - - bgc_fp64_quaternion_multiply_by_real(&pose->_real_part, &pose->_real_part, multiplier); - bgc_fp64_quaternion_multiply_by_real(&pose->_dual_part, &pose->_dual_part, multiplier); -} - -extern inline void _bgc_fp32_rigid_pose3_normalize(BGC_FP32_RigidPose3* pose); -extern inline void _bgc_fp64_rigid_pose3_normalize(BGC_FP64_RigidPose3* pose); - extern inline void bgc_fp32_rigid_pose3_reset(BGC_FP32_RigidPose3* pose); extern inline void bgc_fp64_rigid_pose3_reset(BGC_FP64_RigidPose3* pose); diff --git a/basic-geometry/rigid-pose3.h b/basic-geometry/rigid-pose3.h index 0a9b850..1c1c8dc 100644 --- a/basic-geometry/rigid-pose3.h +++ b/basic-geometry/rigid-pose3.h @@ -1,112 +1,117 @@ #ifndef _BGC_RIGID_POSE3_H_INCLUDED_ #define _BGC_RIGID_POSE3_H_INCLUDED_ +#include + #include "types.h" #include "quaternion.h" - -// ================= Normalize ================== // - -void _bgc_fp32_rigid_pose3_normalize_parts(BGC_FP32_RigidPose3* pose, const float square_magnitude); - -void _bgc_fp64_rigid_pose3_normalize_parts(BGC_FP64_RigidPose3* pose, const double square_magnitude); - -inline void _bgc_fp32_rigid_pose3_normalize(BGC_FP32_RigidPose3* pose) -{ - const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(&pose->_real_part); - - if (!bgc_fp32_is_square_unit(square_magnitude)) { - _bgc_fp32_rigid_pose3_normalize_parts(pose, square_magnitude); - } - - const float dot_product = bgc_fp32_quaternion_get_dot_product(&pose->_real_part, &pose->_dual_part); - - bgc_fp32_quaternion_subtract_scaled(&pose->_dual_part, &pose->_dual_part, &pose->_real_part, dot_product); -} - -inline void _bgc_fp64_rigid_pose3_normalize(BGC_FP64_RigidPose3* pose) -{ - const double square_magnitude = bgc_fp64_quaternion_get_square_magnitude(&pose->_real_part); - - if (!bgc_fp64_is_square_unit(square_magnitude)) { - _bgc_fp64_rigid_pose3_normalize_parts(pose, square_magnitude); - } - - const double dot_product = bgc_fp64_quaternion_get_dot_product(&pose->_real_part, &pose->_dual_part); - - bgc_fp64_quaternion_subtract_scaled(&pose->_dual_part, &pose->_dual_part, &pose->_real_part, dot_product); -} +#include "dual-quaternion.h" // ==================== Reset =================== // inline void bgc_fp32_rigid_pose3_reset(BGC_FP32_RigidPose3* pose) { - pose->_real_part.s0 = 1.0f; - pose->_real_part.x1 = 0.0f; - pose->_real_part.x2 = 0.0f; - pose->_real_part.x3 = 0.0f; + pose->_versor.real_part.s0 = 1.0f; + pose->_versor.real_part.x1 = 0.0f; + pose->_versor.real_part.x2 = 0.0f; + pose->_versor.real_part.x3 = 0.0f; - pose->_dual_part.s0 = 0.0f; - pose->_dual_part.x1 = 0.0f; - pose->_dual_part.x2 = 0.0f; - pose->_dual_part.x3 = 0.0f; + pose->_versor.dual_part.s0 = 0.0f; + pose->_versor.dual_part.x1 = 0.0f; + pose->_versor.dual_part.x2 = 0.0f; + pose->_versor.dual_part.x3 = 0.0f; } inline void bgc_fp64_rigid_pose3_reset(BGC_FP64_RigidPose3* pose) { - pose->_real_part.s0 = 1.0; - pose->_real_part.x1 = 0.0; - pose->_real_part.x2 = 0.0; - pose->_real_part.x3 = 0.0; + pose->_versor.real_part.s0 = 1.0; + pose->_versor.real_part.x1 = 0.0; + pose->_versor.real_part.x2 = 0.0; + pose->_versor.real_part.x3 = 0.0; - pose->_dual_part.s0 = 0.0; - pose->_dual_part.x1 = 0.0; - pose->_dual_part.x2 = 0.0; - pose->_dual_part.x3 = 0.0; + pose->_versor.dual_part.s0 = 0.0; + pose->_versor.dual_part.x1 = 0.0; + pose->_versor.dual_part.x2 = 0.0; + pose->_versor.dual_part.x3 = 0.0; +} + +// ================= Normalize ================== // + +inline void _bgc_fp32_rigid_pose3_normalize(BGC_FP32_RigidPose3* pose) +{ + const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(&pose->_versor.real_part); + + if (square_magnitude <= BGC_FP32_SQUARE_EPSILON || isnan(square_magnitude)) { + bgc_fp32_rigid_pose3_reset(pose); + return; + } + + if (!bgc_fp32_is_square_unit(square_magnitude)) { + const float multiplier = sqrtf(1.0f / square_magnitude); + + bgc_fp32_dual_quaternion_multiply_by_real_number(&pose->_versor, &pose->_versor, multiplier); + } + + const float dot_product = bgc_fp32_quaternion_get_dot_product(&pose->_versor.real_part, &pose->_versor.dual_part); + + bgc_fp32_quaternion_subtract_scaled(&pose->_versor.dual_part, &pose->_versor.dual_part, &pose->_versor.real_part, dot_product); +} + +inline void _bgc_fp64_rigid_pose3_normalize(BGC_FP64_RigidPose3* pose) +{ + const double square_magnitude = bgc_fp64_quaternion_get_square_magnitude(&pose->_versor.real_part); + + if (square_magnitude <= BGC_FP64_SQUARE_EPSILON || isnan(square_magnitude)) { + bgc_fp64_rigid_pose3_reset(pose); + return; + } + + if (!bgc_fp64_is_square_unit(square_magnitude)) { + const double multiplier = sqrt(1.0 / square_magnitude); + + bgc_fp64_dual_quaternion_multiply_by_real_number(&pose->_versor, &pose->_versor, multiplier); + } + + const double dot_product = bgc_fp64_quaternion_get_dot_product(&pose->_versor.real_part, &pose->_versor.dual_part); + + bgc_fp64_quaternion_subtract_scaled(&pose->_versor.dual_part, &pose->_versor.dual_part, &pose->_versor.real_part, dot_product); } // ==================== Copy ==================== // inline void bgc_fp32_rigid_pose3_copy(BGC_FP32_RigidPose3* destination, const BGC_FP32_RigidPose3* source) { - bgc_fp32_quaternion_copy(&destination->_real_part, &source->_real_part); - bgc_fp32_quaternion_copy(&destination->_dual_part, &source->_dual_part); + bgc_fp32_dual_quaternion_copy(&destination->_versor, &source->_versor); } inline void bgc_fp64_rigid_pose3_copy(BGC_FP64_RigidPose3* destination, const BGC_FP64_RigidPose3* source) { - bgc_fp64_quaternion_copy(&destination->_real_part, &source->_real_part); - bgc_fp64_quaternion_copy(&destination->_dual_part, &source->_dual_part); + bgc_fp64_dual_quaternion_copy(&destination->_versor, &source->_versor); } // ==================== Swap ==================== // inline void bgc_fp32_rigid_pose3_swap(BGC_FP32_RigidPose3* pose1, BGC_FP32_RigidPose3* pose2) { - bgc_fp32_quaternion_swap(&pose1->_real_part, &pose2->_real_part); - bgc_fp32_quaternion_swap(&pose1->_dual_part, &pose2->_dual_part); + bgc_fp32_dual_quaternion_swap(&pose1->_versor, &pose2->_versor); } inline void bgc_fp64_rigid_pose3_swap(BGC_FP64_RigidPose3* pose1, BGC_FP64_RigidPose3* pose2) { - bgc_fp64_quaternion_swap(&pose1->_real_part, &pose2->_real_part); - bgc_fp64_quaternion_swap(&pose1->_dual_part, &pose2->_dual_part); + bgc_fp64_dual_quaternion_swap(&pose1->_versor, &pose2->_versor); } // ================== Convert =================== // inline void bgc_fp32_rigid_pose3_convert_to_fp64(BGC_FP64_RigidPose3* destination, const BGC_FP32_RigidPose3* 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); - + bgc_fp32_dual_quaternion_convert_to_fp64(&destination->_versor, &source->_versor); _bgc_fp64_rigid_pose3_normalize(destination); } inline void bgc_fp64_rigid_pose3_convert_to_fp32(BGC_FP32_RigidPose3* destination, const BGC_FP64_RigidPose3* 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); - + bgc_fp64_dual_quaternion_convert_to_fp32(&destination->_versor, &source->_versor); _bgc_fp32_rigid_pose3_normalize(destination); } @@ -114,27 +119,13 @@ inline void bgc_fp64_rigid_pose3_convert_to_fp32(BGC_FP32_RigidPose3* destinatio inline void bgc_fp32_rigid_pose3_combine(BGC_FP32_RigidPose3* combination, const BGC_FP32_RigidPose3* first, const BGC_FP32_RigidPose3* second) { - BGC_FP32_Quaternion dual_part1, dual_part2; - - _bgc_fp32_restrict_quaternion_multiply_by_quaternion(&dual_part1, &second->_real_part, &first->_dual_part); - _bgc_fp32_restrict_quaternion_multiply_by_quaternion(&dual_part2, &second->_dual_part, &first->_real_part); - - bgc_fp32_quaternion_multiply_by_quaternion(&combination->_real_part, &second->_real_part, &first->_real_part); - bgc_fp32_quaternion_add(&combination->_dual_part, &dual_part1, &dual_part2); - + bgc_fp32_dual_quaternion_multiply_by_dual_quaternion(&combination->_versor, &second->_versor, &first->_versor); _bgc_fp32_rigid_pose3_normalize(combination); } inline void bgc_fp64_rigid_pose3_combine(BGC_FP64_RigidPose3* combination, const BGC_FP64_RigidPose3* first, const BGC_FP64_RigidPose3* second) { - BGC_FP64_Quaternion dual_part1, dual_part2; - - _bgc_fp64_restrict_quaternion_multiply_by_quaternion(&dual_part1, &second->_real_part, &first->_dual_part); - _bgc_fp64_restrict_quaternion_multiply_by_quaternion(&dual_part2, &second->_dual_part, &first->_real_part); - - bgc_fp64_quaternion_multiply_by_quaternion(&combination->_real_part, &second->_real_part, &first->_real_part); - bgc_fp64_quaternion_add(&combination->_dual_part, &dual_part1, &dual_part2); - + bgc_fp64_dual_quaternion_multiply_by_dual_quaternion(&combination->_versor, &second->_versor, &first->_versor); _bgc_fp64_rigid_pose3_normalize(combination); } diff --git a/basic-geometry/slerp3.h b/basic-geometry/slerp3.h index ffc1e13..e6f2614 100644 --- a/basic-geometry/slerp3.h +++ b/basic-geometry/slerp3.h @@ -94,7 +94,7 @@ inline void bgc_fp32_slerp3_get_phase_turn(BGC_FP32_Turn3* turn, const BGC_FP32_ BGC_FP32_Quaternion q; - bgc_fp32_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine); + bgc_fp32_quaternion_multiply_by_real_number(&q, &slerp->_cosine_weight, cosine); bgc_fp32_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine); bgc_fp32_turn3_set_quaternion(turn, &q); @@ -108,7 +108,7 @@ inline void bgc_fp64_slerp3_get_phase_turn(BGC_FP64_Turn3* turn, const BGC_FP64_ BGC_FP64_Quaternion q; - bgc_fp64_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine); + bgc_fp64_quaternion_multiply_by_real_number(&q, &slerp->_cosine_weight, cosine); bgc_fp64_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine); bgc_fp64_turn3_set_quaternion(turn, &q); @@ -124,7 +124,7 @@ inline void bgc_fp32_slerp3_get_phase_rotation_matrix(BGC_FP32_Matrix3x3* rotati BGC_FP32_Quaternion q; - bgc_fp32_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine); + bgc_fp32_quaternion_multiply_by_real_number(&q, &slerp->_cosine_weight, cosine); bgc_fp32_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine); bgc_fp32_quaternion_get_rotation_matrix(rotation_matrix, &q); @@ -138,7 +138,7 @@ inline void bgc_fp64_slerp3_get_phase_rotation_matrix(BGC_FP64_Matrix3x3* rotati BGC_FP64_Quaternion q; - bgc_fp64_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine); + bgc_fp64_quaternion_multiply_by_real_number(&q, &slerp->_cosine_weight, cosine); bgc_fp64_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine); bgc_fp64_quaternion_get_rotation_matrix(rotation_matrix, &q); @@ -154,7 +154,7 @@ inline void bgc_fp32_slerp3_get_phase_reverse_matrix(BGC_FP32_Matrix3x3* reverse BGC_FP32_Quaternion q; - bgc_fp32_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine); + bgc_fp32_quaternion_multiply_by_real_number(&q, &slerp->_cosine_weight, cosine); bgc_fp32_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine); bgc_fp32_quaternion_get_reverse_matrix(reverse_matrix, &q); @@ -168,7 +168,7 @@ inline void bgc_fp64_slerp3_get_phase_reverse_matrix(BGC_FP64_Matrix3x3* reverse BGC_FP64_Quaternion q; - bgc_fp64_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine); + bgc_fp64_quaternion_multiply_by_real_number(&q, &slerp->_cosine_weight, cosine); bgc_fp64_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine); bgc_fp64_quaternion_get_reverse_matrix(reverse_matrix, &q); @@ -184,7 +184,7 @@ inline void bgc_fp32_slerp3_get_phase_both_matrices(BGC_FP32_Matrix3x3* rotation BGC_FP32_Quaternion q; - bgc_fp32_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine); + bgc_fp32_quaternion_multiply_by_real_number(&q, &slerp->_cosine_weight, cosine); bgc_fp32_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine); bgc_fp32_quaternion_get_both_matrices(rotation_matrix, reverse_matrix, &q); @@ -198,7 +198,7 @@ inline void bgc_fp64_slerp3_get_phase_both_matrices(BGC_FP64_Matrix3x3* rotation BGC_FP64_Quaternion q; - bgc_fp64_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine); + bgc_fp64_quaternion_multiply_by_real_number(&q, &slerp->_cosine_weight, cosine); bgc_fp64_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine); bgc_fp64_quaternion_get_both_matrices(rotation_matrix, reverse_matrix, &q); diff --git a/basic-geometry/turn3.c b/basic-geometry/turn3.c index 17dbe9b..3a09baf 100644 --- a/basic-geometry/turn3.c +++ b/basic-geometry/turn3.c @@ -86,7 +86,7 @@ void _bgc_fp32_turn3_normalize(BGC_FP32_Turn3* turn, const float square_modulus) return; } - bgc_fp32_quaternion_multiply_by_real(&turn->_versor, &turn->_versor, sqrtf(1.0f / square_modulus)); + bgc_fp32_quaternion_multiply_by_real_number(&turn->_versor, &turn->_versor, sqrtf(1.0f / square_modulus)); } void _bgc_fp64_turn3_normalize(BGC_FP64_Turn3* turn, const double square_modulus) @@ -96,7 +96,7 @@ void _bgc_fp64_turn3_normalize(BGC_FP64_Turn3* turn, const double square_modulus return; } - bgc_fp64_quaternion_multiply_by_real(&turn->_versor, &turn->_versor, sqrt(1.0 / square_modulus)); + bgc_fp64_quaternion_multiply_by_real_number(&turn->_versor, &turn->_versor, sqrt(1.0 / square_modulus)); } diff --git a/basic-geometry/types.h b/basic-geometry/types.h index bf34326..36c8819 100644 --- a/basic-geometry/types.h +++ b/basic-geometry/types.h @@ -214,11 +214,11 @@ typedef struct { // ================ Rigid Pose3 ================= // typedef struct { - BGC_FP32_Quaternion _real_part, _dual_part; + BGC_FP32_DualQuaternion _versor; } BGC_FP32_RigidPose3; typedef struct { - BGC_FP64_Quaternion _real_part, _dual_part; + BGC_FP64_DualQuaternion _versor; } BGC_FP64_RigidPose3; #endif