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

basic-geometry/dual-quaternion.c

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

basic-geometry/dual-quaternion.h

@@ -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);
+inline int bgc_fp32_dual_quaternion_normalize(BGC_FP32_DualQuaternion* quaternion)
-
-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)
 {
     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)) {
+    if (square_magnitude <= BGC_FP64_SQUARE_EPSILON || isnan(square_magnitude)) {
-        _bgc_fp64_quaternion_normalize_normalize_parts(quaternion, 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_number(&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->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_number(&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->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_number(&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->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_number(&quotient->real_part, &dividend->real_part, multiplier);