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

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);
-    bgc_fp64_quaternion_multiply_by_real(&quotient->dual_part, &dividend->dual_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_number(&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->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_number(&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->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_number(&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->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_number(&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->dual_part, &quotient->dual_part, multiplier);
 
     return BGC_SUCCESS;
 }

basic-geometry/quaternion.c

@@ -43,8 +43,8 @@ extern inline void bgc_fp64_quaternion_subtract(BGC_FP64_Quaternion* difference,
 extern inline void bgc_fp32_quaternion_subtract_scaled(BGC_FP32_Quaternion* difference, const BGC_FP32_Quaternion* basic_quaternion, const BGC_FP32_Quaternion* scalable_quaternion, const float scale);
 extern inline void bgc_fp64_quaternion_subtract_scaled(BGC_FP64_Quaternion* difference, const BGC_FP64_Quaternion* basic_quaternion, const BGC_FP64_Quaternion* scalable_quaternion, const double scale);
 
-extern inline void bgc_fp32_quaternion_multiply_by_real(BGC_FP32_Quaternion* product, const BGC_FP32_Quaternion* multiplicand, const float multiplier);
+extern inline void bgc_fp32_quaternion_multiply_by_real_number(BGC_FP32_Quaternion* product, const BGC_FP32_Quaternion* multiplicand, const float multiplier);
-extern inline void bgc_fp64_quaternion_multiply_by_real(BGC_FP64_Quaternion* product, const BGC_FP64_Quaternion* multiplicand, const double multiplier);
+extern inline void bgc_fp64_quaternion_multiply_by_real_number(BGC_FP64_Quaternion* product, const BGC_FP64_Quaternion* multiplicand, const double multiplier);
 
 extern inline void bgc_fp32_quaternion_multiply_by_quaternion(BGC_FP32_Quaternion* product, const BGC_FP32_Quaternion* left, const BGC_FP32_Quaternion* right);
 extern inline void bgc_fp64_quaternion_multiply_by_quaternion(BGC_FP64_Quaternion* product, const BGC_FP64_Quaternion* left, const BGC_FP64_Quaternion* right);

basic-geometry/quaternion.h

@@ -258,7 +258,7 @@ inline void bgc_fp64_quaternion_subtract_scaled(BGC_FP64_Quaternion* difference,
 
 // ============= Multiply By Number ============= //
 
-inline void bgc_fp32_quaternion_multiply_by_real(BGC_FP32_Quaternion* product, const BGC_FP32_Quaternion* multiplicand, const float multiplier)
+inline void bgc_fp32_quaternion_multiply_by_real_number(BGC_FP32_Quaternion* product, const BGC_FP32_Quaternion* multiplicand, const float multiplier)
 {
     product->s0 = multiplicand->s0 * multiplier;
     product->x1 = multiplicand->x1 * multiplier;
@@ -266,7 +266,7 @@ inline void bgc_fp32_quaternion_multiply_by_real(BGC_FP32_Quaternion* product, c
     product->x3 = multiplicand->x3 * multiplier;
 }
 
-inline void bgc_fp64_quaternion_multiply_by_real(BGC_FP64_Quaternion* product, const BGC_FP64_Quaternion* multiplicand, const double multiplier)
+inline void bgc_fp64_quaternion_multiply_by_real_number(BGC_FP64_Quaternion* product, const BGC_FP64_Quaternion* multiplicand, const double multiplier)
 {
     product->s0 = multiplicand->s0 * multiplier;
     product->x1 = multiplicand->x1 * multiplier;
@@ -370,14 +370,14 @@ inline void _bgc_fp64_restrict_quaternion_multiply_by_conjugate(BGC_FP64_Quatern
 
 inline void bgc_fp32_quaternion_multiply_by_dual_number(BGC_FP32_DualQuaternion* product, const BGC_FP32_Quaternion* multiplicand, const BGC_FP32_DualNumber* multiplier)
 {
-    bgc_fp32_quaternion_multiply_by_real(&product->real_part, multiplicand, multiplier->real_part);
+    bgc_fp32_quaternion_multiply_by_real_number(&product->real_part, multiplicand, multiplier->real_part);
-    bgc_fp32_quaternion_multiply_by_real(&product->dual_part, multiplicand, multiplier->dual_part);
+    bgc_fp32_quaternion_multiply_by_real_number(&product->dual_part, multiplicand, multiplier->dual_part);
 }
 
 inline void bgc_fp64_quaternion_multiply_by_dual_number(BGC_FP64_DualQuaternion* product, const BGC_FP64_Quaternion* multiplicand, const BGC_FP64_DualNumber* multiplier)
 {
-    bgc_fp64_quaternion_multiply_by_real(&product->real_part, multiplicand, multiplier->real_part);
+    bgc_fp64_quaternion_multiply_by_real_number(&product->real_part, multiplicand, multiplier->real_part);
-    bgc_fp64_quaternion_multiply_by_real(&product->dual_part, multiplicand, multiplier->dual_part);
+    bgc_fp64_quaternion_multiply_by_real_number(&product->dual_part, multiplicand, multiplier->dual_part);
 }
 
 // ======== Multiply By Dual Quaternion ========= //
@@ -402,7 +402,7 @@ inline int bgc_fp32_quaternion_divide_by_real(BGC_FP32_Quaternion* quotient, con
         return BGC_FAILURE;
     }
 
-    bgc_fp32_quaternion_multiply_by_real(quotient, dividend, 1.0f / divisor);
+    bgc_fp32_quaternion_multiply_by_real_number(quotient, dividend, 1.0f / divisor);
 
     return BGC_SUCCESS;
 }
@@ -413,7 +413,7 @@ inline int bgc_fp64_quaternion_divide_by_real(BGC_FP64_Quaternion* quotient, con
         return BGC_FAILURE;
     }
 
-    bgc_fp64_quaternion_multiply_by_real(quotient, dividend, 1.0 / divisor);
+    bgc_fp64_quaternion_multiply_by_real_number(quotient, dividend, 1.0 / divisor);
 
     return BGC_SUCCESS;
 }
@@ -429,7 +429,7 @@ inline int bgc_fp32_quaternion_divide_by_quaternion(BGC_FP32_Quaternion* quotien
     }
 
     bgc_fp32_quaternion_multiply_by_conjugate(quotient, divident, divisor);
-    bgc_fp32_quaternion_multiply_by_real(quotient, quotient, 1.0f / square_modulus);
+    bgc_fp32_quaternion_multiply_by_real_number(quotient, quotient, 1.0f / square_modulus);
 
     return BGC_SUCCESS;
 }
@@ -443,7 +443,7 @@ inline int bgc_fp64_quaternion_divide_by_quaternion(BGC_FP64_Quaternion* quotien
     }
 
     bgc_fp64_quaternion_multiply_by_conjugate(quotient, divident, divisor);
-    bgc_fp64_quaternion_multiply_by_real(quotient, quotient, 1.0 / square_modulus);
+    bgc_fp64_quaternion_multiply_by_real_number(quotient, quotient, 1.0 / square_modulus);
 
     return BGC_SUCCESS;
 }
@@ -459,7 +459,7 @@ inline int bgc_fp32_quaternion_divide_by_conjugate(BGC_FP32_Quaternion* quotient
     }
 
     bgc_fp32_quaternion_multiply_by_quaternion(quotient, divisor_to_conjugate, divisor_to_conjugate);
-    bgc_fp32_quaternion_multiply_by_real(quotient, quotient, 1.0f / square_modulus);
+    bgc_fp32_quaternion_multiply_by_real_number(quotient, quotient, 1.0f / square_modulus);
 
     return BGC_SUCCESS;
 }
@@ -473,7 +473,7 @@ inline int bgc_fp64_quaternion_divide_by_conjugate(BGC_FP64_Quaternion* quotient
     }
 
     bgc_fp64_quaternion_multiply_by_quaternion(quotient, divisor_to_conjugate, divisor_to_conjugate);
-    bgc_fp64_quaternion_multiply_by_real(quotient, quotient, 1.0 / square_modulus);
+    bgc_fp64_quaternion_multiply_by_real_number(quotient, quotient, 1.0 / square_modulus);
 
     return BGC_SUCCESS;
 }
@@ -722,7 +722,7 @@ inline int bgc_fp32_quaternion_get_normalized(BGC_FP32_Quaternion* normalized, c
         return BGC_SUCCESS;
     }
 
-    bgc_fp32_quaternion_multiply_by_real(normalized, quaternion, sqrtf(1.0f / square_modulus));
+    bgc_fp32_quaternion_multiply_by_real_number(normalized, quaternion, sqrtf(1.0f / square_modulus));
     return BGC_SUCCESS;
 }
 
@@ -740,7 +740,7 @@ inline int bgc_fp64_quaternion_get_normalized(BGC_FP64_Quaternion* normalized, c
         return BGC_SUCCESS;
     }
 
-    bgc_fp64_quaternion_multiply_by_real(normalized, quaternion, sqrt(1.0 / square_modulus));
+    bgc_fp64_quaternion_multiply_by_real_number(normalized, quaternion, sqrt(1.0 / square_modulus));
     return BGC_SUCCESS;
 }
 

