diff --git a/basic-geometry/dual-quaternion.c b/basic-geometry/dual-quaternion.c index 35bb852..8a625b7 100644 --- a/basic-geometry/dual-quaternion.c +++ b/basic-geometry/dual-quaternion.c @@ -54,8 +54,14 @@ extern inline int bgc_fp64_dual_quaternion_divide_by_real_number(BGC_FP64_DualQu extern inline int bgc_fp32_dual_quaternion_divide_by_dual_number(BGC_FP32_DualQuaternion* quotient, const BGC_FP32_DualQuaternion* dividend, const BGC_FP32_DualNumber* divisor); extern inline int bgc_fp64_dual_quaternion_divide_by_dual_number(BGC_FP64_DualQuaternion* quotient, const BGC_FP64_DualQuaternion* dividend, const BGC_FP64_DualNumber* divisor); -extern inline int bgc_fp32_dual_quaternion_divide_by_conjugate_number(BGC_FP32_DualQuaternion* quotient, const BGC_FP32_DualQuaternion* dividend, const BGC_FP32_DualNumber* divisor_to_conjugate); -extern inline int bgc_fp64_dual_quaternion_divide_by_conjugate_number(BGC_FP64_DualQuaternion* quotient, const BGC_FP64_DualQuaternion* dividend, const BGC_FP64_DualNumber* divisor_to_conjugate); +extern inline int bgc_fp32_dual_quaternion_divide_by_conjugate_dual_number(BGC_FP32_DualQuaternion* quotient, const BGC_FP32_DualQuaternion* dividend, const BGC_FP32_DualNumber* divisor_to_conjugate); +extern inline int bgc_fp64_dual_quaternion_divide_by_conjugate_dual_number(BGC_FP64_DualQuaternion* quotient, const BGC_FP64_DualQuaternion* dividend, const BGC_FP64_DualNumber* divisor_to_conjugate); + +extern inline int bgc_fp32_dual_quaternion_divide_by_quaternion(BGC_FP32_DualQuaternion* quotient, const BGC_FP32_DualQuaternion* dividend, const BGC_FP32_Quaternion* divisor); +extern inline int bgc_fp64_dual_quaternion_divide_by_quaternion(BGC_FP64_DualQuaternion* quotient, const BGC_FP64_DualQuaternion* dividend, const BGC_FP64_Quaternion* divisor); + +extern inline int bgc_fp32_dual_quaternion_divide_by_conjugate_quaternion(BGC_FP32_DualQuaternion* quotient, const BGC_FP32_DualQuaternion* dividend, const BGC_FP32_Quaternion* divisor_to_conjugate); +extern inline int bgc_fp64_dual_quaternion_divide_by_conjugate_quaternion(BGC_FP64_DualQuaternion* quotient, const BGC_FP64_DualQuaternion* dividend, const BGC_FP64_Quaternion* divisor_to_conjugate); extern inline void bgc_fp32_dual_quaternion_get_mean2(BGC_FP32_DualQuaternion* mean, const BGC_FP32_DualQuaternion* quaternion1, const BGC_FP32_DualQuaternion* quaternion2); extern inline void bgc_fp64_dual_quaternion_get_mean2(BGC_FP64_DualQuaternion* mean, const BGC_FP64_DualQuaternion* quaternion1, const BGC_FP64_DualQuaternion* quaternion2); diff --git a/basic-geometry/dual-quaternion.h b/basic-geometry/dual-quaternion.h index 308d8a8..a666694 100644 --- a/basic-geometry/dual-quaternion.h +++ b/basic-geometry/dual-quaternion.h @@ -184,7 +184,7 @@ inline void bgc_fp64_dual_quaternion_multiply_by_dual_number(BGC_FP64_DualQuater // ===== Multiply by Conjugate Dual Number ====== // -inline void bgc_fp32_dual_quaternion_multiply_by_conjugate_number(BGC_FP32_DualQuaternion* product, const BGC_FP32_DualQuaternion* multiplicand, const BGC_FP32_DualNumber* multiplier_to_conjugate) +inline void bgc_fp32_dual_quaternion_multiply_by_conjugate_dual_number(BGC_FP32_DualQuaternion* product, const BGC_FP32_DualQuaternion* multiplicand, const BGC_FP32_DualNumber* multiplier_to_conjugate) { BGC_FP32_Quaternion dual_part; @@ -195,7 +195,7 @@ inline void bgc_fp32_dual_quaternion_multiply_by_conjugate_number(BGC_FP32_DualQ bgc_fp32_quaternion_copy(&product->dual_part, &dual_part); } -inline void bgc_fp64_dual_quaternion_multiply_by_conjugate_number(BGC_FP64_DualQuaternion* product, const BGC_FP64_DualQuaternion* multiplicand, const BGC_FP64_DualNumber* multiplier_to_conjugate) +inline void bgc_fp64_dual_quaternion_multiply_by_conjugate_dual_number(BGC_FP64_DualQuaternion* product, const BGC_FP64_DualQuaternion* multiplicand, const BGC_FP64_DualNumber* multiplier_to_conjugate) { BGC_FP64_Quaternion dual_part; @@ -258,6 +258,30 @@ inline void bgc_fp64_dual_quaternion_multiply_by_dual_quaternion(BGC_FP64_DualQu bgc_fp64_quaternion_add(&product->dual_part, &dual_part1, &dual_part2); } +// === Multiply by Conjugate Dual Quaternion ==== // + +inline void bgc_fp32_dual_quaternion_multiply_by_conjugate_dual_quaternion(BGC_FP32_DualQuaternion* product, const BGC_FP32_DualQuaternion* multiplicand, const BGC_FP32_DualQuaternion* multiplier_to_conjugate) +{ + BGC_FP32_Quaternion dual_part1, dual_part2; + + bgc_fp32_quaternion_multiply_by_conjugate(&dual_part1, &multiplicand->real_part, &multiplier_to_conjugate->dual_part); + bgc_fp32_quaternion_multiply_by_conjugate(&dual_part2, &multiplicand->dual_part, &multiplier_to_conjugate->real_part); + + bgc_fp32_quaternion_multiply_by_conjugate(&product->real_part, &multiplicand->real_part, &multiplier_to_conjugate->real_part); + bgc_fp32_quaternion_add(&product->dual_part, &dual_part1, &dual_part2); +} + +inline void bgc_fp64_dual_quaternion_multiply_by_conjugate_dual_quaternion(BGC_FP64_DualQuaternion* product, const BGC_FP64_DualQuaternion* multiplicand, const BGC_FP64_DualQuaternion* multiplier_to_conjugate) +{ + BGC_FP64_Quaternion dual_part1, dual_part2; + + bgc_fp64_quaternion_multiply_by_conjugate(&dual_part1, &multiplicand->real_part, &multiplier_to_conjugate->dual_part); + bgc_fp64_quaternion_multiply_by_conjugate(&dual_part2, &multiplicand->dual_part, &multiplier_to_conjugate->real_part); + + bgc_fp64_quaternion_multiply_by_quaternion(&product->real_part, &multiplicand->real_part, &multiplier_to_conjugate->real_part); + bgc_fp64_quaternion_add(&product->dual_part, &dual_part1, &dual_part2); +} + // =================== Divide =================== // inline int bgc_fp32_dual_quaternion_divide_by_real_number(BGC_FP32_DualQuaternion* quotient, const BGC_FP32_DualQuaternion* dividend, const float divisor) @@ -298,7 +322,7 @@ inline int bgc_fp32_dual_quaternion_divide_by_dual_number(BGC_FP32_DualQuaternio return BGC_FAILURE; } - bgc_fp32_dual_quaternion_multiply_by_conjugate_number(quotient, dividend, divisor); + bgc_fp32_dual_quaternion_multiply_by_conjugate_dual_number(quotient, dividend, divisor); bgc_fp32_dual_quaternion_multiply_by_real_number(quotient, quotient, 1.0f / square_modulus); return BGC_SUCCESS; @@ -312,7 +336,7 @@ inline int bgc_fp64_dual_quaternion_divide_by_dual_number(BGC_FP64_DualQuaternio return BGC_FAILURE; } - bgc_fp64_dual_quaternion_multiply_by_conjugate_number(quotient, dividend, divisor); + bgc_fp64_dual_quaternion_multiply_by_conjugate_dual_number(quotient, dividend, divisor); bgc_fp64_dual_quaternion_multiply_by_real_number(quotient, quotient, 1.0 / square_modulus); return BGC_SUCCESS; @@ -320,7 +344,7 @@ inline int bgc_fp64_dual_quaternion_divide_by_dual_number(BGC_FP64_DualQuaternio // ====== Divide by Conjugate Dual Number ======= // -inline int bgc_fp32_dual_quaternion_divide_by_conjugate_number(BGC_FP32_DualQuaternion* quotient, const BGC_FP32_DualQuaternion* dividend, const BGC_FP32_DualNumber* divisor_to_conjugate) +inline int bgc_fp32_dual_quaternion_divide_by_conjugate_dual_number(BGC_FP32_DualQuaternion* quotient, const BGC_FP32_DualQuaternion* dividend, const BGC_FP32_DualNumber* divisor_to_conjugate) { const float square_modulus = divisor_to_conjugate->real_part * divisor_to_conjugate->real_part; @@ -334,7 +358,7 @@ inline int bgc_fp32_dual_quaternion_divide_by_conjugate_number(BGC_FP32_DualQuat return BGC_SUCCESS; } -inline int bgc_fp64_dual_quaternion_divide_by_conjugate_number(BGC_FP64_DualQuaternion* quotient, const BGC_FP64_DualQuaternion* dividend, const BGC_FP64_DualNumber* divisor_to_conjugate) +inline int bgc_fp64_dual_quaternion_divide_by_conjugate_dual_number(BGC_FP64_DualQuaternion* quotient, const BGC_FP64_DualQuaternion* dividend, const BGC_FP64_DualNumber* divisor_to_conjugate) { const double square_modulus = divisor_to_conjugate->real_part * divisor_to_conjugate->real_part; @@ -348,6 +372,86 @@ inline int bgc_fp64_dual_quaternion_divide_by_conjugate_number(BGC_FP64_DualQuat return BGC_SUCCESS; } +// ======= Divide by (Regular) Quaternion ======= // + +inline int bgc_fp32_dual_quaternion_divide_by_quaternion(BGC_FP32_DualQuaternion* quotient, const BGC_FP32_DualQuaternion* dividend, const BGC_FP32_Quaternion* divisor) +{ + const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(divisor); + + if (square_magnitude <= BGC_FP32_SQUARE_EPSILON) { + return BGC_FAILURE; + } + + const float multiplier = 1.0f / square_magnitude; + + bgc_fp32_quaternion_multiply_by_conjugate("ient->real_part, ÷nd->real_part, divisor); + bgc_fp32_quaternion_multiply_by_conjugate("ient->dual_part, ÷nd->dual_part, divisor); + + bgc_fp32_quaternion_multiply_by_real("ient->real_part, "ient->real_part, multiplier); + bgc_fp32_quaternion_multiply_by_real("ient->dual_part, "ient->dual_part, multiplier); + + return BGC_SUCCESS; +} + +inline int bgc_fp64_dual_quaternion_divide_by_quaternion(BGC_FP64_DualQuaternion* quotient, const BGC_FP64_DualQuaternion* dividend, const BGC_FP64_Quaternion* divisor) +{ + const double square_magnitude = bgc_fp64_quaternion_get_square_magnitude(divisor); + + if (square_magnitude <= BGC_FP64_SQUARE_EPSILON) { + return BGC_FAILURE; + } + + const double multiplier = 1.0 / square_magnitude; + + bgc_fp64_quaternion_multiply_by_conjugate("ient->real_part, ÷nd->real_part, divisor); + bgc_fp64_quaternion_multiply_by_conjugate("ient->dual_part, ÷nd->dual_part, divisor); + + bgc_fp64_quaternion_multiply_by_real("ient->real_part, "ient->real_part, multiplier); + bgc_fp64_quaternion_multiply_by_real("ient->dual_part, "ient->dual_part, multiplier); + + return BGC_SUCCESS; +} + +// ======= Divide by Conjugate Quaternion ======= // + +inline int bgc_fp32_dual_quaternion_divide_by_conjugate_quaternion(BGC_FP32_DualQuaternion* quotient, const BGC_FP32_DualQuaternion* dividend, const BGC_FP32_Quaternion* divisor_to_conjugate) +{ + const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(divisor_to_conjugate); + + if (square_magnitude <= BGC_FP32_SQUARE_EPSILON) { + return BGC_FAILURE; + } + + const float multiplier = 1.0f / square_magnitude; + + bgc_fp32_quaternion_multiply_by_quaternion("ient->real_part, ÷nd->real_part, divisor_to_conjugate); + bgc_fp32_quaternion_multiply_by_quaternion("ient->dual_part, ÷nd->dual_part, divisor_to_conjugate); + + bgc_fp32_quaternion_multiply_by_real("ient->real_part, "ient->real_part, multiplier); + bgc_fp32_quaternion_multiply_by_real("ient->dual_part, "ient->dual_part, multiplier); + + return BGC_SUCCESS; +} + +inline int bgc_fp64_dual_quaternion_divide_by_conjugate_quaternion(BGC_FP64_DualQuaternion* quotient, const BGC_FP64_DualQuaternion* dividend, const BGC_FP64_Quaternion* divisor_to_conjugate) +{ + const double square_magnitude = bgc_fp64_quaternion_get_square_magnitude(divisor_to_conjugate); + + if (square_magnitude <= BGC_FP64_SQUARE_EPSILON) { + return BGC_FAILURE; + } + + const double multiplier = 1.0 / square_magnitude; + + bgc_fp64_quaternion_multiply_by_quaternion("ient->real_part, ÷nd->real_part, divisor_to_conjugate); + bgc_fp64_quaternion_multiply_by_quaternion("ient->dual_part, ÷nd->dual_part, divisor_to_conjugate); + + bgc_fp64_quaternion_multiply_by_real("ient->real_part, "ient->real_part, multiplier); + bgc_fp64_quaternion_multiply_by_real("ient->dual_part, "ient->dual_part, multiplier); + + return BGC_SUCCESS; +} + // ================ Mean of Two ================= // inline void bgc_fp32_dual_quaternion_get_mean2(BGC_FP32_DualQuaternion* mean, const BGC_FP32_DualQuaternion* quaternion1, const BGC_FP32_DualQuaternion* quaternion2)