Kimelas/bgc-c
2
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Исправления в нормализации дуальных кватернионов и позиционирования с помощью дуальных кватернионов

Browse source
This commit is contained in:
Andrey Pokidov 2026-03-18 20:23:50 +07:00
parent 24d37be75d
commit a57f13f4a9
9 changed files with 206 additions and 170 deletions
Download patch file Download diff file Expand all files Collapse all files

138
basic-geometry/dual-quaternion.h
View file

@ -260,34 +260,102 @@ inline void bgc_fp64_dual_quaternion_get_fully_conjugate(BGC_FP64_DualQuaternion
// ================= Normalize ================== //
void _bgc_fp32_dual_quaternion_normalize_parts(BGC_FP32_DualQuaternion* quaternion, const float square_magnitude);
void _bgc_fp64_dual_quaternion_normalize_parts(BGC_FP64_DualQuaternion* quaternion, const double square_magnitude);
inline void bgc_fp32_dual_quaternion_normalize(BGC_FP32_DualQuaternion* quaternion)
inline int bgc_fp32_dual_quaternion_normalize(BGC_FP32_DualQuaternion* quaternion)
{
const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(&quaternion->real_part);
if (square_magnitude <= BGC_FP32_SQUARE_EPSILON || isnan(square_magnitude)) {
return BGC_FAILURE;
}
if (!bgc_fp32_is_square_unit(square_magnitude)) {
_bgc_fp32_quaternion_normalize_normalize_parts(quaternion, square_magnitude);
const float multiplier = sqrtf(1.0f / square_magnitude);
bgc_fp32_quaternion_multiply_by_real_number(&quaternion->real_part, &quaternion->real_part, multiplier);
bgc_fp32_quaternion_multiply_by_real_number(&quaternion->dual_part, &quaternion->dual_part, multiplier);
}
const float dot_product = bgc_fp32_quaternion_get_dot_product(&quaternion->real_part, &quaternion->dual_part);
bgc_fp32_quaternion_subtract_scaled(&quaternion->dual_part, &quaternion->dual_part, &quaternion->real_part, dot_product);
return BGC_SUCCESS;
}
inline void bgc_fp64_dual_quaternion_normalize(BGC_FP64_DualQuaternion* quaternion)
inline int bgc_fp64_dual_quaternion_normalize(BGC_FP64_DualQuaternion* quaternion)
{
const double square_magnitude = bgc_fp64_quaternion_get_square_magnitude(&quaternion->real_part);
if (!bgc_fp64_is_square_unit(square_magnitude)) {
_bgc_fp64_quaternion_normalize_normalize_parts(quaternion, square_magnitude);
if (square_magnitude <= BGC_FP64_SQUARE_EPSILON || isnan(square_magnitude)) {
return BGC_FAILURE;
}
const double dot_product = bgc_fp64_quaternion_get_dot_product(&pose->_real_part, &pose->_dual_part);
if (!bgc_fp64_is_square_unit(square_magnitude)) {
const double multiplier = sqrt(1.0 / square_magnitude);
bgc_fp64_quaternion_subtract_scaled(&pose->_dual_part, &pose->_dual_part, &pose->_real_part, dot_product);
bgc_fp64_quaternion_multiply_by_real_number(&quaternion->real_part, &quaternion->real_part, multiplier);
bgc_fp64_quaternion_multiply_by_real_number(&quaternion->dual_part, &quaternion->dual_part, multiplier);
}
const double dot_product = bgc_fp64_quaternion_get_dot_product(&quaternion->real_part, &quaternion->dual_part);
bgc_fp64_quaternion_subtract_scaled(&quaternion->dual_part, &quaternion->dual_part, &quaternion->real_part, dot_product);
return BGC_SUCCESS;
}
// =============== Get Normalized =============== //
inline int bgc_fp32_dual_quaternion_get_normalized(BGC_FP32_DualQuaternion* normalized, const BGC_FP32_DualQuaternion* quaternion)
{
const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(&quaternion->real_part);
if (square_magnitude <= BGC_FP32_SQUARE_EPSILON || isnan(square_magnitude)) {
return BGC_FAILURE;
}
if (bgc_fp32_is_square_unit(square_magnitude)) {
bgc_fp32_quaternion_copy(&normalized->real_part, &quaternion->real_part);
bgc_fp32_quaternion_copy(&normalized->dual_part, &quaternion->dual_part);
}
else {
const float multiplier = sqrtf(1.0f / square_magnitude);
bgc_fp32_quaternion_multiply_by_real_number(&normalized->real_part, &quaternion->real_part, multiplier);
bgc_fp32_quaternion_multiply_by_real_number(&normalized->dual_part, &quaternion->dual_part, multiplier);
}
const float dot_product = bgc_fp32_quaternion_get_dot_product(&normalized->real_part, &normalized->dual_part);
bgc_fp32_quaternion_subtract_scaled(&normalized->dual_part, &normalized->dual_part, &normalized->real_part, dot_product);
return BGC_SUCCESS;
}
inline int bgc_fp64_dual_quaternion_get_normalized(BGC_FP64_DualQuaternion* normalized, const BGC_FP64_DualQuaternion* quaternion)
{
const double square_magnitude = bgc_fp64_quaternion_get_square_magnitude(&quaternion->real_part);
if (square_magnitude <= BGC_FP64_SQUARE_EPSILON || isnan(square_magnitude)) {
return BGC_FAILURE;
}
if (bgc_fp64_is_square_unit(square_magnitude)) {
bgc_fp64_quaternion_copy(&normalized->real_part, &quaternion->real_part);
bgc_fp64_quaternion_copy(&normalized->dual_part, &quaternion->dual_part);
}
else {
const double multiplier = sqrt(1.0 / square_magnitude);
bgc_fp64_quaternion_multiply_by_real_number(&normalized->real_part, &quaternion->real_part, multiplier);
bgc_fp64_quaternion_multiply_by_real_number(&normalized->dual_part, &quaternion->dual_part, multiplier);
}
const double dot_product = bgc_fp64_quaternion_get_dot_product(&normalized->real_part, &normalized->dual_part);
bgc_fp64_quaternion_subtract_scaled(&normalized->dual_part, &normalized->dual_part, &normalized->real_part, dot_product);
return BGC_SUCCESS;
}
// ==================== Add ===================== //
@ -350,14 +418,14 @@ inline void bgc_fp64_dual_quaternion_subtract_scaled(BGC_FP64_DualQuaternion* di
inline void bgc_fp32_dual_quaternion_multiply_by_real_number(BGC_FP32_DualQuaternion* product, const BGC_FP32_DualQuaternion* multiplicand, const float multiplier)
{
bgc_fp32_quaternion_multiply_by_real(&product->real_part, &multiplicand->real_part, multiplier);
bgc_fp32_quaternion_multiply_by_real(&product->dual_part, &multiplicand->dual_part, multiplier);
bgc_fp32_quaternion_multiply_by_real_number(&product->real_part, &multiplicand->real_part, multiplier);
bgc_fp32_quaternion_multiply_by_real_number(&product->dual_part, &multiplicand->dual_part, multiplier);
}
inline void bgc_fp64_dual_quaternion_multiply_by_real_number(BGC_FP64_DualQuaternion* product, const BGC_FP64_DualQuaternion* multiplicand, const double multiplier)
{
bgc_fp64_quaternion_multiply_by_real(&product->real_part, &multiplicand->real_part, multiplier);
bgc_fp64_quaternion_multiply_by_real(&product->dual_part, &multiplicand->dual_part, multiplier);
bgc_fp64_quaternion_multiply_by_real_number(&product->real_part, &multiplicand->real_part, multiplier);
bgc_fp64_quaternion_multiply_by_real_number(&product->dual_part, &multiplicand->dual_part, multiplier);
}
// ========== Multiply by Dual Number =========== //
@ -366,10 +434,10 @@ inline void bgc_fp32_dual_quaternion_multiply_by_dual_number(BGC_FP32_DualQuater
{
BGC_FP32_Quaternion dual_part;
bgc_fp32_quaternion_multiply_by_real(&dual_part, &multiplicand->dual_part, multiplier->real_part);
bgc_fp32_quaternion_multiply_by_real_number(&dual_part, &multiplicand->dual_part, multiplier->real_part);
bgc_fp32_quaternion_add_scaled(&dual_part, &dual_part, &multiplicand->real_part, multiplier->dual_part);
bgc_fp32_quaternion_multiply_by_real(&product->real_part, &multiplicand->real_part, multiplier->real_part);
bgc_fp32_quaternion_multiply_by_real_number(&product->real_part, &multiplicand->real_part, multiplier->real_part);
bgc_fp32_quaternion_copy(&product->dual_part, &dual_part);
}
@ -377,10 +445,10 @@ inline void bgc_fp64_dual_quaternion_multiply_by_dual_number(BGC_FP64_DualQuater
{
BGC_FP64_Quaternion dual_part;
bgc_fp64_quaternion_multiply_by_real(&dual_part, &multiplicand->dual_part, multiplier->real_part);
bgc_fp64_quaternion_multiply_by_real_number(&dual_part, &multiplicand->dual_part, multiplier->real_part);
bgc_fp64_quaternion_add_scaled(&dual_part, &dual_part, &multiplicand->real_part, multiplier->dual_part);
bgc_fp64_quaternion_multiply_by_real(&product->real_part, &multiplicand->real_part, multiplier->real_part);
bgc_fp64_quaternion_multiply_by_real_number(&product->real_part, &multiplicand->real_part, multiplier->real_part);
bgc_fp64_quaternion_copy(&product->dual_part, &dual_part);
}
@ -390,10 +458,10 @@ inline void bgc_fp32_dual_quaternion_multiply_by_conjugate_dual_number(BGC_FP32_
{
BGC_FP32_Quaternion dual_part;
bgc_fp32_quaternion_multiply_by_real(&dual_part, &multiplicand->dual_part, multiplier_to_conjugate->real_part);
bgc_fp32_quaternion_multiply_by_real_number(&dual_part, &multiplicand->dual_part, multiplier_to_conjugate->real_part);
bgc_fp32_quaternion_subtract_scaled(&dual_part, &dual_part, &multiplicand->real_part, multiplier_to_conjugate->dual_part);
bgc_fp32_quaternion_multiply_by_real(&product->real_part, &multiplicand->real_part, multiplier_to_conjugate->real_part);
bgc_fp32_quaternion_multiply_by_real_number(&product->real_part, &multiplicand->real_part, multiplier_to_conjugate->real_part);
bgc_fp32_quaternion_copy(&product->dual_part, &dual_part);
}
@ -401,10 +469,10 @@ inline void bgc_fp64_dual_quaternion_multiply_by_conjugate_dual_number(BGC_FP64_
{
BGC_FP64_Quaternion dual_part;
bgc_fp64_quaternion_multiply_by_real(&dual_part, &multiplicand->dual_part, multiplier_to_conjugate->real_part);
bgc_fp64_quaternion_multiply_by_real_number(&dual_part, &multiplicand->dual_part, multiplier_to_conjugate->real_part);
bgc_fp64_quaternion_subtract_scaled(&dual_part, &dual_part, &multiplicand->real_part, multiplier_to_conjugate->dual_part);
bgc_fp64_quaternion_multiply_by_real(&product->real_part, &multiplicand->real_part, multiplier_to_conjugate->real_part);
bgc_fp64_quaternion_multiply_by_real_number(&product->real_part, &multiplicand->real_part, multiplier_to_conjugate->real_part);
bgc_fp64_quaternion_copy(&product->dual_part, &dual_part);
}
@ -470,8 +538,8 @@ inline int bgc_fp32_dual_quaternion_divide_by_real_number(BGC_FP32_DualQuaternio
const float multiplier = 1.0f / divisor;
bgc_fp32_quaternion_multiply_by_real(&quotient->real_part, &dividend->real_part, multiplier);
bgc_fp32_quaternion_multiply_by_real(&quotient->dual_part, &dividend->dual_part, multiplier);
bgc_fp32_quaternion_multiply_by_real_number(&quotient->real_part, &dividend->real_part, multiplier);
bgc_fp32_quaternion_multiply_by_real_number(&quotient->dual_part, &dividend->dual_part, multiplier);
return BGC_SUCCESS;
}
@ -484,8 +552,8 @@ inline int bgc_fp64_dual_quaternion_divide_by_real_number(BGC_FP64_DualQuaternio
const double multiplier = 1.0 / divisor;
bgc_fp64_quaternion_multiply_by_real(&quotient->real_part, &dividend->real_part, multiplier);
bgc_fp64_quaternion_multiply_by_real(&quotient->dual_part, &dividend->dual_part, multiplier);
bgc_fp64_quaternion_multiply_by_real_number(&quotient->real_part, &dividend->real_part, multiplier);
bgc_fp64_quaternion_multiply_by_real_number(&quotient->dual_part, &dividend->dual_part, multiplier);
return BGC_SUCCESS;
}
@ -565,8 +633,8 @@ inline int bgc_fp32_dual_quaternion_divide_by_quaternion(BGC_FP32_DualQuaternion
bgc_fp32_quaternion_multiply_by_conjugate(&quotient->real_part, &dividend->real_part, divisor);
bgc_fp32_quaternion_multiply_by_conjugate(&quotient->dual_part, &dividend->dual_part, divisor);
bgc_fp32_quaternion_multiply_by_real(&quotient->real_part, &quotient->real_part, multiplier);
bgc_fp32_quaternion_multiply_by_real(&quotient->dual_part, &quotient->dual_part, multiplier);
bgc_fp32_quaternion_multiply_by_real_number(&quotient->real_part, &quotient->real_part, multiplier);
bgc_fp32_quaternion_multiply_by_real_number(&quotient->dual_part, &quotient->dual_part, multiplier);
return BGC_SUCCESS;
}
@ -584,8 +652,8 @@ inline int bgc_fp64_dual_quaternion_divide_by_quaternion(BGC_FP64_DualQuaternion
bgc_fp64_quaternion_multiply_by_conjugate(&quotient->real_part, &dividend->real_part, divisor);
bgc_fp64_quaternion_multiply_by_conjugate(&quotient->dual_part, &dividend->dual_part, divisor);
bgc_fp64_quaternion_multiply_by_real(&quotient->real_part, &quotient->real_part, multiplier);
bgc_fp64_quaternion_multiply_by_real(&quotient->dual_part, &quotient->dual_part, multiplier);
bgc_fp64_quaternion_multiply_by_real_number(&quotient->real_part, &quotient->real_part, multiplier);
bgc_fp64_quaternion_multiply_by_real_number(&quotient->dual_part, &quotient->dual_part, multiplier);
return BGC_SUCCESS;
}
@ -605,8 +673,8 @@ inline int bgc_fp32_dual_quaternion_divide_by_conjugate_quaternion(BGC_FP32_Dual
bgc_fp32_quaternion_multiply_by_quaternion(&quotient->real_part, &dividend->real_part, divisor_to_conjugate);
bgc_fp32_quaternion_multiply_by_quaternion(&quotient->dual_part, &dividend->dual_part, divisor_to_conjugate);
bgc_fp32_quaternion_multiply_by_real(&quotient->real_part, &quotient->real_part, multiplier);
bgc_fp32_quaternion_multiply_by_real(&quotient->dual_part, &quotient->dual_part, multiplier);
bgc_fp32_quaternion_multiply_by_real_number(&quotient->real_part, &quotient->real_part, multiplier);
bgc_fp32_quaternion_multiply_by_real_number(&quotient->dual_part, &quotient->dual_part, multiplier);
return BGC_SUCCESS;
}
@ -624,8 +692,8 @@ inline int bgc_fp64_dual_quaternion_divide_by_conjugate_quaternion(BGC_FP64_Dual
bgc_fp64_quaternion_multiply_by_quaternion(&quotient->real_part, &dividend->real_part, divisor_to_conjugate);
bgc_fp64_quaternion_multiply_by_quaternion(&quotient->dual_part, &dividend->dual_part, divisor_to_conjugate);
bgc_fp64_quaternion_multiply_by_real(&quotient->real_part, &quotient->real_part, multiplier);
bgc_fp64_quaternion_multiply_by_real(&quotient->dual_part, &quotient->dual_part, multiplier);
bgc_fp64_quaternion_multiply_by_real_number(&quotient->real_part, &quotient->real_part, multiplier);
bgc_fp64_quaternion_multiply_by_real_number(&quotient->dual_part, &quotient->dual_part, multiplier);
return BGC_SUCCESS;
}