basic-geometry/rigid-pose3.c

@@ -1,34 +1,5 @@
 #include "rigid-pose3.h"
 
-void _bgc_fp32_rigid_pose3_normalize_parts(BGC_FP32_RigidPose3* pose, const float square_magnitude)
-{
-    if (square_magnitude <= BGC_FP32_SQUARE_EPSILON || isnan(square_magnitude)) {
-        bgc_fp32_rigid_pose3_reset(pose);
-        return;
-    }
-
-    const float multiplier = sqrtf(1.0f / square_magnitude);
-
-    bgc_fp32_quaternion_multiply_by_real(&pose->_real_part, &pose->_real_part, multiplier);
-    bgc_fp32_quaternion_multiply_by_real(&pose->_dual_part, &pose->_dual_part, multiplier);
-}
-
-void _bgc_fp64_rigid_pose3_normalize_parts(BGC_FP64_RigidPose3* pose, const double square_magnitude)
-{
-    if (square_magnitude <= BGC_FP64_SQUARE_EPSILON || isnan(square_magnitude)) {
-        bgc_fp64_rigid_pose3_reset(pose);
-        return;
-    }
-
-    const double multiplier = sqrt(1.0 / square_magnitude);
-
-    bgc_fp64_quaternion_multiply_by_real(&pose->_real_part, &pose->_real_part, multiplier);
-    bgc_fp64_quaternion_multiply_by_real(&pose->_dual_part, &pose->_dual_part, multiplier);
-}
-
-extern inline void _bgc_fp32_rigid_pose3_normalize(BGC_FP32_RigidPose3* pose);
-extern inline void _bgc_fp64_rigid_pose3_normalize(BGC_FP64_RigidPose3* pose);
-
 extern inline void bgc_fp32_rigid_pose3_reset(BGC_FP32_RigidPose3* pose);
 extern inline void bgc_fp64_rigid_pose3_reset(BGC_FP64_RigidPose3* pose);
 

