diff --git a/basic-geometry/dual-quaternion.c b/basic-geometry/dual-quaternion.c index ab14d0f..659394e 100644 --- a/basic-geometry/dual-quaternion.c +++ b/basic-geometry/dual-quaternion.c @@ -18,6 +18,12 @@ extern inline void bgc_fp64_dual_quaternion_revert(BGC_FP64_DualQuaternion* quat extern inline void bgc_fp32_dual_quaternion_get_reverse(BGC_FP32_DualQuaternion* reverse, const BGC_FP32_DualQuaternion* quaternion); extern inline void bgc_fp64_dual_quaternion_get_reverse(BGC_FP64_DualQuaternion* reverse, const BGC_FP64_DualQuaternion* quaternion); +extern inline int bgc_fp32_dual_quaternion_invert(BGC_FP32_DualQuaternion* quaternion); +extern inline int bgc_fp64_dual_quaternion_invert(BGC_FP64_DualQuaternion* quaternion); + +extern inline int bgc_fp32_dual_quaternion_get_inverse(BGC_FP32_DualQuaternion* inverse, const BGC_FP32_DualQuaternion* quaternion); +extern inline int bgc_fp64_dual_quaternion_get_inverse(BGC_FP64_DualQuaternion* inverse, const BGC_FP64_DualQuaternion* quaternion); + extern inline void bgc_fp32_dual_quaternion_component_conjugate(BGC_FP32_DualQuaternion* quaternion); extern inline void bgc_fp64_dual_quaternion_component_conjugate(BGC_FP64_DualQuaternion* quaternion); diff --git a/basic-geometry/dual-quaternion.h b/basic-geometry/dual-quaternion.h index 4c9450d..36f008a 100644 --- a/basic-geometry/dual-quaternion.h +++ b/basic-geometry/dual-quaternion.h @@ -88,6 +88,74 @@ inline void bgc_fp64_dual_quaternion_get_reverse(BGC_FP64_DualQuaternion* revers bgc_fp64_quaternion_get_reverse(&reverse->dual_part, &quaternion->dual_part); } +// =================== Invert =================== // + +inline int bgc_fp32_dual_quaternion_invert(BGC_FP32_DualQuaternion* quaternion) +{ + if (bgc_fp32_quaternion_get_inverse(&quaternion->real_part, &quaternion->real_part) !== BGC_SUCCESS) { + return BGC_FAILURE; + } + + BGC_FP32_Quaternion temporary; + + bgc_fp32_quaternion_multiply_by_quaternion(&temporary, &quaternion->real_part, &quaternion->dual_part); + bgc_fp32_quaternion_multiply_by_quaternion(&quaternion->dual_part, &temporary, &quaternion->real_part); + + bgc_fp32_quaternion_revert(&quaternion->dual_part); + + return BGC_SUCCESS; +} + +inline int bgc_fp64_dual_quaternion_invert(BGC_FP64_DualQuaternion* quaternion) +{ + if (bgc_fp64_quaternion_get_inverse(&quaternion->real_part, &quaternion->real_part) !== BGC_SUCCESS) { + return BGC_FAILURE; + } + + BGC_FP64_Quaternion temporary; + + bgc_fp64_quaternion_multiply_by_quaternion(&temporary, &quaternion->real_part, &quaternion->dual_part); + bgc_fp64_quaternion_multiply_by_quaternion(&quaternion->dual_part, &temporary, &quaternion->real_part); + + bgc_fp64_quaternion_revert(&quaternion->dual_part); + + return BGC_SUCCESS; +} + +// ================ Get Inverse ================= // + +inline int bgc_fp32_dual_quaternion_get_inverse(BGC_FP32_DualQuaternion* inverse, const BGC_FP32_DualQuaternion* quaternion) +{ + if (bgc_fp32_quaternion_get_inverse(&inverse->real_part, &quaternion->real_part) !== BGC_SUCCESS) { + return BGC_FAILURE; + } + + BGC_FP32_Quaternion temporary; + + bgc_fp32_quaternion_multiply_by_quaternion(&temporary, &inverse->real_part, &quaternion->dual_part); + bgc_fp32_quaternion_multiply_by_quaternion(&inverse->dual_part, &temporary, &inverse->real_part); + + bgc_fp32_quaternion_revert(&inverse->dual_part); + + return BGC_SUCCESS; +} + +inline int bgc_fp64_dual_quaternion_get_inverse(BGC_FP64_DualQuaternion* inverse, const BGC_FP64_DualQuaternion* quaternion) +{ + if (bgc_fp64_quaternion_get_inverse(&inverse->real_part, &quaternion->real_part) !== BGC_SUCCESS) { + return BGC_FAILURE; + } + + BGC_FP64_Quaternion temporary; + + bgc_fp64_quaternion_multiply_by_quaternion(&temporary, &inverse->real_part, &quaternion->dual_part); + bgc_fp64_quaternion_multiply_by_quaternion(&inverse->dual_part, &temporary, &inverse->real_part); + + bgc_fp64_quaternion_revert(&inverse->dual_part); + + return BGC_SUCCESS; +} + // ============ Component Conjugate ============= // inline void bgc_fp32_dual_quaternion_component_conjugate(BGC_FP32_DualQuaternion* quaternion) @@ -132,13 +200,13 @@ inline void bgc_fp64_dual_quaternion_dual_conjugate(BGC_FP64_DualQuaternion* qua inline void bgc_fp32_dual_quaternion_get_dual_conjugate(BGC_FP32_DualQuaternion* conjugate, const BGC_FP32_DualQuaternion* quaternion) { - bgc_fp32_quaternion_copy(&conjugate->real_part, &quaternion->dual_part); + bgc_fp32_quaternion_copy(&conjugate->real_part, &quaternion->real_part); bgc_fp32_quaternion_get_reverse(&conjugate->real_part, &quaternion->dual_part); } inline void bgc_fp64_dual_quaternion_get_dual_conjugate(BGC_FP64_DualQuaternion* conjugate, const BGC_FP64_DualQuaternion* quaternion) { - bgc_fp64_quaternion_copy(&conjugate->real_part, &quaternion->dual_part); + bgc_fp64_quaternion_copy(&conjugate->real_part, &quaternion->real_part); bgc_fp64_quaternion_get_reverse(&conjugate->real_part, &quaternion->dual_part); }