Реорганизация функций для получения матрицы повотора и матрицы обратного поворота для версоров, добавлены функции получения матриц поворота и аффинных преобразований для Rigid Pose3

basic-geometry/quaternion.c

@@ -109,11 +109,11 @@ extern inline int bgc_fp64_quaternion_normalize(BGC_FP64_Quaternion* quaternion)
 extern inline int bgc_fp32_quaternion_get_normalized(BGC_FP32_Quaternion* normalized, const BGC_FP32_Quaternion* quaternion);
 extern inline int bgc_fp64_quaternion_get_normalized(BGC_FP64_Quaternion* normalized, const BGC_FP64_Quaternion* quaternion);
 
-extern inline void _bgc_fp32_quaternion_turn_vector_roughly(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Quaternion* quaternion, const BGC_FP32_Vector3* original_vector);
+extern inline void _bgc_fp32_versor_turn_vector(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Quaternion* quaternion, const BGC_FP32_Vector3* original_vector);
-extern inline void _bgc_fp64_quaternion_turn_vector_roughly(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Quaternion* quaternion, const BGC_FP64_Vector3* original_vector);
+extern inline void _bgc_fp64_versor_turn_vector(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Quaternion* quaternion, const BGC_FP64_Vector3* original_vector);
 
-extern inline void _bgc_fp32_quaternion_turn_vector_back_roughly(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Quaternion* quaternion, const BGC_FP32_Vector3* original_vector);
+extern inline void _bgc_fp32_versor_turn_vector_back(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Quaternion* quaternion, const BGC_FP32_Vector3* original_vector);
-extern inline void _bgc_fp64_quaternion_turn_vector_back_roughly(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Quaternion* quaternion, const BGC_FP64_Vector3* original_vector);
+extern inline void _bgc_fp64_versor_turn_vector_back(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Quaternion* quaternion, const BGC_FP64_Vector3* original_vector);
 
 extern inline int bgc_fp32_quaternion_turn_vector(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Quaternion* quaternion, const BGC_FP32_Vector3* original_vector);
 extern inline int bgc_fp64_quaternion_turn_vector(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Quaternion* quaternion, const BGC_FP64_Vector3* original_vector);
@@ -121,6 +121,12 @@ extern inline int bgc_fp64_quaternion_turn_vector(BGC_FP64_Vector3* turned_vecto
 extern inline int bgc_fp32_quaternion_turn_vector_back(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Quaternion* quaternion, const BGC_FP32_Vector3* original_vector);
 extern inline int bgc_fp64_quaternion_turn_vector_back(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Quaternion* quaternion, const BGC_FP64_Vector3* original_vector);
 
+extern inline void _bgc_fp32_versor_get_rotation_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_Quaternion* versor);
+extern inline void _bgc_fp64_versor_get_rotation_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_Quaternion* versor);
+
+extern inline void _bgc_fp32_versor_get_reverse_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_Quaternion* versor);
+extern inline void _bgc_fp64_versor_get_reverse_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_Quaternion* versor);
+
 extern inline int bgc_fp32_quaternion_get_rotation_matrix(BGC_FP32_Matrix3x3* rotation, const BGC_FP32_Quaternion* quaternion);
 extern inline int bgc_fp64_quaternion_get_rotation_matrix(BGC_FP64_Matrix3x3* rotation, const BGC_FP64_Quaternion* quaternion);
 

basic-geometry/quaternion.h