basic-geometry/rigid-pose3.h

@@ -1,112 +1,117 @@
 #ifndef _BGC_RIGID_POSE3_H_INCLUDED_
 #define _BGC_RIGID_POSE3_H_INCLUDED_
 
+#include <math.h>
+
 #include "types.h"
 #include "quaternion.h"
-
+#include "dual-quaternion.h"
-// ================= Normalize ================== //
-
-void _bgc_fp32_rigid_pose3_normalize_parts(BGC_FP32_RigidPose3* pose, const float square_magnitude);
-
-void _bgc_fp64_rigid_pose3_normalize_parts(BGC_FP64_RigidPose3* pose, const double square_magnitude);
-
-inline void _bgc_fp32_rigid_pose3_normalize(BGC_FP32_RigidPose3* pose)
-{
-    const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(&pose->_real_part);
-
-    if (!bgc_fp32_is_square_unit(square_magnitude)) {
-        _bgc_fp32_rigid_pose3_normalize_parts(pose, square_magnitude);
-    }
-
-    const float dot_product = bgc_fp32_quaternion_get_dot_product(&pose->_real_part, &pose->_dual_part);
-
-    bgc_fp32_quaternion_subtract_scaled(&pose->_dual_part, &pose->_dual_part, &pose->_real_part, dot_product);
-}
-
-inline void _bgc_fp64_rigid_pose3_normalize(BGC_FP64_RigidPose3* pose)
-{
-    const double square_magnitude = bgc_fp64_quaternion_get_square_magnitude(&pose->_real_part);
-
-    if (!bgc_fp64_is_square_unit(square_magnitude)) {
-        _bgc_fp64_rigid_pose3_normalize_parts(pose, square_magnitude);
-    }
-
-    const double dot_product = bgc_fp64_quaternion_get_dot_product(&pose->_real_part, &pose->_dual_part);
-
-    bgc_fp64_quaternion_subtract_scaled(&pose->_dual_part, &pose->_dual_part, &pose->_real_part, dot_product);
-}
 
 // ==================== Reset =================== //
 
 inline void bgc_fp32_rigid_pose3_reset(BGC_FP32_RigidPose3* pose)
 {
-    pose->_real_part.s0 = 1.0f;
+    pose->_versor.real_part.s0 = 1.0f;
-    pose->_real_part.x1 = 0.0f;
+    pose->_versor.real_part.x1 = 0.0f;
-    pose->_real_part.x2 = 0.0f;
+    pose->_versor.real_part.x2 = 0.0f;
-    pose->_real_part.x3 = 0.0f;
+    pose->_versor.real_part.x3 = 0.0f;
 
-    pose->_dual_part.s0 = 0.0f;
+    pose->_versor.dual_part.s0 = 0.0f;
-    pose->_dual_part.x1 = 0.0f;
+    pose->_versor.dual_part.x1 = 0.0f;
-    pose->_dual_part.x2 = 0.0f;
+    pose->_versor.dual_part.x2 = 0.0f;
-    pose->_dual_part.x3 = 0.0f;
+    pose->_versor.dual_part.x3 = 0.0f;
 }
 
 inline void bgc_fp64_rigid_pose3_reset(BGC_FP64_RigidPose3* pose)
 {
-    pose->_real_part.s0 = 1.0;
+    pose->_versor.real_part.s0 = 1.0;
-    pose->_real_part.x1 = 0.0;
+    pose->_versor.real_part.x1 = 0.0;
-    pose->_real_part.x2 = 0.0;
+    pose->_versor.real_part.x2 = 0.0;
-    pose->_real_part.x3 = 0.0;
+    pose->_versor.real_part.x3 = 0.0;
 
-    pose->_dual_part.s0 = 0.0;
+    pose->_versor.dual_part.s0 = 0.0;
-    pose->_dual_part.x1 = 0.0;
+    pose->_versor.dual_part.x1 = 0.0;
-    pose->_dual_part.x2 = 0.0;
+    pose->_versor.dual_part.x2 = 0.0;
-    pose->_dual_part.x3 = 0.0;
+    pose->_versor.dual_part.x3 = 0.0;
+}
+
+// ================= Normalize ================== //
+
+inline void _bgc_fp32_rigid_pose3_normalize(BGC_FP32_RigidPose3* pose)
+{
+    const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(&pose->_versor.real_part);
+
+    if (square_magnitude <= BGC_FP32_SQUARE_EPSILON || isnan(square_magnitude)) {
+        bgc_fp32_rigid_pose3_reset(pose);
+        return;
+    }
+
+    if (!bgc_fp32_is_square_unit(square_magnitude)) {
+        const float multiplier = sqrtf(1.0f / square_magnitude);
+
+        bgc_fp32_dual_quaternion_multiply_by_real_number(&pose->_versor, &pose->_versor, multiplier);
+    }
+
+    const float dot_product = bgc_fp32_quaternion_get_dot_product(&pose->_versor.real_part, &pose->_versor.dual_part);
+
+    bgc_fp32_quaternion_subtract_scaled(&pose->_versor.dual_part, &pose->_versor.dual_part, &pose->_versor.real_part, dot_product);
+}
+
+inline void _bgc_fp64_rigid_pose3_normalize(BGC_FP64_RigidPose3* pose)
+{
+    const double square_magnitude = bgc_fp64_quaternion_get_square_magnitude(&pose->_versor.real_part);
+
+    if (square_magnitude <= BGC_FP64_SQUARE_EPSILON || isnan(square_magnitude)) {
+        bgc_fp64_rigid_pose3_reset(pose);
+        return;
+    }
+
+    if (!bgc_fp64_is_square_unit(square_magnitude)) {
+        const double multiplier = sqrt(1.0 / square_magnitude);
+
+        bgc_fp64_dual_quaternion_multiply_by_real_number(&pose->_versor, &pose->_versor, multiplier);
+    }
+
+    const double dot_product = bgc_fp64_quaternion_get_dot_product(&pose->_versor.real_part, &pose->_versor.dual_part);
+
+    bgc_fp64_quaternion_subtract_scaled(&pose->_versor.dual_part, &pose->_versor.dual_part, &pose->_versor.real_part, dot_product);
 }
 
 // ==================== Copy ==================== //
 
 inline void bgc_fp32_rigid_pose3_copy(BGC_FP32_RigidPose3* destination, const BGC_FP32_RigidPose3* source)
 {
-    bgc_fp32_quaternion_copy(&destination->_real_part, &source->_real_part);
+    bgc_fp32_dual_quaternion_copy(&destination->_versor, &source->_versor);
-    bgc_fp32_quaternion_copy(&destination->_dual_part, &source->_dual_part);
 }
 
 inline void bgc_fp64_rigid_pose3_copy(BGC_FP64_RigidPose3* destination, const BGC_FP64_RigidPose3* source)
 {
-    bgc_fp64_quaternion_copy(&destination->_real_part, &source->_real_part);
+    bgc_fp64_dual_quaternion_copy(&destination->_versor, &source->_versor);
-    bgc_fp64_quaternion_copy(&destination->_dual_part, &source->_dual_part);
 }
 
 // ==================== Swap ==================== //
 
 inline void bgc_fp32_rigid_pose3_swap(BGC_FP32_RigidPose3* pose1, BGC_FP32_RigidPose3* pose2)
 {
-    bgc_fp32_quaternion_swap(&pose1->_real_part, &pose2->_real_part);
+    bgc_fp32_dual_quaternion_swap(&pose1->_versor, &pose2->_versor);
-    bgc_fp32_quaternion_swap(&pose1->_dual_part, &pose2->_dual_part);
 }
 
 inline void bgc_fp64_rigid_pose3_swap(BGC_FP64_RigidPose3* pose1, BGC_FP64_RigidPose3* pose2)
 {
-    bgc_fp64_quaternion_swap(&pose1->_real_part, &pose2->_real_part);
+    bgc_fp64_dual_quaternion_swap(&pose1->_versor, &pose2->_versor);
-    bgc_fp64_quaternion_swap(&pose1->_dual_part, &pose2->_dual_part);
 }
 
 // ================== Convert =================== //
 
 inline void bgc_fp32_rigid_pose3_convert_to_fp64(BGC_FP64_RigidPose3* destination, const BGC_FP32_RigidPose3* source)
 {
-    bgc_fp32_quaternion_convert_to_fp64(&destination->_real_part, &source->_real_part);
+    bgc_fp32_dual_quaternion_convert_to_fp64(&destination->_versor, &source->_versor);
-    bgc_fp32_quaternion_convert_to_fp64(&destination->_dual_part, &source->_dual_part);
-
     _bgc_fp64_rigid_pose3_normalize(destination);
 }
 
 inline void bgc_fp64_rigid_pose3_convert_to_fp32(BGC_FP32_RigidPose3* destination, const BGC_FP64_RigidPose3* source)
 {
-    bgc_fp64_quaternion_convert_to_fp32(&destination->_real_part, &source->_real_part);
+    bgc_fp64_dual_quaternion_convert_to_fp32(&destination->_versor, &source->_versor);
-    bgc_fp64_quaternion_convert_to_fp32(&destination->_dual_part, &source->_dual_part);
-
     _bgc_fp32_rigid_pose3_normalize(destination);
 }
 
