From b2af8408ce8d80b18624a5e3066a4c14a07aae7e Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Thu, 19 Mar 2026 19:40:41 +0700 Subject: [PATCH] =?UTF-8?q?=D0=9F=D0=BE=D0=BB=D1=83=D1=87=D0=B5=D0=BD?= =?UTF-8?q?=D0=B8=D0=B5=20=D0=BE=D0=B1=D1=80=D0=B0=D1=82=D0=BD=D0=BE=D0=B3?= =?UTF-8?q?=D0=BE=20=D0=B7=D0=BD=D0=B0=D1=87=D0=B5=D0=BD=D0=B8=D1=8F=20?= =?UTF-8?q?=D0=B4=D0=BB=D1=8F=20rigid=20pose3,=20=D0=B0=20=D1=82=D0=B0?= =?UTF-8?q?=D0=BA=D0=B6=D0=B5=20=D0=B2=D1=8B=D1=87=D0=B8=D1=82=D0=B0=D0=BD?= =?UTF-8?q?=D0=B8=D0=B5=20=D0=BF=D0=BE=D0=B7=D0=B8=D1=86=D0=B8=D0=B9=20(ri?= =?UTF-8?q?gid=20pose3)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- basic-geometry/rigid-pose3.c | 24 ++++++++ basic-geometry/rigid-pose3.h | 106 +++++++++++++++++++++++++++++++++++ 2 files changed, 130 insertions(+) diff --git a/basic-geometry/rigid-pose3.c b/basic-geometry/rigid-pose3.c index 72cb97b..ab60b7f 100644 --- a/basic-geometry/rigid-pose3.c +++ b/basic-geometry/rigid-pose3.c @@ -3,6 +3,21 @@ extern inline void bgc_fp32_rigid_pose3_reset(BGC_FP32_RigidPose3* pose); extern inline void bgc_fp64_rigid_pose3_reset(BGC_FP64_RigidPose3* pose); +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_get_dual_quaternion(BGC_FP32_DualQuaternion* quaternion, const BGC_FP32_RigidPose3* pose); +extern inline void bgc_fp64_rigid_pose3_get_dual_quaternion(BGC_FP64_DualQuaternion* quaternion, const BGC_FP64_RigidPose3* pose); + +extern inline void bgc_fp32_rigid_pose3_set_dual_quaternion(BGC_FP32_RigidPose3* pose, const BGC_FP32_DualQuaternion* quaternion); +extern inline void bgc_fp64_rigid_pose3_set_dual_quaternion(BGC_FP64_RigidPose3* pose, const BGC_FP64_DualQuaternion* quaternion); + +extern inline void bgc_fp32_rigid_pose3_get_real_part(BGC_FP32_Quaternion* quaternion, const BGC_FP32_RigidPose3* pose); +extern inline void bgc_fp64_rigid_pose3_get_real_part(BGC_FP64_Quaternion* quaternion, const BGC_FP64_RigidPose3* pose); + +extern inline void bgc_fp32_rigid_pose3_get_dual_part(BGC_FP32_Quaternion* quaternion, const BGC_FP32_RigidPose3* pose); +extern inline void bgc_fp64_rigid_pose3_get_dual_part(BGC_FP64_Quaternion* quaternion, const BGC_FP64_RigidPose3* pose); + extern inline void bgc_fp32_rigid_pose3_copy(BGC_FP32_RigidPose3* destination, const BGC_FP32_RigidPose3* source); extern inline void bgc_fp64_rigid_pose3_copy(BGC_FP64_RigidPose3* destination, const BGC_FP64_RigidPose3* source); @@ -12,5 +27,14 @@ extern inline void bgc_fp64_rigid_pose3_swap(BGC_FP64_RigidPose3* pose1, BGC_FP6 extern inline void bgc_fp32_rigid_pose3_convert_to_fp64(BGC_FP64_RigidPose3* destination, const BGC_FP32_RigidPose3* source); extern inline void bgc_fp64_rigid_pose3_convert_to_fp32(BGC_FP32_RigidPose3* destination, const BGC_FP64_RigidPose3* source); +extern inline void bgc_fp32_rigid_pose3_revert(BGC_FP32_RigidPose3* pose); +extern inline void bgc_fp64_rigid_pose3_revert(BGC_FP64_RigidPose3* pose); + +extern inline void bgc_fp32_rigid_pose3_get_reverse(BGC_FP32_RigidPose3* reverse, const BGC_FP32_RigidPose3* pose); +extern inline void bgc_fp64_rigid_pose3_get_reverse(BGC_FP64_RigidPose3* reverse, const BGC_FP64_RigidPose3* pose); + extern inline void bgc_fp32_rigid_pose3_combine(BGC_FP32_RigidPose3* combination, const BGC_FP32_RigidPose3* first, const BGC_FP32_RigidPose3* second); extern inline void bgc_fp64_rigid_pose3_combine(BGC_FP64_RigidPose3* combination, const BGC_FP64_RigidPose3* first, const BGC_FP64_RigidPose3* second); + +extern inline void bgc_fp32_rigid_pose3_exclude(BGC_FP32_RigidPose3* difference, const BGC_FP32_RigidPose3* base, const BGC_FP32_RigidPose3* excludant); +extern inline void bgc_fp64_rigid_pose3_exclude(BGC_FP64_RigidPose3* difference, const BGC_FP64_RigidPose3* base, const BGC_FP64_RigidPose3* excludant); diff --git a/basic-geometry/rigid-pose3.h b/basic-geometry/rigid-pose3.h index 1c1c8dc..c5adea4 100644 --- a/basic-geometry/rigid-pose3.h +++ b/basic-geometry/rigid-pose3.h @@ -77,6 +77,56 @@ inline void _bgc_fp64_rigid_pose3_normalize(BGC_FP64_RigidPose3* pose) bgc_fp64_quaternion_subtract_scaled(&pose->_versor.dual_part, &pose->_versor.dual_part, &pose->_versor.real_part, dot_product); } +// ============ Get Dual Quaternion ============= // + +inline void bgc_fp32_rigid_pose3_get_dual_quaternion(BGC_FP32_DualQuaternion* quaternion, const BGC_FP32_RigidPose3* pose) +{ + bgc_fp32_dual_quaternion_copy(quaternion, &pose->_versor); +} + +inline void bgc_fp64_rigid_pose3_get_dual_quaternion(BGC_FP64_DualQuaternion* quaternion, const BGC_FP64_RigidPose3* pose) +{ + bgc_fp64_dual_quaternion_copy(quaternion, &pose->_versor); +} + +// ============ Get Dual Quaternion ============= // + +inline void bgc_fp32_rigid_pose3_set_dual_quaternion(BGC_FP32_RigidPose3* pose, const BGC_FP32_DualQuaternion* quaternion) +{ + bgc_fp32_dual_quaternion_copy(&pose->_versor, quaternion); + _bgc_fp32_rigid_pose3_normalize(destination); +} + +inline void bgc_fp64_rigid_pose3_set_dual_quaternion(BGC_FP64_RigidPose3* pose, const BGC_FP64_DualQuaternion* quaternion) +{ + bgc_fp64_dual_quaternion_copy(&pose->_versor, quaternion); + _bgc_fp64_rigid_pose3_normalize(destination); +} + +// =============== Get Real Part ================ // + +inline void bgc_fp32_rigid_pose3_get_real_part(BGC_FP32_Quaternion* quaternion, const BGC_FP32_RigidPose3* pose) +{ + bgc_fp32_quaternion_copy(quaternion, &pose->_versor.real_part); +} + +inline void bgc_fp64_rigid_pose3_get_real_part(BGC_FP64_Quaternion* quaternion, const BGC_FP64_RigidPose3* pose) +{ + bgc_fp64_quaternion_copy(quaternion, &pose->_versor.real_part); +} + +// =============== Get Dual Part ================ // + +inline void bgc_fp32_rigid_pose3_get_dual_part(BGC_FP32_Quaternion* quaternion, const BGC_FP32_RigidPose3* pose) +{ + bgc_fp32_quaternion_copy(quaternion, &pose->_versor.dual_part); +} + +inline void bgc_fp64_rigid_pose3_get_dual_part(BGC_FP64_Quaternion* quaternion, const BGC_FP64_RigidPose3* pose) +{ + bgc_fp64_quaternion_copy(quaternion, &pose->_versor.dual_part); +} + // ==================== Copy ==================== // inline void bgc_fp32_rigid_pose3_copy(BGC_FP32_RigidPose3* destination, const BGC_FP32_RigidPose3* source) @@ -115,6 +165,34 @@ inline void bgc_fp64_rigid_pose3_convert_to_fp32(BGC_FP32_RigidPose3* destinatio _bgc_fp32_rigid_pose3_normalize(destination); } +// =================== Revert =================== // + +inline void bgc_fp32_rigid_pose3_revert(BGC_FP32_RigidPose3* pose) +{ + bgc_fp32_quaternion_conjugate(&pose->_versor.real_part); + bgc_fp32_quaternion_conjugate(&pose->_versor.dual_part); +} + +inline void bgc_fp64_rigid_pose3_revert(BGC_FP64_RigidPose3* pose) +{ + bgc_fp64_quaternion_conjugate(&pose->_versor.real_part); + bgc_fp64_quaternion_conjugate(&pose->_versor.dual_part); +} + +// ================ Get Reverse ================= // + +inline void bgc_fp32_rigid_pose3_get_reverse(BGC_FP32_RigidPose3* reverse, const BGC_FP32_RigidPose3* pose) +{ + bgc_fp32_quaternion_get_conjugate(&reverse->_versor.real_part, &pose->_versor.real_part); + bgc_fp32_quaternion_get_conjugate(&reverse->_versor.dual_part, &pose->_versor.dual_part); +} + +inline void bgc_fp64_rigid_pose3_get_reverse(BGC_FP64_RigidPose3* reverse, const BGC_FP64_RigidPose3* pose) +{ + bgc_fp64_quaternion_get_conjugate(&reverse->_versor.real_part, &pose->_versor.real_part); + bgc_fp64_quaternion_get_conjugate(&reverse->_versor.dual_part, &pose->_versor.dual_part); +} + // ================== Combine =================== // inline void bgc_fp32_rigid_pose3_combine(BGC_FP32_RigidPose3* combination, const BGC_FP32_RigidPose3* first, const BGC_FP32_RigidPose3* second) @@ -129,4 +207,32 @@ inline void bgc_fp64_rigid_pose3_combine(BGC_FP64_RigidPose3* combination, const _bgc_fp64_rigid_pose3_normalize(combination); } +// ================== Exclude =================== // + +inline void bgc_fp32_rigid_pose3_exclude(BGC_FP32_RigidPose3* difference, const BGC_FP32_RigidPose3* base, const BGC_FP32_RigidPose3* excludant) +{ + BGC_FP32_Quaternion dual_part1, dual_part2; + + _bgc_fp32_restrict_quaternion_multiply_by_conjugate(&dual_part1, &base->_versor.real_part, &excludant->_versor.dual_part); + _bgc_fp32_restrict_quaternion_multiply_by_conjugate(&dual_part2, &base->_versor.dual_part, &excludant->_versor.real_part); + + bgc_fp32_quaternion_multiply_by_conjugate(&difference->_versor.real_part, &base->_versor.real_part, &excludant->_versor.real_part); + bgc_fp32_quaternion_add(&difference->_versor.dual_part, &dual_part1, &dual_part2); + + _bgc_fp32_rigid_pose3_normalize(difference); +} + +inline void bgc_fp64_rigid_pose3_exclude(BGC_FP64_RigidPose3* difference, const BGC_FP64_RigidPose3* base, const BGC_FP64_RigidPose3* excludant) +{ + BGC_FP64_Quaternion dual_part1, dual_part2; + + _bgc_fp64_restrict_quaternion_multiply_by_conjugate(&dual_part1, &base->_versor.real_part, &excludant->_versor.dual_part); + _bgc_fp64_restrict_quaternion_multiply_by_conjugate(&dual_part2, &base->_versor.dual_part, &excludant->_versor.real_part); + + bgc_fp64_quaternion_multiply_by_conjugate(&difference->_versor.real_part, &base->_versor.real_part, &excludant->_versor.real_part); + bgc_fp64_quaternion_add(&difference->_versor.dual_part, &dual_part1, &dual_part2); + + _bgc_fp64_rigid_pose3_normalize(difference); +} + #endif