diff --git a/basic-geometry/posture3.c b/basic-geometry/posture3.c index f3e76b2..87dd872 100644 --- a/basic-geometry/posture3.c +++ b/basic-geometry/posture3.c @@ -12,6 +12,8 @@ extern inline void bgc_fp64_posture3_swap(BGC_FP64_Posture3* posture1, BGC_FP64_ extern inline void bgc_fp32_posture3_convert_to_fp64(BGC_FP64_Posture3* destination, const BGC_FP32_Posture3* source); extern inline void bgc_fp64_posture3_convert_to_fp32(BGC_FP32_Posture3* destination, const BGC_FP64_Posture3* source); +extern inline void bgc_fp32_posture3_combine(BGC_FP32_Posture3* combination, const BGC_FP32_Posture3* first, const BGC_FP32_Posture3* second); +extern inline void bgc_fp64_posture3_combine(BGC_FP64_Posture3* combination, const BGC_FP64_Posture3* first, const BGC_FP64_Posture3* second); void _bgc_fp32_posture3_normalize(BGC_FP32_Posture3* posture, const float square_magnitude) { @@ -22,8 +24,7 @@ void _bgc_fp32_posture3_normalize(BGC_FP32_Posture3* posture, const float square const float multiplier = sqrtf(1.0f / square_magnitude); - bgc_fp32_quaternion_multiply_by_real(&posture->_real_part, &posture->_real_part, multiplier); - bgc_fp32_quaternion_multiply_by_real(&posture->_dual_part, &posture->_dual_part, multiplier); + bgc_fp32_dual_quaternion_multiply_by_real_number(&posture->_versor, &posture->_versor, multiplier); } void _bgc_fp64_posture3_normalize(BGC_FP64_Posture3* posture, const double square_magnitude) @@ -35,6 +36,5 @@ void _bgc_fp64_posture3_normalize(BGC_FP64_Posture3* posture, const double squar const double multiplier = sqrt(1.0 / square_magnitude); - bgc_fp64_quaternion_multiply_by_real(&posture->_real_part, &posture->_real_part, multiplier); - bgc_fp64_quaternion_multiply_by_real(&posture->_dual_part, &posture->_dual_part, multiplier); + bgc_fp64_dual_quaternion_multiply_by_real_number(&posture->_versor, &posture->_versor, multiplier); } diff --git a/basic-geometry/posture3.h b/basic-geometry/posture3.h index 014ea2e..1427bb0 100644 --- a/basic-geometry/posture3.h +++ b/basic-geometry/posture3.h @@ -15,66 +15,61 @@ void _bgc_fp64_posture3_normalize(BGC_FP64_Posture3* posture, const double squar inline void bgc_fp32_posture3_reset(BGC_FP32_Posture3* posture) { - posture->_real_part.s0 = 1.0f; - posture->_real_part.x1 = 0.0f; - posture->_real_part.x2 = 0.0f; - posture->_real_part.x3 = 0.0f; + posture->_versor.real_part.s0 = 1.0f; + posture->_versor.real_part.x1 = 0.0f; + posture->_versor.real_part.x2 = 0.0f; + posture->_versor.real_part.x3 = 0.0f; - posture->_dual_part.s0 = 0.0f; - posture->_dual_part.x1 = 0.0f; - posture->_dual_part.x2 = 0.0f; - posture->_dual_part.x3 = 0.0f; + posture->_versor.dual_part.s0 = 0.0f; + posture->_versor.dual_part.x1 = 0.0f; + posture->_versor.dual_part.x2 = 0.0f; + posture->_versor.dual_part.x3 = 0.0f; } inline void bgc_fp64_posture3_reset(BGC_FP64_Posture3* posture) { - posture->_real_part.s0 = 1.0; - posture->_real_part.x1 = 0.0; - posture->_real_part.x2 = 0.0; - posture->_real_part.x3 = 0.0; + posture->_versor.real_part.s0 = 1.0; + posture->_versor.real_part.x1 = 0.0; + posture->_versor.real_part.x2 = 0.0; + posture->_versor.real_part.x3 = 0.0; - posture->_dual_part.s0 = 0.0; - posture->_dual_part.x1 = 0.0; - posture->_dual_part.x2 = 0.0; - posture->_dual_part.x3 = 0.0; + posture->_versor.dual_part.s0 = 0.0; + posture->_versor.dual_part.x1 = 0.0; + posture->_versor.dual_part.x2 = 0.0; + posture->_versor.dual_part.x3 = 0.0; } // ==================== Copy ==================== // inline void bgc_fp32_posture3_copy(BGC_FP32_Posture3* destination, const BGC_FP32_Posture3* source) { - bgc_fp32_quaternion_copy(&destination->_real_part, &source->_real_part); - bgc_fp32_quaternion_copy(&destination->_dual_part, &source->_dual_part); + bgc_fp32_dual_quaternion_copy(&destination->_versor, &source->_versor); } inline void bgc_fp64_posture3_copy(BGC_FP64_Posture3* destination, const BGC_FP64_Posture3* source) { - bgc_fp64_quaternion_copy(&destination->_real_part, &source->_real_part); - bgc_fp64_quaternion_copy(&destination->_dual_part, &source->_dual_part); + bgc_fp64_dual_quaternion_copy(&destination->_versor, &source->_versor); } // ==================== Swap ==================== // inline void bgc_fp32_posture3_swap(BGC_FP32_Posture3* posture1, BGC_FP32_Posture3* posture2) { - bgc_fp32_quaternion_swap(&posture1->_real_part, &posture2->_real_part); - bgc_fp32_quaternion_swap(&posture1->_dual_part, &posture2->_dual_part); + bgc_fp32_dual_quaternion_swap(&posture1->_versor, &posture2->_versor); } inline void bgc_fp64_posture3_swap(BGC_FP64_Posture3* posture1, BGC_FP64_Posture3* posture2) { - bgc_fp64_quaternion_swap(&posture1->_real_part, &posture2->_real_part); - bgc_fp64_quaternion_swap(&posture1->_dual_part, &posture2->_dual_part); + bgc_fp64_dual_quaternion_swap(&posture1->_versor, &posture2->_versor); } // ================== Convert =================== // inline void bgc_fp32_posture3_convert_to_fp64(BGC_FP64_Posture3* destination, const BGC_FP32_Posture3* 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); + bgc_fp32_dual_quaternion_convert_to_fp64(&destination->_versor, &source->_versor); - const double square_magnitude = bgc_fp64_quaternion_get_square_magnitude(&destination->_real_part); + const double square_magnitude = bgc_fp64_quaternion_get_square_magnitude(&destination->_versor.real_part); if (!bgc_fp64_is_square_unit(square_magnitude)) { _bgc_fp64_posture3_normalize(destination, square_magnitude); @@ -83,10 +78,9 @@ inline void bgc_fp32_posture3_convert_to_fp64(BGC_FP64_Posture3* destination, co inline void bgc_fp64_posture3_convert_to_fp32(BGC_FP32_Posture3* destination, const BGC_FP64_Posture3* 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); + bgc_fp64_dual_quaternion_convert_to_fp32(&destination->_versor, &source->_versor); + + const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(&destination->_versor.real_part); if (!bgc_fp32_is_square_unit(square_magnitude)) { _bgc_fp32_posture3_normalize(destination, square_magnitude); @@ -95,9 +89,26 @@ inline void bgc_fp64_posture3_convert_to_fp32(BGC_FP32_Posture3* destination, co // ================== Combine =================== // -inline void bgc_fp32_posture3_combine(BGC_FP32_Posture3* combination, const BGC_FP64_Posture3* first, const BGC_FP64_Posture3* second) +inline void bgc_fp32_posture3_combine(BGC_FP32_Posture3* combination, const BGC_FP32_Posture3* first, const BGC_FP32_Posture3* 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->_versor.real_part); + + if (!bgc_fp32_is_square_unit(square_magnitude)) { + _bgc_fp32_posture3_normalize(combination, square_magnitude); + } +} + +inline void bgc_fp64_posture3_combine(BGC_FP64_Posture3* combination, const BGC_FP64_Posture3* first, const BGC_FP64_Posture3* 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->_versor.real_part); + + if (!bgc_fp64_is_square_unit(square_magnitude)) { + _bgc_fp64_posture3_normalize(combination, square_magnitude); + } } #endif diff --git a/basic-geometry/types.h b/basic-geometry/types.h index 430709f..ba9961e 100644 --- a/basic-geometry/types.h +++ b/basic-geometry/types.h @@ -214,11 +214,11 @@ typedef struct { // ================== Posture3 ================== // typedef struct { - BGC_FP32_Quaternion _real_part, _dual_part; + BGC_FP32_DualQuaternion _versor; } BGC_FP32_Posture3; typedef struct { - BGC_FP64_Quaternion _real_part, _dual_part; + BGC_FP64_DualQuaternion _versor; } BGC_FP64_Posture3; #endif