@@ -753,7 +753,7 @@ int bgc_fp64_quaternion_get_power(BGC_FP64_Quaternion* power, const BGC_FP64_Qua
 // ============== Raw Turn Vector3 ============== //
 
 // An internal function
-inline void _bgc_fp32_quaternion_turn_vector_roughly(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Quaternion* quaternion, const BGC_FP32_Vector3* original_vector)
+inline void _bgc_fp32_versor_turn_vector(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Quaternion* quaternion, const BGC_FP32_Vector3* original_vector)
 {
     const float tx1 = 2.0f * (quaternion->x2 * original_vector->x3 - quaternion->x3 * original_vector->x2);
     const float tx2 = 2.0f * (quaternion->x3 * original_vector->x1 - quaternion->x1 * original_vector->x3);
@@ -769,7 +769,7 @@ inline void _bgc_fp32_quaternion_turn_vector_roughly(BGC_FP32_Vector3* turned_ve
 }
 
 // An internal function
-inline void _bgc_fp64_quaternion_turn_vector_roughly(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Quaternion* quaternion, const BGC_FP64_Vector3* original_vector)
+inline void _bgc_fp64_versor_turn_vector(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Quaternion* quaternion, const BGC_FP64_Vector3* original_vector)
 {
     const double tx1 = 2.0f * (quaternion->x2 * original_vector->x3 - quaternion->x3 * original_vector->x2);
     const double tx2 = 2.0f * (quaternion->x3 * original_vector->x1 - quaternion->x1 * original_vector->x3);
@@ -787,7 +787,7 @@ inline void _bgc_fp64_quaternion_turn_vector_roughly(BGC_FP64_Vector3* turned_ve
 // ========= Raw Turn Vector3 Backwards ========= //
 
 // An internal function
-inline void _bgc_fp32_quaternion_turn_vector_back_roughly(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Quaternion* quaternion, const BGC_FP32_Vector3* original_vector)
+inline void _bgc_fp32_versor_turn_vector_back(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Quaternion* quaternion, const BGC_FP32_Vector3* original_vector)
 {
     const float tx1 = 2.0f * (quaternion->x2 * original_vector->x3 - quaternion->x3 * original_vector->x2);
     const float tx2 = 2.0f * (quaternion->x3 * original_vector->x1 - quaternion->x1 * original_vector->x3);
@@ -803,7 +803,7 @@ inline void _bgc_fp32_quaternion_turn_vector_back_roughly(BGC_FP32_Vector3* turn
 }
 
 // An internal function
-inline void _bgc_fp64_quaternion_turn_vector_back_roughly(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Quaternion* quaternion, const BGC_FP64_Vector3* original_vector)
+inline void _bgc_fp64_versor_turn_vector_back(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Quaternion* quaternion, const BGC_FP64_Vector3* original_vector)
 {
     const double tx1 = 2.0f * (quaternion->x2 * original_vector->x3 - quaternion->x3 * original_vector->x2);
     const double tx2 = 2.0f * (quaternion->x3 * original_vector->x1 - quaternion->x1 * original_vector->x3);
@@ -926,6 +926,126 @@ inline int bgc_fp64_quaternion_turn_vector_back(BGC_FP64_Vector3* turned_vector,
     return BGC_SUCCESS;
 }
 
+// ========= Get Versor Rotation Matrix ========= //
+
+inline void _bgc_fp32_versor_get_rotation_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_Quaternion* versor)
+{
+    const float s0s0 = versor->s0 * versor->s0;
+    const float x1x1 = versor->x1 * versor->x1;
+    const float x2x2 = versor->x2 * versor->x2;
+    const float x3x3 = versor->x3 * versor->x3;
+
+    const float s0x1 = versor->s0 * versor->x1;
+    const float s0x2 = versor->s0 * versor->x2;
+    const float s0x3 = versor->s0 * versor->x3;
+
+    const float x1x2 = versor->x1 * versor->x2;
+    const float x1x3 = versor->x1 * versor->x3;
+
+    const float x2x3 = versor->x2 * versor->x3;
+
+    matrix->r1c1 = ((s0s0 + x1x1) - (x2x2 + x3x3));
+    matrix->r2c2 = ((s0s0 + x2x2) - (x1x1 + x3x3));
+    matrix->r3c3 = ((s0s0 + x3x3) - (x1x1 + x2x2));
+
+    matrix->r1c2 = 2.0f * (x1x2 - s0x3);
+    matrix->r2c3 = 2.0f * (x2x3 - s0x1);
+    matrix->r3c1 = 2.0f * (x1x3 - s0x2);
+
+    matrix->r2c1 = 2.0f * (x1x2 + s0x3);
+    matrix->r3c2 = 2.0f * (x2x3 + s0x1);
+    matrix->r1c3 = 2.0f * (x1x3 + s0x2);
+}
+
+inline void _bgc_fp64_versor_get_rotation_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_Quaternion* versor)
+{
+    const double s0s0 = versor->s0 * versor->s0;
+    const double x1x1 = versor->x1 * versor->x1;
+    const double x2x2 = versor->x2 * versor->x2;
+    const double x3x3 = versor->x3 * versor->x3;
+
+    const double s0x1 = versor->s0 * versor->x1;
+    const double s0x2 = versor->s0 * versor->x2;
+    const double s0x3 = versor->s0 * versor->x3;
+
+    const double x1x2 = versor->x1 * versor->x2;
+    const double x1x3 = versor->x1 * versor->x3;
+
+    const double x2x3 = versor->x2 * versor->x3;
+
+    matrix->r1c1 = ((s0s0 + x1x1) - (x2x2 + x3x3));
+    matrix->r2c2 = ((s0s0 + x2x2) - (x1x1 + x3x3));
+    matrix->r3c3 = ((s0s0 + x3x3) - (x1x1 + x2x2));
+
+    matrix->r1c2 = 2.0 * (x1x2 - s0x3);
+    matrix->r2c3 = 2.0 * (x2x3 - s0x1);
+    matrix->r3c1 = 2.0 * (x1x3 - s0x2);
+
+    matrix->r2c1 = 2.0 * (x1x2 + s0x3);
+    matrix->r3c2 = 2.0 * (x2x3 + s0x1);
+    matrix->r1c3 = 2.0 * (x1x3 + s0x2);
+}
+
+// ========= Get Versor Reverse Matrix ========== //
+
+inline void _bgc_fp32_versor_get_reverse_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_Quaternion* versor)
+{
+    const float s0s0 = versor->s0 * versor->s0;
+    const float x1x1 = versor->x1 * versor->x1;
+    const float x2x2 = versor->x2 * versor->x2;
+    const float x3x3 = versor->x3 * versor->x3;
+
+    const float s0x1 = versor->s0 * versor->x1;
+    const float s0x2 = versor->s0 * versor->x2;
+    const float s0x3 = versor->s0 * versor->x3;
+
+    const float x1x2 = versor->x1 * versor->x2;
+    const float x1x3 = versor->x1 * versor->x3;
+
+    const float x2x3 = versor->x2 * versor->x3;
+
+    matrix->r1c1 = ((s0s0 + x1x1) - (x2x2 + x3x3));
+    matrix->r2c2 = ((s0s0 + x2x2) - (x1x1 + x3x3));
+    matrix->r3c3 = ((s0s0 + x3x3) - (x1x1 + x2x2));
+
+    matrix->r1c2 = 2.0f * (x1x2 + s0x3);
+    matrix->r2c3 = 2.0f * (x2x3 + s0x1);
+    matrix->r3c1 = 2.0f * (x1x3 + s0x2);
+
+    matrix->r2c1 = 2.0f * (x1x2 - s0x3);
+    matrix->r3c2 = 2.0f * (x2x3 - s0x1);
+    matrix->r1c3 = 2.0f * (x1x3 - s0x2);
+}
+
+inline void _bgc_fp64_versor_get_reverse_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_Quaternion* versor)
+{
+    const double s0s0 = versor->s0 * versor->s0;
+    const double x1x1 = versor->x1 * versor->x1;
+    const double x2x2 = versor->x2 * versor->x2;
+    const double x3x3 = versor->x3 * versor->x3;
+
+    const double s0x1 = versor->s0 * versor->x1;
+    const double s0x2 = versor->s0 * versor->x2;
+    const double s0x3 = versor->s0 * versor->x3;
+
+    const double x1x2 = versor->x1 * versor->x2;
+    const double x1x3 = versor->x1 * versor->x3;
+
+    const double x2x3 = versor->x2 * versor->x3;
+
+    matrix->r1c1 = ((s0s0 + x1x1) - (x2x2 + x3x3));
+    matrix->r2c2 = ((s0s0 + x2x2) - (x1x1 + x3x3));
+    matrix->r3c3 = ((s0s0 + x3x3) - (x1x1 + x2x2));
+
+    matrix->r1c2 = 2.0 * (x1x2 + s0x3);
+    matrix->r2c3 = 2.0 * (x2x3 + s0x1);
+    matrix->r3c1 = 2.0 * (x1x3 + s0x2);
+
+    matrix->r2c1 = 2.0 * (x1x2 - s0x3);
+    matrix->r3c2 = 2.0 * (x2x3 - s0x1);
+    matrix->r1c3 = 2.0 * (x1x3 - s0x2);
+}
+
 // ============ Get Rotation Matrix ============= //
 
 inline int bgc_fp32_quaternion_get_rotation_matrix(BGC_FP32_Matrix3x3* rotation, const BGC_FP32_Quaternion* quaternion)

basic-geometry/rigid-pose3.c

@@ -38,3 +38,14 @@ extern inline void bgc_fp64_rigid_pose3_combine(BGC_FP64_RigidPose3* combination
 
 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);
+
+extern inline void bgc_fp32_rigid_pose3_get_outward_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_RigidPose3* pose);
+extern inline void bgc_fp64_rigid_pose3_get_outward_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_RigidPose3* pose);
+
+extern inline void bgc_fp32_rigid_pose3_get_inward_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_RigidPose3* pose);
+extern inline void bgc_fp64_rigid_pose3_get_inward_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_RigidPose3* pose);
+
+extern inline void bgc_fp32_rigid_pose3_get_outward_affine3(BGC_FP32_Affine3* affine_map, const BGC_FP32_RigidPose3* pose);
+extern inline void bgc_fp64_rigid_pose3_get_outward_affine3(BGC_FP64_Affine3* affine_map, const BGC_FP64_RigidPose3* pose);
+
+extern inline void bgc_fp32_rigid_pose3_get_inward_affine3(BGC_FP32_Affine3* affine_map, const BGC_FP32_RigidPose3* pose);

