diff --git a/basic-geometry/basic-geometry.vcxproj b/basic-geometry/basic-geometry.vcxproj index 42d8983..5bbc782 100644 --- a/basic-geometry/basic-geometry.vcxproj +++ b/basic-geometry/basic-geometry.vcxproj @@ -31,6 +31,7 @@ + @@ -52,6 +53,7 @@ + diff --git a/basic-geometry/basic-geometry.vcxproj.filters b/basic-geometry/basic-geometry.vcxproj.filters index c94a8ad..49d9826 100644 --- a/basic-geometry/basic-geometry.vcxproj.filters +++ b/basic-geometry/basic-geometry.vcxproj.filters @@ -84,6 +84,9 @@ Файлы заголовков + + Файлы заголовков + @@ -143,5 +146,8 @@ Исходные файлы + + Исходные файлы + \ No newline at end of file diff --git a/basic-geometry/posture3.c b/basic-geometry/posture3.c new file mode 100644 index 0000000..8c248f0 --- /dev/null +++ b/basic-geometry/posture3.c @@ -0,0 +1,19 @@ +#include "posture3.h" + +extern inline void bgc_fp32_posture3_reset(BGC_FP32_Posture3* posture); +extern inline void bgc_fp64_posture3_reset(BGC_FP64_Posture3* posture); + +void _bgc_fp32_posture3_normalize(BGC_FP32_Posture3* posture) +{ + const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(&posture->_dual_versor.real_part); + + if (square_magnitude <= BGC_FP32_SQUARE_EPSILON || isnan(square_magnitude)) { + bgc_fp32_posture3_reset(posture); + return; + } + + const float multiplier = sqrtf(1.0f / square_magnitude); + + bgc_fp32_quaternion_multiply_by_real(&posture->_dual_versor.real_part, &posture->_dual_versor.real_part, multiplier); + bgc_fp32_quaternion_multiply_by_real(&posture->_dual_versor.dual_part, &posture->_dual_versor.dual_part, multiplier); +} diff --git a/basic-geometry/posture3.h b/basic-geometry/posture3.h new file mode 100644 index 0000000..e89c642 --- /dev/null +++ b/basic-geometry/posture3.h @@ -0,0 +1,39 @@ +#ifndef _BGC_POSTURE3_H_INCLUDED_ +#define _BGC_POSTURE3_H_INCLUDED_ + +#include "types.h" +#include "quaternion.h" +#include "dual-quaternion.h" + +// ==================== Reset =================== // + +inline void bgc_fp32_posture3_reset(BGC_FP32_Posture3* posture) +{ + posture->_dual_versor.real_part.s0 = 1.0f; + posture->_dual_versor.real_part.x1 = 0.0f; + posture->_dual_versor.real_part.x2 = 0.0f; + posture->_dual_versor.real_part.x3 = 0.0f; + + posture->_dual_versor.dual_part.s0 = 0.0f; + posture->_dual_versor.dual_part.x1 = 0.0f; + posture->_dual_versor.dual_part.x2 = 0.0f; + posture->_dual_versor.dual_part.x3 = 0.0f; +} + +inline void bgc_fp64_posture3_reset(BGC_FP64_Posture3* posture) +{ + posture->_dual_versor.real_part.s0 = 1.0; + posture->_dual_versor.real_part.x1 = 0.0; + posture->_dual_versor.real_part.x2 = 0.0; + posture->_dual_versor.real_part.x3 = 0.0; + + posture->_dual_versor.dual_part.s0 = 0.0; + posture->_dual_versor.dual_part.x1 = 0.0; + posture->_dual_versor.dual_part.x2 = 0.0; + posture->_dual_versor.dual_part.x3 = 0.0; +} + +void _bgc_fp32_posture3_normalize(BGC_FP32_Posture3* posture); + + +#endif diff --git a/basic-geometry/types.h b/basic-geometry/types.h index 1d0e564..ce6c8d4 100644 --- a/basic-geometry/types.h +++ b/basic-geometry/types.h @@ -211,4 +211,14 @@ typedef struct { BGC_FP64_Quaternion real_part, dual_part; } BGC_FP64_DualQuaternion; +// ================== Posture3 ================== // + +typedef struct { + BGC_FP32_DualQuaternion _dual_versor; +} BGC_FP32_Posture3; + +typedef struct { + BGC_FP64_DualQuaternion _dual_versor; +} BGC_FP64_Posture3; + #endif