From 405af2f3a751d776138b35ebd83e84851cfd2f51 Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Thu, 26 Mar 2026 01:49:18 +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=B8=D0=B5=20=D0=BE=D0=BF=D0=B5=D1=80=D0=B0=D1=86=D0=B8?= =?UTF-8?q?=D0=B9=20=D0=BF=D1=80=D0=BE=D0=B2=D0=B5=D1=80=D0=BA=D0=B8=20?= =?UTF-8?q?=D1=81=D0=BE=D1=81=D1=82=D0=BE=D1=8F=D0=BD=D0=B8=D0=B9=20=D0=BA?= =?UTF-8?q?=D0=B2=D0=B0=D1=82=D0=B5=D1=80=D0=BD=D0=B8=D0=BE=D0=BD=D0=BE?= =?UTF-8?q?=D0=B2=20=D0=B8=20=D0=B4=D1=83=D0=B0=D0=BB=D1=8C=D0=BD=D1=8B?= =?UTF-8?q?=D1=85=20=D0=BA=D0=B2=D0=B0=D1=82=D0=B5=D1=80=D0=BD=D0=B8=D0=BE?= =?UTF-8?q?=D0=BD=D0=BE=D0=B2,=20=D0=B0=20=D1=82=D0=B0=D0=BA=D0=B6=D0=B5?= =?UTF-8?q?=20=D0=BE=D0=BF=D0=B5=D1=80=D0=B0=D1=86=D0=B8=D0=B9=20=D1=82?= =?UTF-8?q?=D1=80=D0=B0=D0=BD=D1=81=D1=84=D0=BE=D1=80=D0=BC=D0=B0=D1=86?= =?UTF-8?q?=D0=B8=D0=B8=20=D0=B2=D0=B5=D0=BA=D1=82=D0=BE=D1=80=D0=B0=20?= =?UTF-8?q?=D1=81=20=D0=BF=D0=BE=D0=BC=D0=BE=D1=89=D1=8C=D1=8E=20rigid=20p?= =?UTF-8?q?ose?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- basic-geometry/position3.h | 2 - basic-geometry/quaternion.c | 6 +++ basic-geometry/quaternion.h | 24 +++++++++ basic-geometry/rigid-pose3.c | 9 ++++ basic-geometry/rigid-pose3.h | 36 ++++++++++++++ basic-geometry/turn2.c | 3 ++ basic-geometry/turn2.h | 96 ++++++++++++++++++++++++++---------- basic-geometry/turn3.h | 8 +-- 8 files changed, 152 insertions(+), 32 deletions(-) diff --git a/basic-geometry/position3.h b/basic-geometry/position3.h index 3876249..0d011a8 100644 --- a/basic-geometry/position3.h +++ b/basic-geometry/position3.h @@ -209,7 +209,6 @@ inline void bgc_fp32_position3_transform_point_back(BGC_FP32_Vector3* const tran BGC_FP32_Vector3 relative_point; bgc_fp32_vector3_subtract(&relative_point, initial_point, &position->shift); bgc_fp32_turn3_vector_back(transformed_point, &position->turn, &relative_point); - } inline void bgc_fp64_position3_transform_point_back(BGC_FP64_Vector3* const transformed_point, const BGC_FP64_Position3* const position, const BGC_FP64_Vector3* const initial_point) @@ -217,7 +216,6 @@ inline void bgc_fp64_position3_transform_point_back(BGC_FP64_Vector3* const tran BGC_FP64_Vector3 relative_point; bgc_fp64_vector3_subtract(&relative_point, initial_point, &position->shift); bgc_fp64_turn3_vector_back(transformed_point, &position->turn, &relative_point); - } // ============== Transform Vector =============== // diff --git a/basic-geometry/quaternion.c b/basic-geometry/quaternion.c index 858e141..9ff7eff 100644 --- a/basic-geometry/quaternion.c +++ b/basic-geometry/quaternion.c @@ -22,6 +22,12 @@ extern inline int bgc_fp64_quaternion_is_zero(const BGC_FP64_Quaternion* const q extern inline int bgc_fp32_quaternion_is_unit(const BGC_FP32_Quaternion* const quaternion); extern inline int bgc_fp64_quaternion_is_unit(const BGC_FP64_Quaternion* const quaternion); +extern inline int bgc_fp32_quaternion_is_pure(const BGC_FP32_Quaternion* const quaternion); +extern inline int bgc_fp64_quaternion_is_pure(const BGC_FP64_Quaternion* const quaternion); + +extern inline int bgc_fp32_quaternion_is_real(const BGC_FP32_Quaternion* const quaternion); +extern inline int bgc_fp64_quaternion_is_real(const BGC_FP64_Quaternion* const quaternion); + extern inline void bgc_fp32_quaternion_copy(BGC_FP32_Quaternion* const destination, const BGC_FP32_Quaternion* const source); extern inline void bgc_fp64_quaternion_copy(BGC_FP64_Quaternion* const destination, const BGC_FP64_Quaternion* const source); diff --git a/basic-geometry/quaternion.h b/basic-geometry/quaternion.h index 8527f51..ba229f2 100644 --- a/basic-geometry/quaternion.h +++ b/basic-geometry/quaternion.h @@ -110,6 +110,30 @@ inline int bgc_fp64_quaternion_is_unit(const BGC_FP64_Quaternion* const quaterni return bgc_fp64_is_square_unit(bgc_fp64_quaternion_get_square_magnitude(quaternion)); } +// ================== Is Pure =================== // + +inline int bgc_fp32_quaternion_is_pure(const BGC_FP32_Quaternion* const quaternion) +{ + return bgc_fp32_is_zero(quaternion->s0); +} + +inline int bgc_fp64_quaternion_is_pure(const BGC_FP64_Quaternion* const quaternion) +{ + return bgc_fp64_is_zero(quaternion->s0); +} + +// ================== Is Real =================== // + +inline int bgc_fp32_quaternion_is_real(const BGC_FP32_Quaternion* const quaternion) +{ + return quaternion->x1 * quaternion->x1 + quaternion->x2 * quaternion->x2 + quaternion->x3 * quaternion->x3 <= BGC_FP32_SQUARE_EPSILON; +} + +inline int bgc_fp64_quaternion_is_real(const BGC_FP64_Quaternion* const quaternion) +{ + return quaternion->x1 * quaternion->x1 + quaternion->x2 * quaternion->x2 + quaternion->x3 * quaternion->x3 <= BGC_FP64_SQUARE_EPSILON; +} + // ==================== Copy ==================== // inline void bgc_fp32_quaternion_copy(BGC_FP32_Quaternion* const destination, const BGC_FP32_Quaternion* const source) diff --git a/basic-geometry/rigid-pose3.c b/basic-geometry/rigid-pose3.c index d55c28b..1ec9dbd 100644 --- a/basic-geometry/rigid-pose3.c +++ b/basic-geometry/rigid-pose3.c @@ -24,6 +24,9 @@ extern inline void bgc_fp64_rigid_pose3_copy(BGC_FP64_RigidPose3* const destinat extern inline void bgc_fp32_rigid_pose3_swap(BGC_FP32_RigidPose3* const pose1, BGC_FP32_RigidPose3* const pose2); extern inline void bgc_fp64_rigid_pose3_swap(BGC_FP64_RigidPose3* const pose1, BGC_FP64_RigidPose3* const pose2); +extern inline int bgc_fp32_rigid_pose3_is_idle(const BGC_FP32_RigidPose3* const pose); +extern inline int bgc_fp64_rigid_pose3_is_idle(const BGC_FP64_RigidPose3* const pose); + extern inline void bgc_fp32_rigid_pose3_convert_to_fp64(BGC_FP64_RigidPose3* const destination, const BGC_FP32_RigidPose3* const source); extern inline void bgc_fp64_rigid_pose3_convert_to_fp32(BGC_FP32_RigidPose3* const destination, const BGC_FP64_RigidPose3* const source); @@ -80,3 +83,9 @@ extern inline void bgc_fp64_rigid_pose3_get_reverse_position(BGC_FP64_Position3* extern inline void bgc_fp32_rigid_pose3_set_reverse_position(BGC_FP32_RigidPose3* const pose, const BGC_FP32_Position3* const position); extern inline void bgc_fp64_rigid_pose3_set_reverse_position(BGC_FP64_RigidPose3* const pose, const BGC_FP64_Position3* const position); + +extern inline void bgc_fp32_rigid_pose3_transform_vector(BGC_FP32_Vector3* const turned_vector, const BGC_FP32_RigidPose3* const pose, const BGC_FP32_Vector3* const original_vector); +extern inline void bgc_fp64_rigid_pose3_transform_vector(BGC_FP64_Vector3* const turned_vector, const BGC_FP64_RigidPose3* const pose, const BGC_FP64_Vector3* const original_vector); + +extern inline void bgc_fp32_rigid_pose3_transform_vector_back(BGC_FP32_Vector3* const turned_vector, const BGC_FP32_RigidPose3* const pose, const BGC_FP32_Vector3* const original_vector); +extern inline void bgc_fp64_rigid_pose3_transform_vector_back(BGC_FP64_Vector3* const turned_vector, const BGC_FP64_RigidPose3* const pose, const BGC_FP64_Vector3* const original_vector); diff --git a/basic-geometry/rigid-pose3.h b/basic-geometry/rigid-pose3.h index 5d1e6a9..6aaa3d6 100644 --- a/basic-geometry/rigid-pose3.h +++ b/basic-geometry/rigid-pose3.h @@ -152,6 +152,18 @@ inline void bgc_fp64_rigid_pose3_swap(BGC_FP64_RigidPose3* const pose1, BGC_FP64 bgc_fp64_dual_quaternion_swap(&pose1->_versor, &pose2->_versor); } +// ================== Is Idle =================== // + +inline int bgc_fp32_rigid_pose3_is_idle(const BGC_FP32_RigidPose3* const pose) +{ + return bgc_fp32_quaternion_is_real(&pose->_versor.real_part) && bgc_fp32_quaternion_is_zero(&pose->_versor.dual_part); +} + +inline int bgc_fp64_rigid_pose3_is_idle(const BGC_FP64_RigidPose3* const pose) +{ + return bgc_fp64_quaternion_is_real(&pose->_versor.real_part) && bgc_fp64_quaternion_is_zero(&pose->_versor.dual_part); +} + // ================== Convert =================== // inline void bgc_fp32_rigid_pose3_convert_to_fp64(BGC_FP64_RigidPose3* const destination, const BGC_FP32_RigidPose3* const source) @@ -488,4 +500,28 @@ inline void bgc_fp64_rigid_pose3_set_reverse_position(BGC_FP64_RigidPose3* const pose->_versor.dual_part.x3 = -0.5f * (versor->s0 * shift->x3 + versor->x2 * shift->x1 - versor->x1 * shift->x2); } +// ============== Transform Vector ============== // + +inline void bgc_fp32_rigid_pose3_transform_vector(BGC_FP32_Vector3* const turned_vector, const BGC_FP32_RigidPose3* const pose, const BGC_FP32_Vector3* const original_vector) +{ + _bgc_fp32_versor_turn_vector(turned_vector, &pose->_versor.real_part, original_vector); +} + +inline void bgc_fp64_rigid_pose3_transform_vector(BGC_FP64_Vector3* const turned_vector, const BGC_FP64_RigidPose3* const pose, const BGC_FP64_Vector3* const original_vector) +{ + _bgc_fp64_versor_turn_vector(turned_vector, &pose->_versor.real_part, original_vector); +} + +// =========== Transform Vector Back ============ // + +inline void bgc_fp32_rigid_pose3_transform_vector_back(BGC_FP32_Vector3* const turned_vector, const BGC_FP32_RigidPose3* const pose, const BGC_FP32_Vector3* const original_vector) +{ + _bgc_fp32_versor_turn_vector_back(turned_vector, &pose->_versor.real_part, original_vector); +} + +inline void bgc_fp64_rigid_pose3_transform_vector_back(BGC_FP64_Vector3* const turned_vector, const BGC_FP64_RigidPose3* const pose, const BGC_FP64_Vector3* const original_vector) +{ + _bgc_fp64_versor_turn_vector_back(turned_vector, &pose->_versor.real_part, original_vector); +} + #endif diff --git a/basic-geometry/turn2.c b/basic-geometry/turn2.c index 98a018b..1f63d49 100644 --- a/basic-geometry/turn2.c +++ b/basic-geometry/turn2.c @@ -43,6 +43,9 @@ extern inline void bgc_fp64_turn2_get_power(BGC_FP64_Turn2* const power, const B extern inline void bgc_fp32_turn2_combine(BGC_FP32_Turn2* const combination, const BGC_FP32_Turn2* const turn1, const BGC_FP32_Turn2* const turn2); extern inline void bgc_fp64_turn2_combine(BGC_FP64_Turn2* const combination, const BGC_FP64_Turn2* const turn1, const BGC_FP64_Turn2* const turn2); +extern inline void bgc_fp32_turn2_combine3(BGC_FP32_Turn2* const combination, const BGC_FP32_Turn2* const turn1, const BGC_FP32_Turn2* const turn2, const BGC_FP32_Turn2* const turn3); +extern inline void bgc_fp64_turn2_combine3(BGC_FP64_Turn2* const combination, const BGC_FP64_Turn2* const turn1, const BGC_FP64_Turn2* const turn2, const BGC_FP64_Turn2* const turn3); + extern inline void bgc_fp32_turn2_exclude(BGC_FP32_Turn2* const difference, const BGC_FP32_Turn2* const base, const BGC_FP32_Turn2* const excludant); extern inline void bgc_fp64_turn2_exclude(BGC_FP64_Turn2* const difference, const BGC_FP64_Turn2* const base, const BGC_FP64_Turn2* const excludant); diff --git a/basic-geometry/turn2.h b/basic-geometry/turn2.h index 26700a4..822759d 100644 --- a/basic-geometry/turn2.h +++ b/basic-geometry/turn2.h @@ -171,14 +171,20 @@ inline void bgc_fp64_turn2_swap(BGC_FP64_Turn2* const turn1, BGC_FP64_Turn2* con // ================== Convert =================== // -inline void bgc_fp64_turn2_convert_to_fp32(BGC_FP32_Turn2* const destination, const BGC_FP64_Turn2* const source) -{ - bgc_fp32_turn2_set_values(destination, (float)source->_cos, (float)source->_sin); -} - inline void bgc_fp32_turn2_convert_to_fp64(BGC_FP64_Turn2* const destination, const BGC_FP32_Turn2* const source) { - bgc_fp64_turn2_set_values(destination, (double)source->_cos, (double)source->_sin); + destination->_cos = source->_cos; + destination->_sin = source->_sin; + + _bgc_fp64_turn2_normalize(destination); +} + +inline void bgc_fp64_turn2_convert_to_fp32(BGC_FP32_Turn2* const destination, const BGC_FP64_Turn2* const source) +{ + destination->_cos = (float)source->_cos; + destination->_sin = (float)source->_sin; + + _bgc_fp32_turn2_normalize(destination); } // =================== Revert =================== // @@ -227,40 +233,78 @@ inline void bgc_fp64_turn2_get_power(BGC_FP64_Turn2* const power, const BGC_FP64 inline void bgc_fp32_turn2_combine(BGC_FP32_Turn2* const combination, const BGC_FP32_Turn2* const turn1, const BGC_FP32_Turn2* const turn2) { - bgc_fp32_turn2_set_values( - combination, - turn1->_cos * turn2->_cos - turn1->_sin * turn2->_sin, - turn1->_cos * turn2->_sin + turn1->_sin * turn2->_cos - ); + const float _cos = turn1->_cos * turn2->_cos - turn1->_sin * turn2->_sin; + const float _sin = turn1->_cos * turn2->_sin + turn1->_sin * turn2->_cos; + + combination->_cos = _cos; + combination->_sin = _sin; + + _bgc_fp32_turn2_normalize(combination); } inline void bgc_fp64_turn2_combine(BGC_FP64_Turn2* const combination, const BGC_FP64_Turn2* const turn1, const BGC_FP64_Turn2* const turn2) { - bgc_fp64_turn2_set_values( - combination, - turn1->_cos * turn2->_cos - turn1->_sin * turn2->_sin, - turn1->_cos * turn2->_sin + turn1->_sin * turn2->_cos - ); + const double _cos = turn1->_cos * turn2->_cos - turn1->_sin * turn2->_sin; + const double _sin = turn1->_cos * turn2->_sin + turn1->_sin * turn2->_cos; + + combination->_cos = _cos; + combination->_sin = _sin; + + _bgc_fp64_turn2_normalize(combination); +} + +// ============ Combination of three ============ // + +inline void bgc_fp32_turn2_combine3(BGC_FP32_Turn2* const combination, const BGC_FP32_Turn2* const turn1, const BGC_FP32_Turn2* const turn2, const BGC_FP32_Turn2* const turn3) +{ + const float _cos_of_two = turn1->_cos * turn2->_cos - turn1->_sin * turn2->_sin; + const float _sin_of_two = turn1->_cos * turn2->_sin + turn1->_sin * turn2->_cos; + + const float _cos_of_three = _cos_of_two * turn3->_cos - _sin_of_two * turn3->_sin; + const float _sin_of_three = _cos_of_two * turn3->_sin + _sin_of_two * turn3->_cos; + + combination->_cos = _cos_of_three; + combination->_sin = _sin_of_three; + + _bgc_fp32_turn2_normalize(combination); +} + +inline void bgc_fp64_turn2_combine3(BGC_FP64_Turn2* const combination, const BGC_FP64_Turn2* const turn1, const BGC_FP64_Turn2* const turn2, const BGC_FP64_Turn2* const turn3) +{ + const double _cos_of_two = turn1->_cos * turn2->_cos - turn1->_sin * turn2->_sin; + const double _sin_of_two = turn1->_cos * turn2->_sin + turn1->_sin * turn2->_cos; + + const double _cos_of_three = _cos_of_two * turn3->_cos - _sin_of_two * turn3->_sin; + const double _sin_of_three = _cos_of_two * turn3->_sin + _sin_of_two * turn3->_cos; + + combination->_cos = _cos_of_three; + combination->_sin = _sin_of_three; + + _bgc_fp64_turn2_normalize(combination); } // ================= Exclusion ================== // inline void bgc_fp32_turn2_exclude(BGC_FP32_Turn2* const difference, const BGC_FP32_Turn2* const base, const BGC_FP32_Turn2* const excludant) { - bgc_fp32_turn2_set_values( - difference, - base->_cos * excludant->_cos + base->_sin * excludant->_sin, - base->_sin * excludant->_cos - base->_cos * excludant->_sin - ); + const float _cos = base->_cos * excludant->_cos + base->_sin * excludant->_sin; + const float _sin = base->_sin * excludant->_cos - base->_cos * excludant->_sin; + + difference->_cos = _cos; + difference->_sin = _sin; + + _bgc_fp32_turn2_normalize(difference); } inline void bgc_fp64_turn2_exclude(BGC_FP64_Turn2* const difference, const BGC_FP64_Turn2* const base, const BGC_FP64_Turn2* const excludant) { - bgc_fp64_turn2_set_values( - difference, - base->_cos * excludant->_cos + base->_sin * excludant->_sin, - base->_sin * excludant->_cos - base->_cos * excludant->_sin - ); + const double _cos = base->_cos * excludant->_cos + base->_sin * excludant->_sin; + const double _sin = base->_sin * excludant->_cos - base->_cos * excludant->_sin; + + difference->_cos = _cos; + difference->_sin = _sin; + + _bgc_fp64_turn2_normalize(difference); } // ============== Rotation Matrix =============== // diff --git a/basic-geometry/turn3.h b/basic-geometry/turn3.h index b07649c..4fffe15 100644 --- a/basic-geometry/turn3.h +++ b/basic-geometry/turn3.h @@ -204,12 +204,12 @@ inline void bgc_fp64_turn3_swap(BGC_FP64_Turn3* const turn1, BGC_FP64_Turn3* con inline int bgc_fp32_turn3_is_idle(const BGC_FP32_Turn3* const turn) { - return turn->_versor.x1 * turn->_versor.x1 + turn->_versor.x2 * turn->_versor.x2 + turn->_versor.x3 * turn->_versor.x3 <= BGC_FP32_SQUARE_EPSILON; + return bgc_fp32_quaternion_is_real(&turn->_versor); } inline int bgc_fp64_turn3_is_idle(const BGC_FP64_Turn3* const turn) { - return turn->_versor.x1 * turn->_versor.x1 + turn->_versor.x2 * turn->_versor.x2 + turn->_versor.x3 * turn->_versor.x3 <= BGC_FP64_SQUARE_EPSILON; + return bgc_fp64_quaternion_is_real(&turn->_versor); } // ================== Convert =================== // @@ -333,7 +333,7 @@ inline void bgc_fp32_turn3_combine3(BGC_FP32_Turn3* const combination, const BGC { BGC_FP32_Quaternion product; - bgc_fp32_quaternion_multiply_by_quaternion(&product, &second->_versor, &first->_versor); + _bgc_fp32_restrict_quaternion_multiply_by_quaternion(&product, &second->_versor, &first->_versor); bgc_fp32_quaternion_multiply_by_quaternion(&combination->_versor, &third->_versor, &product); @@ -344,7 +344,7 @@ inline void bgc_fp64_turn3_combine3(BGC_FP64_Turn3* const combination, const BGC { BGC_FP64_Quaternion product; - bgc_fp64_quaternion_multiply_by_quaternion(&product, &second->_versor, &first->_versor); + _bgc_fp64_restrict_quaternion_multiply_by_quaternion(&product, &second->_versor, &first->_versor); bgc_fp64_quaternion_multiply_by_quaternion(&combination->_versor, &third->_versor, &product);