@@ -114,27 +119,13 @@ inline void bgc_fp64_rigid_pose3_convert_to_fp32(BGC_FP32_RigidPose3* destinatio
 
 inline void bgc_fp32_rigid_pose3_combine(BGC_FP32_RigidPose3* combination, const BGC_FP32_RigidPose3* first, const BGC_FP32_RigidPose3* second)
 {
-    BGC_FP32_Quaternion dual_part1, dual_part2;
+    bgc_fp32_dual_quaternion_multiply_by_dual_quaternion(&combination->_versor, &second->_versor, &first->_versor);
-
-    _bgc_fp32_restrict_quaternion_multiply_by_quaternion(&dual_part1, &second->_real_part, &first->_dual_part);
-    _bgc_fp32_restrict_quaternion_multiply_by_quaternion(&dual_part2, &second->_dual_part, &first->_real_part);
-
-    bgc_fp32_quaternion_multiply_by_quaternion(&combination->_real_part, &second->_real_part, &first->_real_part);
-    bgc_fp32_quaternion_add(&combination->_dual_part, &dual_part1, &dual_part2);
-
     _bgc_fp32_rigid_pose3_normalize(combination);
 }
 
 inline void bgc_fp64_rigid_pose3_combine(BGC_FP64_RigidPose3* combination, const BGC_FP64_RigidPose3* first, const BGC_FP64_RigidPose3* second)
 {
-    BGC_FP64_Quaternion dual_part1, dual_part2;
+    bgc_fp64_dual_quaternion_multiply_by_dual_quaternion(&combination->_versor, &second->_versor, &first->_versor);
-
-    _bgc_fp64_restrict_quaternion_multiply_by_quaternion(&dual_part1, &second->_real_part, &first->_dual_part);
-    _bgc_fp64_restrict_quaternion_multiply_by_quaternion(&dual_part2, &second->_dual_part, &first->_real_part);
-
-    bgc_fp64_quaternion_multiply_by_quaternion(&combination->_real_part, &second->_real_part, &first->_real_part);
-    bgc_fp64_quaternion_add(&combination->_dual_part, &dual_part1, &dual_part2);
-
     _bgc_fp64_rigid_pose3_normalize(combination);
 }
 

basic-geometry/slerp3.h

@@ -94,7 +94,7 @@ inline void bgc_fp32_slerp3_get_phase_turn(BGC_FP32_Turn3* turn, const BGC_FP32_
 
     BGC_FP32_Quaternion q;
 
-    bgc_fp32_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine);
+    bgc_fp32_quaternion_multiply_by_real_number(&q, &slerp->_cosine_weight, cosine);
     bgc_fp32_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine);
 
     bgc_fp32_turn3_set_quaternion(turn, &q);
