Kimelas/bgc-c
2
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Получение обратного значения для rigid pose3, а также вычитание позиций (rigid pose3)

Browse source
This commit is contained in:
Andrey Pokidov 2026-03-19 19:40:41 +07:00
parent a57f13f4a9
commit b2af8408ce
2 changed files with 130 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

106
basic-geometry/rigid-pose3.h
View file

@ -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