diff --git a/basic-geometry/basic-geometry.vcxproj b/basic-geometry/basic-geometry.vcxproj index 5bbc782..6c03bb8 100644 --- a/basic-geometry/basic-geometry.vcxproj +++ b/basic-geometry/basic-geometry.vcxproj @@ -31,7 +31,7 @@ - + @@ -53,7 +53,7 @@ - + diff --git a/basic-geometry/basic-geometry.vcxproj.filters b/basic-geometry/basic-geometry.vcxproj.filters index 49d9826..5ddf03b 100644 --- a/basic-geometry/basic-geometry.vcxproj.filters +++ b/basic-geometry/basic-geometry.vcxproj.filters @@ -84,7 +84,7 @@ Файлы заголовков - + Файлы заголовков @@ -146,7 +146,7 @@ Исходные файлы - + Исходные файлы diff --git a/basic-geometry/dual-quaternion.h b/basic-geometry/dual-quaternion.h index 273657b..28f9e7d 100644 --- a/basic-geometry/dual-quaternion.h +++ b/basic-geometry/dual-quaternion.h @@ -410,8 +410,8 @@ inline void bgc_fp32_dual_quaternion_multiply_by_dual_quaternion(BGC_FP32_DualQu { BGC_FP32_Quaternion dual_part1, dual_part2; - bgc_fp32_quaternion_multiply_by_quaternion(&dual_part1, &left->real_part, &right->dual_part); - bgc_fp32_quaternion_multiply_by_quaternion(&dual_part2, &left->dual_part, &right->real_part); + _bgc_fp32_restrict_quaternion_multiply_by_quaternion(&dual_part1, &left->real_part, &right->dual_part); + _bgc_fp32_restrict_quaternion_multiply_by_quaternion(&dual_part2, &left->dual_part, &right->real_part); bgc_fp32_quaternion_multiply_by_quaternion(&product->real_part, &left->real_part, &right->real_part); bgc_fp32_quaternion_add(&product->dual_part, &dual_part1, &dual_part2); diff --git a/basic-geometry/quaternion.c b/basic-geometry/quaternion.c index dd9fac2..5c76579 100644 --- a/basic-geometry/quaternion.c +++ b/basic-geometry/quaternion.c @@ -58,6 +58,12 @@ extern inline void bgc_fp64_quaternion_multiply_by_dual_quaternion(BGC_FP64_Dual extern inline void bgc_fp32_quaternion_multiply_by_conjugate(BGC_FP32_Quaternion* product, const BGC_FP32_Quaternion* left, const BGC_FP32_Quaternion* right); extern inline void bgc_fp64_quaternion_multiply_by_conjugate(BGC_FP64_Quaternion* product, const BGC_FP64_Quaternion* left, const BGC_FP64_Quaternion* right); +extern inline void _bgc_fp32_restrict_quaternion_multiply_by_quaternion(BGC_FP32_Quaternion* restrict product, const BGC_FP32_Quaternion* left, const BGC_FP32_Quaternion* right); +extern inline void _bgc_fp64_restrict_quaternion_multiply_by_quaternion(BGC_FP64_Quaternion* restrict product, const BGC_FP64_Quaternion* left, const BGC_FP64_Quaternion* right); + +extern inline void _bgc_fp32_restrict_quaternion_multiply_by_conjugate(BGC_FP32_Quaternion* restrict product, const BGC_FP32_Quaternion* left, const BGC_FP32_Quaternion* right); +extern inline void _bgc_fp64_restrict_quaternion_multiply_by_conjugate(BGC_FP64_Quaternion* restrict product, const BGC_FP64_Quaternion* left, const BGC_FP64_Quaternion* right); + extern inline int bgc_fp32_quaternion_divide_by_real(BGC_FP32_Quaternion* quotient, const BGC_FP32_Quaternion* dividend, const float divisor); extern inline int bgc_fp64_quaternion_divide_by_real(BGC_FP64_Quaternion* quotient, const BGC_FP64_Quaternion* dividend, const double divisor); diff --git a/basic-geometry/quaternion.h b/basic-geometry/quaternion.h index 4edc232..5e513ac 100644 --- a/basic-geometry/quaternion.h +++ b/basic-geometry/quaternion.h @@ -330,6 +330,42 @@ inline void bgc_fp64_quaternion_multiply_by_conjugate(BGC_FP64_Quaternion* produ product->x3 = x3; } +// ====== Restrict Multiply By Quaternion ======= // + +inline void _bgc_fp32_restrict_quaternion_multiply_by_quaternion(BGC_FP32_Quaternion* restrict product, const BGC_FP32_Quaternion* left, const BGC_FP32_Quaternion* right) +{ + product->s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3); + product->x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3); + product->x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1); + product->x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2); +} + +inline void _bgc_fp64_restrict_quaternion_multiply_by_quaternion(BGC_FP64_Quaternion* restrict product, const BGC_FP64_Quaternion* left, const BGC_FP64_Quaternion* right) +{ + product->s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3); + product->x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3); + product->x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1); + product->x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2); +} + +// = Restrict Multiply By Conjugate Quaternion == // + +inline void _bgc_fp32_restrict_quaternion_multiply_by_conjugate(BGC_FP32_Quaternion* restrict product, const BGC_FP32_Quaternion* left, const BGC_FP32_Quaternion* right) +{ + product->s0 = (left->s0 * right->s0 + left->x1 * right->x1) + (left->x2 * right->x2 + left->x3 * right->x3); + product->x1 = (left->x1 * right->s0 + left->x3 * right->x2) - (left->s0 * right->x1 + left->x2 * right->x3); + product->x2 = (left->x2 * right->s0 + left->x1 * right->x3) - (left->s0 * right->x2 + left->x3 * right->x1); + product->x3 = (left->x3 * right->s0 + left->x2 * right->x1) - (left->s0 * right->x3 + left->x1 * right->x2); +} + +inline void _bgc_fp64_restrict_quaternion_multiply_by_conjugate(BGC_FP64_Quaternion* restrict product, const BGC_FP64_Quaternion* left, const BGC_FP64_Quaternion* right) +{ + product->s0 = (left->s0 * right->s0 + left->x1 * right->x1) + (left->x2 * right->x2 + left->x3 * right->x3); + product->x1 = (left->x1 * right->s0 + left->x3 * right->x2) - (left->s0 * right->x1 + left->x2 * right->x3); + product->x2 = (left->x2 * right->s0 + left->x1 * right->x3) - (left->s0 * right->x2 + left->x3 * right->x1); + product->x3 = (left->x3 * right->s0 + left->x2 * right->x1) - (left->s0 * right->x3 + left->x1 * right->x2); +} + // ========== Multiply By Dual Number =========== // inline void bgc_fp32_quaternion_multiply_by_dual_number(BGC_FP32_DualQuaternion* product, const BGC_FP32_Quaternion* multiplicand, const BGC_FP32_DualNumber* multiplier) @@ -504,12 +540,12 @@ inline void bgc_fp64_quaternion_interpolate(BGC_FP64_Quaternion* interpolation, inline float bgc_fp32_quaternion_get_dot_product(const BGC_FP32_Quaternion* quaternion1, const BGC_FP32_Quaternion* quaternion2) { - return (quaternion1->s0 * quaternion2->s0 + quaternion1->x1 * quaternion2->x1) + (quaternion1->x2 * quaternion2->x2 + quaternion1->x3 * quaternion2->x3) + return (quaternion1->s0 * quaternion2->s0 + quaternion1->x1 * quaternion2->x1) + (quaternion1->x2 * quaternion2->x2 + quaternion1->x3 * quaternion2->x3); } inline double bgc_fp64_quaternion_get_dot_product(const BGC_FP64_Quaternion* quaternion1, const BGC_FP64_Quaternion* quaternion2) { - return (quaternion1->s0 * quaternion2->s0 + quaternion1->x1 * quaternion2->x1) + (quaternion1->x2 * quaternion2->x2 + quaternion1->x3 * quaternion2->x3) + return (quaternion1->s0 * quaternion2->s0 + quaternion1->x1 * quaternion2->x1) + (quaternion1->x2 * quaternion2->x2 + quaternion1->x3 * quaternion2->x3); } // ================= Conjugate ================== // diff --git a/basic-geometry/rigid-pose3.h b/basic-geometry/rigid-pose3.h index 10b1f3f..8e380ab 100644 --- a/basic-geometry/rigid-pose3.h +++ b/basic-geometry/rigid-pose3.h @@ -79,9 +79,9 @@ inline void bgc_fp32_rigid_pose3_convert_to_fp64(BGC_FP64_RigidPose3* destinatio _bgc_fp64_rigid_pose3_normalize(destination, square_magnitude); } - const double square_magnitude = bgc_fp64_quaternion_get_(&destination->_real_part); + const double dot_product = bgc_fp64_quaternion_get_dot_product(&destination->_real_part, &destination->_dual_part); - bgc_fp64_quaternion_subtract_scaled(&destination->_dual_part, &destination->_dual_part, &destination->_real_part); + bgc_fp64_quaternion_subtract_scaled(&destination->_dual_part, &destination->_dual_part, &destination->_real_part, dot_product); } inline void bgc_fp64_rigid_pose3_convert_to_fp32(BGC_FP32_RigidPose3* destination, const BGC_FP64_RigidPose3* source) @@ -94,30 +94,54 @@ inline void bgc_fp64_rigid_pose3_convert_to_fp32(BGC_FP32_RigidPose3* destinatio if (!bgc_fp32_is_square_unit(square_magnitude)) { _bgc_fp32_rigid_pose3_normalize(destination, square_magnitude); } + + const float dot_product = bgc_fp32_quaternion_get_dot_product(&destination->_real_part, &destination->_dual_part); + + bgc_fp32_quaternion_subtract_scaled(&destination->_dual_part, &destination->_dual_part, &destination->_real_part, dot_product); } // ================== Combine =================== // inline void bgc_fp32_rigid_pose3_combine(BGC_FP32_RigidPose3* combination, const BGC_FP32_RigidPose3* first, const BGC_FP32_RigidPose3* second) { - bgc_fp32_dual_quaternion_multiply_by_dual_quaternion(&combination->_versor, &second->_versor, &first->_versor); + BGC_FP32_Quaternion dual_part1, dual_part2; + + _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); const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(&combination->_real_part); if (!bgc_fp32_is_square_unit(square_magnitude)) { _bgc_fp32_rigid_pose3_normalize(combination, square_magnitude); } + + const float dot_product = bgc_fp32_quaternion_get_dot_product(&combination->_real_part, &combination->_dual_part); + + bgc_fp32_quaternion_subtract_scaled(&combination->_dual_part, &combination->_dual_part, &combination->_real_part, dot_product); } inline void bgc_fp64_rigid_pose3_combine(BGC_FP64_RigidPose3* combination, const BGC_FP64_RigidPose3* first, const BGC_FP64_RigidPose3* second) { - bgc_fp64_dual_quaternion_multiply_by_dual_quaternion(&combination->_versor, &second->_versor, &first->_versor); + BGC_FP64_Quaternion dual_part1, dual_part2; + + _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); const double square_magnitude = bgc_fp64_quaternion_get_square_magnitude(&combination->_real_part); if (!bgc_fp64_is_square_unit(square_magnitude)) { _bgc_fp64_rigid_pose3_normalize(combination, square_magnitude); } + + const double dot_product = bgc_fp64_quaternion_get_dot_product(&combination->_real_part, &combination->_dual_part); + + bgc_fp64_quaternion_subtract_scaled(&combination->_dual_part, &combination->_dual_part, &combination->_real_part, dot_product); } #endif