From 7591b20f41c8bc9f1ed1f993acd8f5538e6e0aed Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Mon, 16 Mar 2026 20:12:09 +0700 Subject: [PATCH] =?UTF-8?q?=D0=9D=D0=BE=D1=80=D0=BC=D0=B0=D0=BB=D0=B8?= =?UTF-8?q?=D0=B7=D0=B0=D1=86=D0=B8=D1=8F=20=D1=83=D0=BD=D0=B8=D1=82=D0=B0?= =?UTF-8?q?=D1=80=D0=BD=D1=8B=D1=85=20=D0=B4=D1=83=D0=B0=D0=BB=D1=8C=D0=BD?= =?UTF-8?q?=D1=8B=D1=85=20=D0=BA=D0=B2=D0=B0=D1=82=D0=B5=D1=80=D0=BD=D0=B8?= =?UTF-8?q?=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/basic-geometry.vcxproj | 4 +- basic-geometry/basic-geometry.vcxproj.filters | 4 +- basic-geometry/dual-quaternion.h | 4 +- basic-geometry/quaternion.c | 6 +++ basic-geometry/quaternion.h | 40 ++++++++++++++++++- basic-geometry/rigid-pose3.h | 32 +++++++++++++-- 6 files changed, 78 insertions(+), 12 deletions(-) diff --git a/basic-geometry/basic-geometry.vcxproj b/basic-geometry/basic-geometry.vcxproj index 5bbc782..6c03bb8 100644 --- a/basic-geometry/basic-geometry.vcxproj +++ b/basic-geometry/basic-geometry.vcxproj @@ -31,7 +31,7 @@ - + @@ -53,7 +53,7 @@ - + diff --git a/basic-geometry/basic-geometry.vcxproj.filters b/basic-geometry/basic-geometry.vcxproj.filters index 49d9826..5ddf03b 100644 --- a/basic-geometry/basic-geometry.vcxproj.filters +++ b/basic-geometry/basic-geometry.vcxproj.filters @@ -84,7 +84,7 @@ Файлы заголовков - + Файлы заголовков @@ -146,7 +146,7 @@ Исходные файлы - + Исходные файлы diff --git a/basic-geometry/dual-quaternion.h b/basic-geometry/dual-quaternion.h index 273657b..28f9e7d 100644 --- a/basic-geometry/dual-quaternion.h +++ b/basic-geometry/dual-quaternion.h @@ -410,8 +410,8 @@ inline void bgc_fp32_dual_quaternion_multiply_by_dual_quaternion(BGC_FP32_DualQu { BGC_FP32_Quaternion dual_part1, dual_part2; - bgc_fp32_quaternion_multiply_by_quaternion(&dual_part1, &left->real_part, &right->dual_part); - bgc_fp32_quaternion_multiply_by_quaternion(&dual_part2, &left->dual_part, &right->real_part); + _bgc_fp32_restrict_quaternion_multiply_by_quaternion(&dual_part1, &left->real_part, &right->dual_part); + _bgc_fp32_restrict_quaternion_multiply_by_quaternion(&dual_part2, &left->dual_part, &right->real_part); bgc_fp32_quaternion_multiply_by_quaternion(&product->real_part, &left->real_part, &right->real_part); bgc_fp32_quaternion_add(&product->dual_part, &dual_part1, &dual_part2); diff --git a/basic-geometry/quaternion.c b/basic-geometry/quaternion.c index dd9fac2..5c76579 100644 --- a/basic-geometry/quaternion.c +++ b/basic-geometry/quaternion.c @@ -58,6 +58,12 @@ extern inline void bgc_fp64_quaternion_multiply_by_dual_quaternion(BGC_FP64_Dual extern inline void bgc_fp32_quaternion_multiply_by_conjugate(BGC_FP32_Quaternion* product, const BGC_FP32_Quaternion* left, const BGC_FP32_Quaternion* right); extern inline void bgc_fp64_quaternion_multiply_by_conjugate(BGC_FP64_Quaternion* product, const BGC_FP64_Quaternion* left, const BGC_FP64_Quaternion* right); +extern inline void _bgc_fp32_restrict_quaternion_multiply_by_quaternion(BGC_FP32_Quaternion* restrict product, const BGC_FP32_Quaternion* left, const BGC_FP32_Quaternion* right); +extern inline void _bgc_fp64_restrict_quaternion_multiply_by_quaternion(BGC_FP64_Quaternion* restrict product, const BGC_FP64_Quaternion* left, const BGC_FP64_Quaternion* right); + +extern inline void _bgc_fp32_restrict_quaternion_multiply_by_conjugate(BGC_FP32_Quaternion* restrict product, const BGC_FP32_Quaternion* left, const BGC_FP32_Quaternion* right); +extern inline void _bgc_fp64_restrict_quaternion_multiply_by_conjugate(BGC_FP64_Quaternion* restrict product, const BGC_FP64_Quaternion* left, const BGC_FP64_Quaternion* right); + extern inline int bgc_fp32_quaternion_divide_by_real(BGC_FP32_Quaternion* quotient, const BGC_FP32_Quaternion* dividend, const float divisor); extern inline int bgc_fp64_quaternion_divide_by_real(BGC_FP64_Quaternion* quotient, const BGC_FP64_Quaternion* dividend, const double divisor); diff --git a/basic-geometry/quaternion.h b/basic-geometry/quaternion.h index 4edc232..5e513ac 100644 --- a/basic-geometry/quaternion.h +++ b/basic-geometry/quaternion.h @@ -330,6 +330,42 @@ inline void bgc_fp64_quaternion_multiply_by_conjugate(BGC_FP64_Quaternion* produ product->x3 = x3; } +// ====== Restrict Multiply By Quaternion ======= // + +inline void _bgc_fp32_restrict_quaternion_multiply_by_quaternion(BGC_FP32_Quaternion* restrict product, const BGC_FP32_Quaternion* left, const BGC_FP32_Quaternion* right) +{ + product->s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3); + product->x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3); + product->x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1); + product->x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2); +} + +inline void _bgc_fp64_restrict_quaternion_multiply_by_quaternion(BGC_FP64_Quaternion* restrict product, const BGC_FP64_Quaternion* left, const BGC_FP64_Quaternion* right) +{ + product->s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3); + product->x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3); + product->x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1); + product->x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2); +} + +// = Restrict Multiply By Conjugate Quaternion == // + +inline void _bgc_fp32_restrict_quaternion_multiply_by_conjugate(BGC_FP32_Quaternion* restrict product, const BGC_FP32_Quaternion* left, const BGC_FP32_Quaternion* right) +{ + product->s0 = (left->s0 * right->s0 + left->x1 * right->x1) + (left->x2 * right->x2 + left->x3 * right->x3); + product->x1 = (left->x1 * right->s0 + left->x3 * right->x2) - (left->s0 * right->x1 + left->x2 * right->x3); + product->x2 = (left->x2 * right->s0 + left->x1 * right->x3) - (left->s0 * right->x2 + left->x3 * right->x1); + product->x3 = (left->x3 * right->s0 + left->x2 * right->x1) - (left->s0 * right->x3 + left->x1 * right->x2); +} + +inline void _bgc_fp64_restrict_quaternion_multiply_by_conjugate(BGC_FP64_Quaternion* restrict product, const BGC_FP64_Quaternion* left, const BGC_FP64_Quaternion* right) +{ + product->s0 = (left->s0 * right->s0 + left->x1 * right->x1) + (left->x2 * right->x2 + left->x3 * right->x3); + product->x1 = (left->x1 * right->s0 + left->x3 * right->x2) - (left->s0 * right->x1 + left->x2 * right->x3); + product->x2 = (left->x2 * right->s0 + left->x1 * right->x3) - (left->s0 * right->x2 + left->x3 * right->x1); + product->x3 = (left->x3 * right->s0 + left->x2 * right->x1) - (left->s0 * right->x3 + left->x1 * right->x2); +} + // ========== Multiply By Dual Number =========== // inline void bgc_fp32_quaternion_multiply_by_dual_number(BGC_FP32_DualQuaternion* product, const BGC_FP32_Quaternion* multiplicand, const BGC_FP32_DualNumber* multiplier) @@ -504,12 +540,12 @@ inline void bgc_fp64_quaternion_interpolate(BGC_FP64_Quaternion* interpolation, inline float bgc_fp32_quaternion_get_dot_product(const BGC_FP32_Quaternion* quaternion1, const BGC_FP32_Quaternion* quaternion2) { - return (quaternion1->s0 * quaternion2->s0 + quaternion1->x1 * quaternion2->x1) + (quaternion1->x2 * quaternion2->x2 + quaternion1->x3 * quaternion2->x3) + return (quaternion1->s0 * quaternion2->s0 + quaternion1->x1 * quaternion2->x1) + (quaternion1->x2 * quaternion2->x2 + quaternion1->x3 * quaternion2->x3); } inline double bgc_fp64_quaternion_get_dot_product(const BGC_FP64_Quaternion* quaternion1, const BGC_FP64_Quaternion* quaternion2) { - return (quaternion1->s0 * quaternion2->s0 + quaternion1->x1 * quaternion2->x1) + (quaternion1->x2 * quaternion2->x2 + quaternion1->x3 * quaternion2->x3) + return (quaternion1->s0 * quaternion2->s0 + quaternion1->x1 * quaternion2->x1) + (quaternion1->x2 * quaternion2->x2 + quaternion1->x3 * quaternion2->x3); } // ================= Conjugate ================== // diff --git a/basic-geometry/rigid-pose3.h b/basic-geometry/rigid-pose3.h index 10b1f3f..8e380ab 100644 --- a/basic-geometry/rigid-pose3.h +++ b/basic-geometry/rigid-pose3.h @@ -79,9 +79,9 @@ inline void bgc_fp32_rigid_pose3_convert_to_fp64(BGC_FP64_RigidPose3* destinatio _bgc_fp64_rigid_pose3_normalize(destination, square_magnitude); } - const double square_magnitude = bgc_fp64_quaternion_get_(&destination->_real_part); + const double dot_product = bgc_fp64_quaternion_get_dot_product(&destination->_real_part, &destination->_dual_part); - bgc_fp64_quaternion_subtract_scaled(&destination->_dual_part, &destination->_dual_part, &destination->_real_part); + bgc_fp64_quaternion_subtract_scaled(&destination->_dual_part, &destination->_dual_part, &destination->_real_part, dot_product); } inline void bgc_fp64_rigid_pose3_convert_to_fp32(BGC_FP32_RigidPose3* destination, const BGC_FP64_RigidPose3* source) @@ -94,30 +94,54 @@ inline void bgc_fp64_rigid_pose3_convert_to_fp32(BGC_FP32_RigidPose3* destinatio if (!bgc_fp32_is_square_unit(square_magnitude)) { _bgc_fp32_rigid_pose3_normalize(destination, square_magnitude); } + + const float dot_product = bgc_fp32_quaternion_get_dot_product(&destination->_real_part, &destination->_dual_part); + + bgc_fp32_quaternion_subtract_scaled(&destination->_dual_part, &destination->_dual_part, &destination->_real_part, dot_product); } // ================== Combine =================== // inline void bgc_fp32_rigid_pose3_combine(BGC_FP32_RigidPose3* combination, const BGC_FP32_RigidPose3* first, const BGC_FP32_RigidPose3* second) { - bgc_fp32_dual_quaternion_multiply_by_dual_quaternion(&combination->_versor, &second->_versor, &first->_versor); + 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); const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(&combination->_real_part); if (!bgc_fp32_is_square_unit(square_magnitude)) { _bgc_fp32_rigid_pose3_normalize(combination, square_magnitude); } + + const float dot_product = bgc_fp32_quaternion_get_dot_product(&combination->_real_part, &combination->_dual_part); + + bgc_fp32_quaternion_subtract_scaled(&combination->_dual_part, &combination->_dual_part, &combination->_real_part, dot_product); } inline void bgc_fp64_rigid_pose3_combine(BGC_FP64_RigidPose3* combination, const BGC_FP64_RigidPose3* first, const BGC_FP64_RigidPose3* second) { - bgc_fp64_dual_quaternion_multiply_by_dual_quaternion(&combination->_versor, &second->_versor, &first->_versor); + 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); const double square_magnitude = bgc_fp64_quaternion_get_square_magnitude(&combination->_real_part); if (!bgc_fp64_is_square_unit(square_magnitude)) { _bgc_fp64_rigid_pose3_normalize(combination, square_magnitude); } + + const double dot_product = bgc_fp64_quaternion_get_dot_product(&combination->_real_part, &combination->_dual_part); + + bgc_fp64_quaternion_subtract_scaled(&combination->_dual_part, &combination->_dual_part, &combination->_real_part, dot_product); } #endif