basic-geometry/rigid-pose3.h

@@ -4,6 +4,7 @@
 #include <math.h>
 
 #include "types.h"
+#include "affine3.h"
 #include "quaternion.h"
 #include "dual-quaternion.h"
 
@@ -235,4 +236,66 @@ inline void bgc_fp64_rigid_pose3_exclude(BGC_FP64_RigidPose3* difference, const
     _bgc_fp64_rigid_pose3_normalize(difference);
 }
 
+// ============= Get Outward Matrix ============= //
+
+inline void bgc_fp32_rigid_pose3_get_outward_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_RigidPose3* pose)
+{
+    _bgc_fp32_versor_get_rotation_matrix(matrix, &pose->_versor.real_part);
+}
+
+inline void bgc_fp64_rigid_pose3_get_outward_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_RigidPose3* pose)
+{
+    _bgc_fp64_versor_get_rotation_matrix(matrix, &pose->_versor.real_part);
+}
+
+// ============= Get Inward Matrix ============== //
+
+inline void bgc_fp32_rigid_pose3_get_inward_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_RigidPose3* pose)
+{
+    _bgc_fp32_versor_get_reverse_matrix(matrix, &pose->_versor.real_part);
+}
+
+inline void bgc_fp64_rigid_pose3_get_inward_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_RigidPose3* pose)
+{
+    _bgc_fp64_versor_get_reverse_matrix(matrix, &pose->_versor.real_part);
+}
+
+// ============ Get Outward Affine3 ============= //
+
+inline void bgc_fp32_rigid_pose3_get_outward_affine3(BGC_FP32_Affine3* affine_map, const BGC_FP32_RigidPose3* pose)
+{
+    _bgc_fp32_versor_get_rotation_matrix(&affine_map->distortion, &pose->_versor.real_part);
+
+    const BGC_FP32_Quaternion * real = &pose->_versor.real_part;
+    const BGC_FP32_Quaternion * dual = &pose->_versor.dual_part;
+
+    affine_map->shift.x1 = (dual->x1 * real->s0 + dual->x3 * real->x2) - (dual->s0 * real->x1 + dual->x2 * real->x3);
+    affine_map->shift.x2 = (dual->x2 * real->s0 + dual->x1 * real->x3) - (dual->s0 * real->x2 + dual->x3 * real->x1);
+    affine_map->shift.x3 = (dual->x3 * real->s0 + dual->x2 * real->x1) - (dual->s0 * real->x3 + dual->x1 * real->x2);
+}
+
+inline void bgc_fp64_rigid_pose3_get_outward_affine3(BGC_FP64_Affine3* affine_map, const BGC_FP64_RigidPose3* pose)
+{
+    _bgc_fp64_versor_get_rotation_matrix(&affine_map->distortion, &pose->_versor.real_part);
+
+    const BGC_FP64_Quaternion * real = &pose->_versor.real_part;
+    const BGC_FP64_Quaternion * dual = &pose->_versor.dual_part;
+
+    affine_map->shift.x1 = (dual->x1 * real->s0 + dual->x3 * real->x2) - (dual->s0 * real->x1 + dual->x2 * real->x3);
+    affine_map->shift.x2 = (dual->x2 * real->s0 + dual->x1 * real->x3) - (dual->s0 * real->x2 + dual->x3 * real->x1);
+    affine_map->shift.x3 = (dual->x3 * real->s0 + dual->x2 * real->x1) - (dual->s0 * real->x3 + dual->x1 * real->x2);
+}
+
+// ============= Get Inward Affine3 ============= //
+
+inline void bgc_fp32_rigid_pose3_get_inward_affine3(BGC_FP32_Affine3* affine_map, const BGC_FP32_RigidPose3* pose)
+{
+    _bgc_fp32_versor_get_reverse_matrix(&affine_map->distortion, &pose->_versor.real_part);
+
+    const BGC_FP32_Quaternion * real = &pose->_versor.real_part;
+    const BGC_FP32_Quaternion * dual = &pose->_versor.dual_part;
+
+    //TODO: set the shift in the affine map
+}
+
 #endif

