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

6
basic-geometry/dual-quaternion.c
View file

@ -42,6 +42,12 @@ extern inline void bgc_fp64_dual_quaternion_fully_conjugate(BGC_FP64_DualQuatern
extern inline void bgc_fp32_dual_quaternion_get_fully_conjugate(BGC_FP32_DualQuaternion* conjugate, const BGC_FP32_DualQuaternion* quaternion);
extern inline void bgc_fp64_dual_quaternion_get_fully_conjugate(BGC_FP64_DualQuaternion* conjugate, const BGC_FP64_DualQuaternion* quaternion);
extern inline int bgc_fp32_dual_quaternion_normalize(BGC_FP32_DualQuaternion* quaternion);
extern inline int bgc_fp64_dual_quaternion_normalize(BGC_FP64_DualQuaternion* quaternion);
extern inline int bgc_fp32_dual_quaternion_get_normalized(BGC_FP32_DualQuaternion* normalized, const BGC_FP32_DualQuaternion* quaternion);
extern inline int bgc_fp64_dual_quaternion_get_normalized(BGC_FP64_DualQuaternion* normalized, const BGC_FP64_DualQuaternion* quaternion);
extern inline void bgc_fp32_dual_quaternion_add(BGC_FP32_DualQuaternion* sum, const BGC_FP32_DualQuaternion* first, const BGC_FP32_DualQuaternion* second);
extern inline void bgc_fp64_dual_quaternion_add(BGC_FP64_DualQuaternion* sum, const BGC_FP64_DualQuaternion* first, const BGC_FP64_DualQuaternion* second);

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

Internal server error - Personal Git Server: Beyond coding. We Forge.

500

Internal server error

Forgejo version: 11.0.1+gitea-1.22.0

 @ -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);