@@ -108,7 +108,7 @@ inline void bgc_fp64_slerp3_get_phase_turn(BGC_FP64_Turn3* turn, const BGC_FP64_
 
     BGC_FP64_Quaternion q;
 
-    bgc_fp64_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine);
+    bgc_fp64_quaternion_multiply_by_real_number(&q, &slerp->_cosine_weight, cosine);
     bgc_fp64_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine);
 
     bgc_fp64_turn3_set_quaternion(turn, &q);
@@ -124,7 +124,7 @@ inline void bgc_fp32_slerp3_get_phase_rotation_matrix(BGC_FP32_Matrix3x3* rotati
 
     BGC_FP32_Quaternion q;
 
-    bgc_fp32_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine);
+    bgc_fp32_quaternion_multiply_by_real_number(&q, &slerp->_cosine_weight, cosine);
     bgc_fp32_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine);
 
     bgc_fp32_quaternion_get_rotation_matrix(rotation_matrix, &q);
@@ -138,7 +138,7 @@ inline void bgc_fp64_slerp3_get_phase_rotation_matrix(BGC_FP64_Matrix3x3* rotati
 
     BGC_FP64_Quaternion q;
 
-    bgc_fp64_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine);
+    bgc_fp64_quaternion_multiply_by_real_number(&q, &slerp->_cosine_weight, cosine);
     bgc_fp64_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine);
 
     bgc_fp64_quaternion_get_rotation_matrix(rotation_matrix, &q);