basic-geometry/turn3.c

@@ -468,7 +468,7 @@ int bgc_fp32_turn3_find_pair_difference(
     _bgc_fp32_turn3_get_turning_quaternion(&q1, &first_fixed_main, &second_fixed_main, &first_fixed_branch);
 
     // Roughly turn first_fixed_branch with q1 turn
-    _bgc_fp32_quaternion_turn_vector_roughly(&first_turned_branch, &q1, &first_fixed_branch);
+    _bgc_fp32_versor_turn_vector(&first_turned_branch, &q1, &first_fixed_branch);
 
     // Calculation of a turn (q2) which turns first_turned_branch into second_fixed_branch
     _bgc_fp32_turn3_get_turning_quaternion(&q2, &first_turned_branch, &second_fixed_branch, &second_fixed_main);
@@ -515,7 +515,7 @@ int bgc_fp64_turn3_find_pair_difference(
     _bgc_fp64_turn3_get_turning_quaternion(&q1, &first_fixed_main, &second_fixed_main, &first_fixed_branch);
 
     // Roughly turn first_fixed_branch with q1 turn
-    _bgc_fp64_quaternion_turn_vector_roughly(&first_turned_branch, &q1, &first_fixed_branch);
+    _bgc_fp64_versor_turn_vector(&first_turned_branch, &q1, &first_fixed_branch);
 
     // Calculation of a turn (q2) which turns first_turned_branch into second_fixed_branch
     _bgc_fp64_turn3_get_turning_quaternion(&q2, &first_turned_branch, &second_fixed_branch, &second_fixed_main);

basic-geometry/turn3.h

@@ -381,133 +381,37 @@ void bgc_fp64_turn3_spherically_interpolate(BGC_FP64_Turn3* interpolation, const
 
 inline void bgc_fp32_turn3_get_rotation_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_Turn3* turn)
 {
-    const float s0s0 = turn->_versor.s0 * turn->_versor.s0;
+    _bgc_fp32_versor_get_rotation_matrix(matrix, &turn->_versor);
-    const float x1x1 = turn->_versor.x1 * turn->_versor.x1;
-    const float x2x2 = turn->_versor.x2 * turn->_versor.x2;
-    const float x3x3 = turn->_versor.x3 * turn->_versor.x3;
-
-    const float s0x1 = turn->_versor.s0 * turn->_versor.x1;
-    const float s0x2 = turn->_versor.s0 * turn->_versor.x2;
-    const float s0x3 = turn->_versor.s0 * turn->_versor.x3;
-
-    const float x1x2 = turn->_versor.x1 * turn->_versor.x2;
-    const float x1x3 = turn->_versor.x1 * turn->_versor.x3;
-
-    const float x2x3 = turn->_versor.x2 * turn->_versor.x3;
-
-    matrix->r1c1 = ((s0s0 + x1x1) - (x2x2 + x3x3));
-    matrix->r2c2 = ((s0s0 + x2x2) - (x1x1 + x3x3));
-    matrix->r3c3 = ((s0s0 + x3x3) - (x1x1 + x2x2));
-
-    matrix->r1c2 = 2.0f * (x1x2 - s0x3);
-    matrix->r2c3 = 2.0f * (x2x3 - s0x1);
-    matrix->r3c1 = 2.0f * (x1x3 - s0x2);
-
-    matrix->r2c1 = 2.0f * (x1x2 + s0x3);
-    matrix->r3c2 = 2.0f * (x2x3 + s0x1);
-    matrix->r1c3 = 2.0f * (x1x3 + s0x2);
 }
 
 inline void bgc_fp64_turn3_get_rotation_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_Turn3* turn)
 {
-    const double s0s0 = turn->_versor.s0 * turn->_versor.s0;
+    _bgc_fp64_versor_get_rotation_matrix(matrix, &turn->_versor);
-    const double x1x1 = turn->_versor.x1 * turn->_versor.x1;
-    const double x2x2 = turn->_versor.x2 * turn->_versor.x2;
-    const double x3x3 = turn->_versor.x3 * turn->_versor.x3;
-
-    const double s0x1 = turn->_versor.s0 * turn->_versor.x1;
-    const double s0x2 = turn->_versor.s0 * turn->_versor.x2;
-    const double s0x3 = turn->_versor.s0 * turn->_versor.x3;
-
-    const double x1x2 = turn->_versor.x1 * turn->_versor.x2;
-    const double x1x3 = turn->_versor.x1 * turn->_versor.x3;
-
-    const double x2x3 = turn->_versor.x2 * turn->_versor.x3;
-
-    matrix->r1c1 = ((s0s0 + x1x1) - (x2x2 + x3x3));
-    matrix->r2c2 = ((s0s0 + x2x2) - (x1x1 + x3x3));
-    matrix->r3c3 = ((s0s0 + x3x3) - (x1x1 + x2x2));
-
-    matrix->r1c2 = 2.0 * (x1x2 - s0x3);
-    matrix->r2c3 = 2.0 * (x2x3 - s0x1);
-    matrix->r3c1 = 2.0 * (x1x3 - s0x2);
-
-    matrix->r2c1 = 2.0 * (x1x2 + s0x3);
-    matrix->r3c2 = 2.0 * (x2x3 + s0x1);
-    matrix->r1c3 = 2.0 * (x1x3 + s0x2);
 }
 
 // ============= Get Reverse Matrix ============= //
 
 inline void bgc_fp32_turn3_get_reverse_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_Turn3* turn)
 {
-    const float s0s0 = turn->_versor.s0 * turn->_versor.s0;
+    _bgc_fp32_versor_get_reverse_matrix(matrix, &turn->_versor);
-    const float x1x1 = turn->_versor.x1 * turn->_versor.x1;
-    const float x2x2 = turn->_versor.x2 * turn->_versor.x2;
-    const float x3x3 = turn->_versor.x3 * turn->_versor.x3;
-
-    const float s0x1 = turn->_versor.s0 * turn->_versor.x1;
-    const float s0x2 = turn->_versor.s0 * turn->_versor.x2;
-    const float s0x3 = turn->_versor.s0 * turn->_versor.x3;
-
-    const float x1x2 = turn->_versor.x1 * turn->_versor.x2;
-    const float x1x3 = turn->_versor.x1 * turn->_versor.x3;
-
-    const float x2x3 = turn->_versor.x2 * turn->_versor.x3;
-
-    matrix->r1c1 = ((s0s0 + x1x1) - (x2x2 + x3x3));
-    matrix->r2c2 = ((s0s0 + x2x2) - (x1x1 + x3x3));
-    matrix->r3c3 = ((s0s0 + x3x3) - (x1x1 + x2x2));
-
-    matrix->r1c2 = 2.0f * (x1x2 + s0x3);
-    matrix->r2c3 = 2.0f * (x2x3 + s0x1);
-    matrix->r3c1 = 2.0f * (x1x3 + s0x2);
-
-    matrix->r2c1 = 2.0f * (x1x2 - s0x3);
-    matrix->r3c2 = 2.0f * (x2x3 - s0x1);
-    matrix->r1c3 = 2.0f * (x1x3 - s0x2);
 }
 
 inline void bgc_fp64_turn3_get_reverse_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_Turn3* turn)
 {
-    const double s0s0 = turn->_versor.s0 * turn->_versor.s0;
+    _bgc_fp64_versor_get_reverse_matrix(matrix, &turn->_versor);
-    const double x1x1 = turn->_versor.x1 * turn->_versor.x1;
-    const double x2x2 = turn->_versor.x2 * turn->_versor.x2;