diff --git a/basic-geometry/versor.c b/basic-geometry/versor.c index 0fadc5c..8405012 100644 --- a/basic-geometry/versor.c +++ b/basic-geometry/versor.c @@ -40,6 +40,9 @@ extern inline void bgc_versor_combine_fp64(const BgcVersorFP64* second, const Bg extern inline void bgc_versor_combine3_fp32(const BgcVersorFP32* third, const BgcVersorFP32* second, const BgcVersorFP32* first, BgcVersorFP32* result); extern inline void bgc_versor_combine3_fp64(const BgcVersorFP64* third, const BgcVersorFP64* second, const BgcVersorFP64* first, BgcVersorFP64* result); +extern inline void bgc_versor_exclude_fp32(const BgcVersorFP32* base, const BgcVersorFP32* excludant, BgcVersorFP32* difference); +extern inline void bgc_versor_exclude_fp64(const BgcVersorFP64* base, const BgcVersorFP64* excludant, BgcVersorFP64* difference); + extern inline void bgc_versor_get_rotation_matrix_fp32(const BgcVersorFP32* versor, BgcMatrix3x3FP32* matrix); extern inline void bgc_versor_get_rotation_matrix_fp64(const BgcVersorFP64* versor, BgcMatrix3x3FP64* matrix); diff --git a/basic-geometry/versor.h b/basic-geometry/versor.h index 8215daa..6240b0f 100644 --- a/basic-geometry/versor.h +++ b/basic-geometry/versor.h @@ -338,6 +338,30 @@ inline void bgc_versor_combine3_fp64(const BgcVersorFP64* third, const BgcVersor ); } +// ================= Exclusion ================== // + +inline void bgc_versor_exclude_fp32(const BgcVersorFP32* base, const BgcVersorFP32* excludant, BgcVersorFP32* difference) +{ + bgc_versor_set_values_fp32( + (base->s0 * excludant->s0 + base->x1 * excludant->x1) + (base->x2 * excludant->x2 + base->x3 * excludant->x3), + (base->x1 * excludant->s0 + base->x3 * excludant->x2) - (base->s0 * excludant->x1 + base->x2 * excludant->x3), + (base->x2 * excludant->s0 + base->x1 * excludant->x3) - (base->s0 * excludant->x2 + base->x3 * excludant->x1), + (base->x3 * excludant->s0 + base->x2 * excludant->x1) - (base->s0 * excludant->x3 + base->x1 * excludant->x2), + difference + ); +} + +inline void bgc_versor_exclude_fp64(const BgcVersorFP64* base, const BgcVersorFP64* excludant, BgcVersorFP64* difference) +{ + bgc_versor_set_values_fp64( + (base->s0 * excludant->s0 + base->x1 * excludant->x1) + (base->x2 * excludant->x2 + base->x3 * excludant->x3), + (base->x1 * excludant->s0 + base->x3 * excludant->x2) - (base->s0 * excludant->x1 + base->x2 * excludant->x3), + (base->x2 * excludant->s0 + base->x1 * excludant->x3) - (base->s0 * excludant->x2 + base->x3 * excludant->x1), + (base->x3 * excludant->s0 + base->x2 * excludant->x1) - (base->s0 * excludant->x3 + base->x1 * excludant->x2), + difference + ); +} + // ================ Get Rotation ================ // void bgc_versor_get_rotation_fp32(const BgcVersorFP32* versor, BgcRotation3FP32* result);