@@ -154,7 +154,7 @@ inline void bgc_fp32_slerp3_get_phase_reverse_matrix(BGC_FP32_Matrix3x3* reverse
 
     BGC_FP32_Quaternion q;
 
-    bgc_fp32_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine);
+    bgc_fp32_quaternion_multiply_by_real_number(&q, &slerp->_cosine_weight, cosine);
     bgc_fp32_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine);
 
     bgc_fp32_quaternion_get_reverse_matrix(reverse_matrix, &q);
@@ -168,7 +168,7 @@ inline void bgc_fp64_slerp3_get_phase_reverse_matrix(BGC_FP64_Matrix3x3* reverse
 
     BGC_FP64_Quaternion q;
 
-    bgc_fp64_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine);
+    bgc_fp64_quaternion_multiply_by_real_number(&q, &slerp->_cosine_weight, cosine);
     bgc_fp64_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine);
 
     bgc_fp64_quaternion_get_reverse_matrix(reverse_matrix, &q);
@@ -184,7 +184,7 @@ inline void bgc_fp32_slerp3_get_phase_both_matrices(BGC_FP32_Matrix3x3* rotation
 
     BGC_FP32_Quaternion q;
 
-    bgc_fp32_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine);
+    bgc_fp32_quaternion_multiply_by_real_number(&q, &slerp->_cosine_weight, cosine);
     bgc_fp32_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine);
 
     bgc_fp32_quaternion_get_both_matrices(rotation_matrix, reverse_matrix, &q);
