diff --git a/basic-geometry/quaternion.c b/basic-geometry/quaternion.c index be269a9..3de43c8 100644 --- a/basic-geometry/quaternion.c +++ b/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_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_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_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_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_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_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); diff --git a/basic-geometry/quaternion.h b/basic-geometry/quaternion.h index a801394..8fab43a 100644 --- a/basic-geometry/quaternion.h +++ b/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) diff --git a/basic-geometry/rigid-pose3.c b/basic-geometry/rigid-pose3.c index ab60b7f..abe7f05 100644 --- a/basic-geometry/rigid-pose3.c +++ b/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); diff --git a/basic-geometry/rigid-pose3.h b/basic-geometry/rigid-pose3.h index c5adea4..13e0ddd 100644 --- a/basic-geometry/rigid-pose3.h +++ b/basic-geometry/rigid-pose3.h @@ -4,6 +4,7 @@ #include #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 diff --git a/basic-geometry/turn3.c b/basic-geometry/turn3.c index 3a09baf..bbcf2ba 100644 --- a/basic-geometry/turn3.c +++ b/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); diff --git a/basic-geometry/turn3.h b/basic-geometry/turn3.h index 33b1c58..87f642a 100644 --- a/basic-geometry/turn3.h +++ b/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; - 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); + _bgc_fp32_versor_get_rotation_matrix(matrix, &turn->_versor); } 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; - 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); + _bgc_fp64_versor_get_rotation_matrix(matrix, &turn->_versor); } // ============= 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; - 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); + _bgc_fp32_versor_get_reverse_matrix(matrix, &turn->_versor); } 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; - 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); + _bgc_fp64_versor_get_reverse_matrix(matrix, &turn->_versor); } // ============= Get Both Matrixes ============== // inline void bgc_fp32_turn3_get_both_matrices(BGC_FP32_Matrix3x3* rotation, BGC_FP32_Matrix3x3* reverse, const BGC_FP32_Turn3* turn) { - bgc_fp32_turn3_get_reverse_matrix(reverse, turn); + _bgc_fp32_versor_get_reverse_matrix(reverse, &turn->_versor); bgc_fp32_matrix3x3_get_transposed(rotation, reverse); } inline void bgc_fp64_turn3_get_both_matrices(BGC_FP64_Matrix3x3* rotation, BGC_FP64_Matrix3x3* reverse, const BGC_FP64_Turn3* turn) { - bgc_fp64_turn3_get_reverse_matrix(reverse, turn); + _bgc_fp64_versor_get_reverse_matrix(reverse, &turn->_versor); bgc_fp64_matrix3x3_get_transposed(rotation, reverse); } @@ -515,24 +419,24 @@ inline void bgc_fp64_turn3_get_both_matrices(BGC_FP64_Matrix3x3* rotation, BGC_F inline void bgc_fp32_turn3_vector(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Turn3* turn, const BGC_FP32_Vector3* original_vector) { - _bgc_fp32_quaternion_turn_vector_roughly(turned_vector, &turn->_versor, original_vector); + _bgc_fp32_versor_turn_vector(turned_vector, &turn->_versor, original_vector); } inline void bgc_fp64_turn3_vector(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Turn3* turn, const BGC_FP64_Vector3* original_vector) { - _bgc_fp64_quaternion_turn_vector_roughly(turned_vector, &turn->_versor, original_vector); + _bgc_fp64_versor_turn_vector(turned_vector, &turn->_versor, original_vector); } // ============== Turn Vector Back ============== // inline void bgc_fp32_turn3_vector_back(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Turn3* turn, const BGC_FP32_Vector3* original_vector) { - _bgc_fp32_quaternion_turn_vector_back_roughly(turned_vector, &turn->_versor, original_vector); + _bgc_fp32_versor_turn_vector_back(turned_vector, &turn->_versor, original_vector); } inline void bgc_fp64_turn3_vector_back(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Turn3* turn, const BGC_FP64_Vector3* original_vector) { - _bgc_fp64_quaternion_turn_vector_back_roughly(turned_vector, &turn->_versor, original_vector); + _bgc_fp64_versor_turn_vector_back(turned_vector, &turn->_versor, original_vector); } // ================== Are Close ================= //