Переименование Posture в Rigit Pose, изменение внутренней организации Rigid Pose

2026-03-12 20:50:21 +07:00 · 2026-03-12 20:50:21 +07:00 · 614daf8ebd
commit 614daf8ebd
parent ce991f0488
6 changed files with 174 additions and 159 deletions
--- a/basic-geometry/rigid-pose3.h
+++ b/basic-geometry/rigid-pose3.h
@ -0,0 +1,123 @@
+#ifndef _BGC_RIGID_POSE3_H_INCLUDED_
+#define _BGC_RIGID_POSE3_H_INCLUDED_
+
+#include "types.h"
+#include "quaternion.h"
+
+// ================= Normalize ================== //
+
+void _bgc_fp32_rigid_pose3_normalize(BGC_FP32_RigidPose3* pose, const float square_magnitude);
+
+void _bgc_fp64_rigid_pose3_normalize(BGC_FP64_RigidPose3* pose, const double square_magnitude);
+
+// ==================== 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->_dual_part.s0 = 0.0f;
+    pose->_dual_part.x1 = 0.0f;
+    pose->_dual_part.x2 = 0.0f;
+    pose->_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->_dual_part.s0 = 0.0;
+    pose->_dual_part.x1 = 0.0;
+    pose->_dual_part.x2 = 0.0;
+    pose->_dual_part.x3 = 0.0;
+}
+
+// ==================== 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);
+}
+
+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);
+}
+
+// ==================== 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);
+}
+
+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);
+}
+
+// ================== 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);
+
+    const double square_magnitude = bgc_fp64_quaternion_get_square_magnitude(&destination->_real_part);
+
+    if (!bgc_fp64_is_square_unit(square_magnitude)) {
+        _bgc_fp64_rigid_pose3_normalize(destination, square_magnitude);
+    }
+
+    const double square_magnitude = bgc_fp64_quaternion_get_(&destination->_real_part);
+
+    bgc_fp64_quaternion_subtract_scaled(&destination->_dual_part, &destination->_dual_part, &destination->_real_part);
+}
+
+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);
+
+    const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(&destination->_real_part);
+
+    if (!bgc_fp32_is_square_unit(square_magnitude)) {
+        _bgc_fp32_rigid_pose3_normalize(destination, square_magnitude);
+    }
+}
+
+// ================== 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);
+
+    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);
+    }
+}
+
+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);
+
+    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);
+    }
+}
+
+#endif