@@ -198,7 +198,7 @@ inline void bgc_fp64_slerp3_get_phase_both_matrices(BGC_FP64_Matrix3x3* rotation
 
     BGC_FP64_Quaternion q;
 
-    bgc_fp64_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine);
+    bgc_fp64_quaternion_multiply_by_real_number(&q, &slerp->_cosine_weight, cosine);
     bgc_fp64_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine);
 
     bgc_fp64_quaternion_get_both_matrices(rotation_matrix, reverse_matrix, &q);

basic-geometry/turn3.c

@@ -86,7 +86,7 @@ void _bgc_fp32_turn3_normalize(BGC_FP32_Turn3* turn, const float square_modulus)
         return;
     }
 
-    bgc_fp32_quaternion_multiply_by_real(&turn->_versor, &turn->_versor, sqrtf(1.0f / square_modulus));
+    bgc_fp32_quaternion_multiply_by_real_number(&turn->_versor, &turn->_versor, sqrtf(1.0f / square_modulus));
 }
 
 void _bgc_fp64_turn3_normalize(BGC_FP64_Turn3* turn, const double square_modulus)
@@ -96,7 +96,7 @@ void _bgc_fp64_turn3_normalize(BGC_FP64_Turn3* turn, const double square_modulus
         return;
     }
 
-    bgc_fp64_quaternion_multiply_by_real(&turn->_versor, &turn->_versor, sqrt(1.0 / square_modulus));
+    bgc_fp64_quaternion_multiply_by_real_number(&turn->_versor, &turn->_versor, sqrt(1.0 / square_modulus));
 }
 
 

basic-geometry/types.h

@@ -214,11 +214,11 @@ typedef struct {
 // ================ Rigid Pose3 ================= //
 
 typedef struct {
-    BGC_FP32_Quaternion _real_part, _dual_part;
+    BGC_FP32_DualQuaternion _versor;
 } BGC_FP32_RigidPose3;
 
 typedef struct {
-    BGC_FP64_Quaternion _real_part, _dual_part;
+    BGC_FP64_DualQuaternion _versor;
 } BGC_FP64_RigidPose3;
 
 #endif