From 2655e43cb4841a1e8719d7a53b91bf88f65accf6 Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Tue, 26 Nov 2024 02:25:04 +0700 Subject: [PATCH] =?UTF-8?q?=D0=A0=D0=B5=D1=84=D0=B0=D0=BA=D1=82=D0=BE?= =?UTF-8?q?=D1=80=D0=B8=D0=BD=D0=B3=20=D0=B8=20=D0=BE=D0=BF=D1=82=D0=B8?= =?UTF-8?q?=D0=BC=D0=B8=D0=B7=D0=B0=D1=86=D0=B8=D1=8F=20=D0=B2=D1=8B=D1=87?= =?UTF-8?q?=D0=B8=D1=81=D0=BB=D0=B5=D0=BD=D0=B8=D0=B9=20/=20Refactoring=20?= =?UTF-8?q?and=20optimization=20of=20computations?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- basic-geometry-dev/main.c | 14 +- basic-geometry/matrix2x2.c | 104 +++++++++ basic-geometry/matrix2x2.h | 144 +++---------- basic-geometry/matrix2x3.h | 18 +- basic-geometry/matrix3x2.h | 16 +- basic-geometry/matrix3x3.c | 80 +++---- basic-geometry/matrix3x3.h | 76 +++---- basic-geometry/quaternion.c | 76 +++++++ basic-geometry/quaternion.h | 74 ++++--- basic-geometry/vector2.c | 40 +++- basic-geometry/vector2.h | 84 +++----- basic-geometry/vector3.c | 40 +++- basic-geometry/vector3.h | 132 +++++------- basic-geometry/versor.c | 327 ++++++++++++++++++++++++++-- basic-geometry/versor.h | 414 ++---------------------------------- 15 files changed, 810 insertions(+), 829 deletions(-) diff --git a/basic-geometry-dev/main.c b/basic-geometry-dev/main.c index 39ea7f9..ec29164 100644 --- a/basic-geometry-dev/main.c +++ b/basic-geometry-dev/main.c @@ -52,7 +52,7 @@ BgFP32Versor * make_random_versors(const unsigned int amount) void print_versor(const BgFP32Versor* versor) { - printf("(%f, %f, %f, %f)\n", versor->s0, versor->x1, versor->x2, versor->x3); + printf("Versor (%f, %f, %f, %f); Delta = %e\n", versor->s0, versor->x1, versor->x2, versor->x3, bg_fp32_versor_get_modulus(versor) - 1.0f); } void print_vector(const BgFP32Vector3* vector) @@ -125,11 +125,11 @@ int main() const unsigned int amount = 1000000; #ifdef _WIN64 - ULONGLONG now; + ULONGLONG now, start, end; now = GetTickCount64(); srand((unsigned int)(now & 0xfffffff)); #else - struct timespec now; + struct timespec now, start, end; clock_gettime(0, &now); srand((unsigned int)(now.tv_nsec & 0xfffffff)); #endif // _WIN64 @@ -159,10 +159,14 @@ int main() } #ifdef _WIN64 - ULONGLONG start, end; + end = GetTickCount64(); + printf("Setup time: %lld\n", end - now); + start = GetTickCount64(); #else - struct timespec start, end; + clock_gettime(CLOCK_REALTIME, &end); + printf("Time: %lf\n", (end.tv_sec - now.tv_sec) * 1000.0 + (end.tv_nsec - now.tv_nsec) * 0.000001); + clock_gettime(CLOCK_REALTIME, &start); #endif // _WIN64 for (int j = 0; j < 1000; j++) { diff --git a/basic-geometry/matrix2x2.c b/basic-geometry/matrix2x2.c index 4bf7977..616f616 100644 --- a/basic-geometry/matrix2x2.c +++ b/basic-geometry/matrix2x2.c @@ -1 +1,105 @@ #include "matrix2x2.h" + +// ================= Inversion ================== // + +int bg_fp32_matrix2x2_invert(BgFP32Matrix2x2* matrix) +{ + const float determinant = bg_fp32_matrix2x2_get_determinant(matrix); + + if (-BG_FP32_EPSYLON <= determinant && determinant <= BG_FP32_EPSYLON) { + return 0; + } + + const float r1c1 = matrix->r2c2; + const float r1c2 = -matrix->r1c2; + + const float r2c1 = -matrix->r2c1; + const float r2c2 = matrix->r1c1; + + const float multiplier = 1.0f / determinant; + + matrix->r1c1 = r1c1 * multiplier; + matrix->r1c2 = r1c2 * multiplier; + + matrix->r2c1 = r2c1 * multiplier; + matrix->r2c2 = r2c2 * multiplier; + + return 1; +} + +int bg_fp64_matrix2x2_invert(BgFP64Matrix2x2* matrix) +{ + const double determinant = bg_fp64_matrix2x2_get_determinant(matrix); + + if (-BG_FP64_EPSYLON <= determinant && determinant <= BG_FP64_EPSYLON) { + return 0; + } + + const double r1c1 = matrix->r2c2; + const double r1c2 = -matrix->r1c2; + + const double r2c1 = -matrix->r2c1; + const double r2c2 = matrix->r1c1; + + const double multiplier = 1.0 / determinant; + + matrix->r1c1 = r1c1 * multiplier; + matrix->r1c2 = r1c2 * multiplier; + + matrix->r2c1 = r2c1 * multiplier; + matrix->r2c2 = r2c2 * multiplier; + + return 1; +} + +// ================ Set Inverted ================ // + +int bg_fp32_matrix2x2_set_inverted(const BgFP32Matrix2x2* from, BgFP32Matrix2x2* to) +{ + const float determinant = bg_fp32_matrix2x2_get_determinant(from); + + if (-BG_FP32_EPSYLON <= determinant && determinant <= BG_FP32_EPSYLON) { + return 0; + } + + const float r1c1 = from->r2c2; + const float r1c2 = -from->r1c2; + + const float r2c1 = -from->r2c1; + const float r2c2 = from->r1c1; + + const float multiplier = 1.0f / determinant; + + to->r1c1 = r1c1 * multiplier; + to->r1c2 = r1c2 * multiplier; + + to->r2c1 = r2c1 * multiplier; + to->r2c2 = r2c2 * multiplier; + + return 1; +} + +int bg_fp64_matrix2x2_set_inverted(const BgFP64Matrix2x2* from, BgFP64Matrix2x2* to) +{ + const double determinant = bg_fp64_matrix2x2_get_determinant(from); + + if (-BG_FP64_EPSYLON <= determinant && determinant <= BG_FP64_EPSYLON) { + return 0; + } + + const double r1c1 = from->r2c2; + const double r1c2 = -from->r1c2; + + const double r2c1 = -from->r2c1; + const double r2c2 = from->r1c1; + + const double multiplier = 1.0 / determinant; + + to->r1c1 = r1c1 * multiplier; + to->r1c2 = r1c2 * multiplier; + + to->r2c1 = r2c1 * multiplier; + to->r2c2 = r2c2 * multiplier; + + return 1; +} diff --git a/basic-geometry/matrix2x2.h b/basic-geometry/matrix2x2.h index 70f8c91..fe4aaad 100644 --- a/basic-geometry/matrix2x2.h +++ b/basic-geometry/matrix2x2.h @@ -171,51 +171,9 @@ static inline void bg_fp64_matrix2x2_transpose(BgFP64Matrix2x2* matrix) // ================= Inversion ================== // -static inline int bg_fp32_matrix2x2_invert(BgFP32Matrix2x2* matrix) -{ - const float determinant = bg_fp32_matrix2x2_get_determinant(matrix); +int bg_fp32_matrix2x2_invert(BgFP32Matrix2x2* matrix); - if (-BG_FP32_EPSYLON <= determinant && determinant <= BG_FP32_EPSYLON) { - return 0; - } - - const float r1c1 = matrix->r2c2; - const float r1c2 = -matrix->r1c2; - - const float r2c1 = -matrix->r2c1; - const float r2c2 = matrix->r1c1; - - matrix->r1c1 = r1c1 / determinant; - matrix->r1c2 = r1c2 / determinant; - - matrix->r2c1 = r2c1 / determinant; - matrix->r2c2 = r2c2 / determinant; - - return 1; -} - -static inline int bg_fp64_matrix2x2_invert(BgFP64Matrix2x2* matrix) -{ - const double determinant = bg_fp64_matrix2x2_get_determinant(matrix); - - if (-BG_FP64_EPSYLON <= determinant && determinant <= BG_FP64_EPSYLON) { - return 0; - } - - const double r1c1 = matrix->r2c2; - const double r1c2 = -matrix->r1c2; - - const double r2c1 = -matrix->r2c1; - const double r2c2 = matrix->r1c1; - - matrix->r1c1 = r1c1 / determinant; - matrix->r1c2 = r1c2 / determinant; - - matrix->r2c1 = r2c1 / determinant; - matrix->r2c2 = r2c2 / determinant; - - return 1; -} +int bg_fp64_matrix2x2_invert(BgFP64Matrix2x2* matrix); // =============== Set Transposed =============== // @@ -243,51 +201,9 @@ static inline void bg_fp64_matrix2x2_set_transposed(const BgFP64Matrix2x2* from, // ================ Set Inverted ================ // -static inline int bg_fp32_matrix2x2_set_inverted(const BgFP32Matrix2x2* from, BgFP32Matrix2x2* to) -{ - const float determinant = bg_fp32_matrix2x2_get_determinant(from); +int bg_fp32_matrix2x2_set_inverted(const BgFP32Matrix2x2* from, BgFP32Matrix2x2* to); - if (-BG_FP32_EPSYLON <= determinant && determinant <= BG_FP32_EPSYLON) { - return 0; - } - - const float r1c1 = from->r2c2; - const float r1c2 = -from->r1c2; - - const float r2c1 = -from->r2c1; - const float r2c2 = from->r1c1; - - to->r1c1 = r1c1 / determinant; - to->r1c2 = r1c2 / determinant; - - to->r2c1 = r2c1 / determinant; - to->r2c2 = r2c2 / determinant; - - return 1; -} - -static inline int bg_fp64_matrix2x2_set_inverted(const BgFP64Matrix2x2* from, BgFP64Matrix2x2* to) -{ - const double determinant = bg_fp64_matrix2x2_get_determinant(from); - - if (-BG_FP64_EPSYLON <= determinant && determinant <= BG_FP64_EPSYLON) { - return 0; - } - - const double r1c1 = from->r2c2; - const double r1c2 = -from->r1c2; - - const double r2c1 = -from->r2c1; - const double r2c2 = from->r1c1; - - to->r1c1 = r1c1 / determinant; - to->r1c2 = r1c2 / determinant; - - to->r2c1 = r2c1 / determinant; - to->r2c2 = r2c2 / determinant; - - return 1; -} +int bg_fp64_matrix2x2_set_inverted(const BgFP64Matrix2x2* from, BgFP64Matrix2x2* to); // ================= Set Row 1 ================== // @@ -429,60 +345,52 @@ static inline void bg_fp64_matrix2x2_multiply(const BgFP64Matrix2x2* multiplican static inline void bg_fp32_matrix2x2_divide(const BgFP32Matrix2x2* dividend, const float divisor, BgFP32Matrix2x2* quotient) { - quotient->r1c1 = dividend->r1c1 / divisor; - quotient->r1c2 = dividend->r1c2 / divisor; - - quotient->r2c1 = dividend->r2c1 / divisor; - quotient->r2c2 = dividend->r2c2 / divisor; + bg_fp32_matrix2x2_multiply(dividend, 1.0f / divisor, quotient); } static inline void bg_fp64_matrix2x2_divide(const BgFP64Matrix2x2* dividend, const double divisor, BgFP64Matrix2x2* quotient) { - quotient->r1c1 = dividend->r1c1 / divisor; - quotient->r1c2 = dividend->r1c2 / divisor; - - quotient->r2c1 = dividend->r2c1 / divisor; - quotient->r2c2 = dividend->r2c2 / divisor; + bg_fp64_matrix2x2_multiply(dividend, 1.0 / divisor, quotient); } // ============ Left Vector Product ============= // static inline void bg_fp32_matrix2x2_left_product(const BgFP32Vector2* vector, const BgFP32Matrix2x2* matrix, BgFP32Vector2* result) { - bg_fp32_vector2_set_values( - vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1, - vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2, - result - ); + const float x1 = vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1; + const float x2 = vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2; + + result->x1 = x1; + result->x2 = x2; } static inline void bg_fp64_matrix2x2_left_product(const BgFP64Vector2* vector, const BgFP64Matrix2x2* matrix, BgFP64Vector2* result) { - bg_fp64_vector2_set_values( - vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1, - vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2, - result - ); + const double x1 = vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1; + const double x2 = vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2; + + result->x1 = x1; + result->x2 = x2; } // ============ Right Vector Product ============ // static inline void bg_fp32_matrix2x2_right_product(const BgFP32Matrix2x2* matrix, const BgFP32Vector2* vector, BgFP32Vector2* result) { - bg_fp32_vector2_set_values( - matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2, - matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2, - result - ); + const float x1 = matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2; + const float x2 = matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2; + + result->x1 = x1; + result->x2 = x2; } static inline void bg_fp64_matrix2x2_right_product(const BgFP64Matrix2x2* matrix, const BgFP64Vector2* vector, BgFP64Vector2* result) { - bg_fp64_vector2_set_values( - matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2, - matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2, - result - ); + const double x1 = matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2; + const double x2 = matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2; + + result->x1 = x1; + result->x2 = x2; } #endif diff --git a/basic-geometry/matrix2x3.h b/basic-geometry/matrix2x3.h index ee3d3d7..e3fa0fd 100644 --- a/basic-geometry/matrix2x3.h +++ b/basic-geometry/matrix2x3.h @@ -317,26 +317,12 @@ static inline void bg_fp64_matrix2x3_multiply(const BgFP64Matrix2x3* multiplican static inline void bg_fp32_matrix2x3_divide(const BgFP32Matrix2x3* dividend, const float divisor, BgFP32Matrix2x3* quotient) { - quotient->r1c1 = dividend->r1c1 / divisor; - quotient->r1c2 = dividend->r1c2 / divisor; - - quotient->r2c1 = dividend->r2c1 / divisor; - quotient->r2c2 = dividend->r2c2 / divisor; - - quotient->r3c1 = dividend->r3c1 / divisor; - quotient->r3c2 = dividend->r3c2 / divisor; + bg_fp32_matrix2x3_multiply(dividend, 1.0f / divisor, quotient); } static inline void bg_fp64_matrix2x3_divide(const BgFP64Matrix2x3* dividend, const double divisor, BgFP64Matrix2x3* quotient) { - quotient->r1c1 = dividend->r1c1 / divisor; - quotient->r1c2 = dividend->r1c2 / divisor; - - quotient->r2c1 = dividend->r2c1 / divisor; - quotient->r2c2 = dividend->r2c2 / divisor; - - quotient->r3c1 = dividend->r3c1 / divisor; - quotient->r3c2 = dividend->r3c2 / divisor; + bg_fp64_matrix2x3_multiply(dividend, 1.0 / divisor, quotient); } // ============ Left Vector Product ============= // diff --git a/basic-geometry/matrix3x2.h b/basic-geometry/matrix3x2.h index 3aa48d5..4f51e77 100644 --- a/basic-geometry/matrix3x2.h +++ b/basic-geometry/matrix3x2.h @@ -275,24 +275,12 @@ static inline void bg_fp64_matrix3x2_multiply(const BgFP64Matrix3x2* multiplican static inline void bg_fp32_matrix3x2_divide(const BgFP32Matrix3x2* dividend, const float divisor, BgFP32Matrix3x2* quotient) { - quotient->r1c1 = dividend->r1c1 / divisor; - quotient->r1c2 = dividend->r1c2 / divisor; - quotient->r1c3 = dividend->r1c3 / divisor; - - quotient->r2c1 = dividend->r2c1 / divisor; - quotient->r2c2 = dividend->r2c2 / divisor; - quotient->r2c3 = dividend->r2c3 / divisor; + bg_fp32_matrix3x2_multiply(dividend, 1.0f / divisor, quotient); } static inline void bg_fp64_matrix3x2_divide(const BgFP64Matrix3x2* dividend, const double divisor, BgFP64Matrix3x2* quotient) { - quotient->r1c1 = dividend->r1c1 / divisor; - quotient->r1c2 = dividend->r1c2 / divisor; - quotient->r1c3 = dividend->r1c3 / divisor; - - quotient->r2c1 = dividend->r2c1 / divisor; - quotient->r2c2 = dividend->r2c2 / divisor; - quotient->r2c3 = dividend->r2c3 / divisor; + bg_fp64_matrix3x2_multiply(dividend, 1.0 / divisor, quotient); } // ============ Left Vector Product ============= // diff --git a/basic-geometry/matrix3x3.c b/basic-geometry/matrix3x3.c index 357b3ca..2bbb63c 100644 --- a/basic-geometry/matrix3x3.c +++ b/basic-geometry/matrix3x3.c @@ -22,17 +22,19 @@ int bg_fp32_matrix3x3_invert(BgFP32Matrix3x3* matrix) const float r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2; const float r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1; - matrix->r1c1 = r1c1 / determinant; - matrix->r1c2 = r1c2 / determinant; - matrix->r1c3 = r1c3 / determinant; + const float multiplier = 1.0f / determinant; - matrix->r2c1 = r2c1 / determinant; - matrix->r2c2 = r2c2 / determinant; - matrix->r2c3 = r2c3 / determinant; + matrix->r1c1 = r1c1 * multiplier; + matrix->r1c2 = r1c2 * multiplier; + matrix->r1c3 = r1c3 * multiplier; - matrix->r3c1 = r3c1 / determinant; - matrix->r3c2 = r3c2 / determinant; - matrix->r3c3 = r3c3 / determinant; + matrix->r2c1 = r2c1 * multiplier; + matrix->r2c2 = r2c2 * multiplier; + matrix->r2c3 = r2c3 * multiplier; + + matrix->r3c1 = r3c1 * multiplier; + matrix->r3c2 = r3c2 * multiplier; + matrix->r3c3 = r3c3 * multiplier; return 1; } @@ -57,17 +59,19 @@ int bg_fp64_matrix3x3_invert(BgFP64Matrix3x3* matrix) const double r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2; const double r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1; - matrix->r1c1 = r1c1 / determinant; - matrix->r1c2 = r1c2 / determinant; - matrix->r1c3 = r1c3 / determinant; + const double multiplier = 1.0 / determinant; - matrix->r2c1 = r2c1 / determinant; - matrix->r2c2 = r2c2 / determinant; - matrix->r2c3 = r2c3 / determinant; + matrix->r1c1 = r1c1 * multiplier; + matrix->r1c2 = r1c2 * multiplier; + matrix->r1c3 = r1c3 * multiplier; - matrix->r3c1 = r3c1 / determinant; - matrix->r3c2 = r3c2 / determinant; - matrix->r3c3 = r3c3 / determinant; + matrix->r2c1 = r2c1 * multiplier; + matrix->r2c2 = r2c2 * multiplier; + matrix->r2c3 = r2c3 * multiplier; + + matrix->r3c1 = r3c1 * multiplier; + matrix->r3c2 = r3c2 * multiplier; + matrix->r3c3 = r3c3 * multiplier; return 1; } @@ -94,17 +98,19 @@ int bg_fp32_matrix3x3_set_inverted(const BgFP32Matrix3x3* matrix, BgFP32Matrix3x const float r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2; const float r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1; - result->r1c1 = r1c1 / determinant; - result->r1c2 = r1c2 / determinant; - result->r1c3 = r1c3 / determinant; + const float multiplier = 1.0f / determinant; - result->r2c1 = r2c1 / determinant; - result->r2c2 = r2c2 / determinant; - result->r2c3 = r2c3 / determinant; + result->r1c1 = r1c1 * multiplier; + result->r1c2 = r1c2 * multiplier; + result->r1c3 = r1c3 * multiplier; - result->r3c1 = r3c1 / determinant; - result->r3c2 = r3c2 / determinant; - result->r3c3 = r3c3 / determinant; + result->r2c1 = r2c1 * multiplier; + result->r2c2 = r2c2 * multiplier; + result->r2c3 = r2c3 * multiplier; + + result->r3c1 = r3c1 * multiplier; + result->r3c2 = r3c2 * multiplier; + result->r3c3 = r3c3 * multiplier; return 1; } @@ -129,17 +135,19 @@ int bg_fp64_matrix3x3_set_inverted(const BgFP64Matrix3x3* matrix, BgFP64Matrix3x const double r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2; const double r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1; - result->r1c1 = r1c1 / determinant; - result->r1c2 = r1c2 / determinant; - result->r1c3 = r1c3 / determinant; + const double multiplier = 1.0 / determinant; - result->r2c1 = r2c1 / determinant; - result->r2c2 = r2c2 / determinant; - result->r2c3 = r2c3 / determinant; + result->r1c1 = r1c1 * multiplier; + result->r1c2 = r1c2 * multiplier; + result->r1c3 = r1c3 * multiplier; - result->r3c1 = r3c1 / determinant; - result->r3c2 = r3c2 / determinant; - result->r3c3 = r3c3 / determinant; + result->r2c1 = r2c1 * multiplier; + result->r2c2 = r2c2 * multiplier; + result->r2c3 = r2c3 * multiplier; + + result->r3c1 = r3c1 * multiplier; + result->r3c2 = r3c2 * multiplier; + result->r3c3 = r3c3 * multiplier; return 1; } diff --git a/basic-geometry/matrix3x3.h b/basic-geometry/matrix3x3.h index d8a2fab..b940c3d 100644 --- a/basic-geometry/matrix3x3.h +++ b/basic-geometry/matrix3x3.h @@ -510,76 +510,60 @@ static inline void bg_fp64_matrix3x3_multiply(const BgFP64Matrix3x3* multiplican static inline void bg_fp32_matrix3x3_divide(const BgFP32Matrix3x3* dividend, const float divisor, BgFP32Matrix3x3* quotient) { - quotient->r1c1 = dividend->r1c1 / divisor; - quotient->r1c2 = dividend->r1c2 / divisor; - quotient->r1c3 = dividend->r1c3 / divisor; - - quotient->r2c1 = dividend->r2c1 / divisor; - quotient->r2c2 = dividend->r2c2 / divisor; - quotient->r2c3 = dividend->r2c3 / divisor; - - quotient->r3c1 = dividend->r3c1 / divisor; - quotient->r3c2 = dividend->r3c2 / divisor; - quotient->r3c3 = dividend->r3c3 / divisor; + bg_fp32_matrix3x3_multiply(dividend, 1.0f / divisor, quotient); } static inline void bg_fp64_matrix3x3_divide(const BgFP64Matrix3x3* dividend, const double divisor, BgFP64Matrix3x3* quotient) { - quotient->r1c1 = dividend->r1c1 / divisor; - quotient->r1c2 = dividend->r1c2 / divisor; - quotient->r1c3 = dividend->r1c3 / divisor; - - quotient->r2c1 = dividend->r2c1 / divisor; - quotient->r2c2 = dividend->r2c2 / divisor; - quotient->r2c3 = dividend->r2c3 / divisor; - - quotient->r3c1 = dividend->r3c1 / divisor; - quotient->r3c2 = dividend->r3c2 / divisor; - quotient->r3c3 = dividend->r3c3 / divisor; + bg_fp64_matrix3x3_multiply(dividend, 1.0 / divisor, quotient); } // ============ Left Vector Product ============= // static inline void bg_fp32_matrix3x3_left_product(const BgFP32Vector3* vector, const BgFP32Matrix3x3* matrix, BgFP32Vector3* result) { - bg_fp32_vector3_set_values( - vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1 + vector->x3 * matrix->r3c1, - vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2 + vector->x3 * matrix->r3c2, - vector->x1 * matrix->r1c3 + vector->x2 * matrix->r2c3 + vector->x3 * matrix->r3c3, - result - ); + const float x1 = vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1 + vector->x3 * matrix->r3c1; + const float x2 = vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2 + vector->x3 * matrix->r3c2; + const float x3 = vector->x1 * matrix->r1c3 + vector->x2 * matrix->r2c3 + vector->x3 * matrix->r3c3; + + result->x1 = x1; + result->x2 = x2; + result->x3 = x3; } static inline void bg_fp64_matrix3x3_left_product(const BgFP64Vector3* vector, const BgFP64Matrix3x3* matrix, BgFP64Vector3* result) { - bg_fp64_vector3_set_values( - vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1 + vector->x3 * matrix->r3c1, - vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2 + vector->x3 * matrix->r3c2, - vector->x1 * matrix->r1c3 + vector->x2 * matrix->r2c3 + vector->x3 * matrix->r3c3, - result - ); + const double x1 = vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1 + vector->x3 * matrix->r3c1; + const double x2 = vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2 + vector->x3 * matrix->r3c2; + const double x3 = vector->x1 * matrix->r1c3 + vector->x2 * matrix->r2c3 + vector->x3 * matrix->r3c3; + + result->x1 = x1; + result->x2 = x2; + result->x3 = x3; } // ============ Right Vector Product ============ // static inline void bg_fp32_matrix3x3_right_product(const BgFP32Matrix3x3* matrix, const BgFP32Vector3* vector, BgFP32Vector3* result) { - bg_fp32_vector3_set_values( - matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2 + matrix->r1c3 * vector->x3, - matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2 + matrix->r2c3 * vector->x3, - matrix->r3c1 * vector->x1 + matrix->r3c2 * vector->x2 + matrix->r3c3 * vector->x3, - result - ); + const float x1 = matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2 + matrix->r1c3 * vector->x3; + const float x2 = matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2 + matrix->r2c3 * vector->x3; + const float x3 = matrix->r3c1 * vector->x1 + matrix->r3c2 * vector->x2 + matrix->r3c3 * vector->x3; + + result->x1 = x1; + result->x2 = x2; + result->x3 = x3; } static inline void bg_fp64_matrix3x3_right_product(const BgFP64Matrix3x3* matrix, const BgFP64Vector3* vector, BgFP64Vector3* result) { - bg_fp64_vector3_set_values( - matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2 + matrix->r1c3 * vector->x3, - matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2 + matrix->r2c3 * vector->x3, - matrix->r3c1 * vector->x1 + matrix->r3c2 * vector->x2 + matrix->r3c3 * vector->x3, - result - ); + const double x1 = matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2 + matrix->r1c3 * vector->x3; + const double x2 = matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2 + matrix->r2c3 * vector->x3; + const double x3 = matrix->r3c1 * vector->x1 + matrix->r3c2 * vector->x2 + matrix->r3c3 * vector->x3; + + result->x1 = x1; + result->x2 = x2; + result->x3 = x3; } #endif diff --git a/basic-geometry/quaternion.c b/basic-geometry/quaternion.c index 2526761..eb16e27 100644 --- a/basic-geometry/quaternion.c +++ b/basic-geometry/quaternion.c @@ -1,5 +1,53 @@ #include "quaternion.h" +// =============== Normalization ================ // + +int bg_fp32_quaternion_normalize(BgFP32Quaternion* quaternion) +{ + const float square_modulus = bg_fp32_quaternion_get_square_modulus(quaternion); + + if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) { + return 1; + } + + if (square_modulus <= BG_FP32_SQUARE_EPSYLON) { + bg_fp32_quaternion_reset(quaternion); + return 0; + } + + const float multiplier = sqrtf(1.0f / square_modulus); + + quaternion->s0 *= multiplier; + quaternion->x1 *= multiplier; + quaternion->x2 *= multiplier; + quaternion->x3 *= multiplier; + + return 1; +} + +int bg_fp64_quaternion_normalize(BgFP64Quaternion* quaternion) +{ + const double square_modulus = bg_fp64_quaternion_get_square_modulus(quaternion); + + if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) { + return 1; + } + + if (square_modulus <= BG_FP32_SQUARE_EPSYLON) { + bg_fp64_quaternion_reset(quaternion); + return 0; + } + + const double multiplier = sqrt(1.0 / square_modulus); + + quaternion->s0 *= multiplier; + quaternion->x1 *= multiplier; + quaternion->x2 *= multiplier; + quaternion->x3 *= multiplier; + + return 1; +} + // ============ Make Rotation Matrix ============ // void bg_fp32_quaternion_get_rotation_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix) @@ -155,3 +203,31 @@ void bg_fp64_quaternion_get_reverse_matrix(const BgFP64Quaternion* quaternion, B matrix->r3c2 = corrector2 * (x2x3 - s0x1); matrix->r1c3 = corrector2 * (x1x3 - s0x2); } + +// ================== Product =================== // + +void bg_fp32_quaternion_get_product(const BgFP32Quaternion* left, const BgFP32Quaternion* right, BgFP32Quaternion* product) +{ + const float s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3); + const float x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3); + const float x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1); + const float x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2); + + product->s0 = s0; + product->x1 = x1; + product->x2 = x2; + product->x3 = x3; +} + +void bg_fp64_quaternion_get_product(const BgFP64Quaternion* left, const BgFP64Quaternion* right, BgFP64Quaternion* product) +{ + const double s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3); + const double x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3); + const double x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1); + const double x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2); + + product->s0 = s0; + product->x1 = x1; + product->x2 = x2; + product->x3 = x3; +} diff --git a/basic-geometry/quaternion.h b/basic-geometry/quaternion.h index 9a27f12..908a9c1 100644 --- a/basic-geometry/quaternion.h +++ b/basic-geometry/quaternion.h @@ -181,6 +181,12 @@ static inline double bg_fp64_quaternion_get_modulus(const BgFP64Quaternion* quat return sqrt(bg_fp64_quaternion_get_square_modulus(quaternion)); } +// =============== Normalization ================ // + +int bg_fp32_quaternion_normalize(BgFP32Quaternion* quaternion); + +int bg_fp64_quaternion_normalize(BgFP64Quaternion* quaternion); + // ============ Make Rotation Matrix ============ // void bg_fp32_quaternion_get_rotation_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix); @@ -213,48 +219,56 @@ static inline void bg_fp64_quaternion_add(const BgFP64Quaternion * quaternion1, // ================== Subtract ================== // -static inline void bg_fp32_quaternion_subtract(const BgFP32Quaternion * minuend, const BgFP32Quaternion * subtrahend, BgFP32Quaternion * result) +static inline void bg_fp32_quaternion_subtract(const BgFP32Quaternion * minuend, const BgFP32Quaternion * subtrahend, BgFP32Quaternion * difference) { - result->s0 = minuend->s0 - subtrahend->s0; - result->x1 = minuend->x1 - subtrahend->x1; - result->x2 = minuend->x2 - subtrahend->x2; - result->x3 = minuend->x3 - subtrahend->x3; + difference->s0 = minuend->s0 - subtrahend->s0; + difference->x1 = minuend->x1 - subtrahend->x1; + difference->x2 = minuend->x2 - subtrahend->x2; + difference->x3 = minuend->x3 - subtrahend->x3; } -static inline void bg_fp64_quaternion_subtract(const BgFP64Quaternion * minuend, const BgFP64Quaternion * subtrahend, BgFP64Quaternion * result) +static inline void bg_fp64_quaternion_subtract(const BgFP64Quaternion * minuend, const BgFP64Quaternion * subtrahend, BgFP64Quaternion * difference) { - result->s0 = minuend->s0 - subtrahend->s0; - result->x1 = minuend->x1 - subtrahend->x1; - result->x2 = minuend->x2 - subtrahend->x2; - result->x3 = minuend->x3 - subtrahend->x3; + difference->s0 = minuend->s0 - subtrahend->s0; + difference->x1 = minuend->x1 - subtrahend->x1; + difference->x2 = minuend->x2 - subtrahend->x2; + difference->x3 = minuend->x3 - subtrahend->x3; } // =============== Multiplication =============== // -static inline void bg_fp32_quaternion_multiply(const BgFP32Quaternion* left, const BgFP32Quaternion* right, BgFP32Quaternion* result) +static inline void bg_fp32_quaternion_multiply(const BgFP32Quaternion* multiplicand, const float multipier, BgFP32Quaternion* product) { - const float s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3); - const float x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3); - const float x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1); - const float x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2); - - result->s0 = s0; - result->x1 = x1; - result->x2 = x2; - result->x3 = x3; + product->s0 = multiplicand->s0 * multipier; + product->x1 = multiplicand->x1 * multipier; + product->x2 = multiplicand->x2 * multipier; + product->x3 = multiplicand->x3 * multipier; } -static inline void bg_fp64_quaternion_multiply(const BgFP64Quaternion* left, const BgFP64Quaternion* right, BgFP64Quaternion* result) +static inline void bg_fp64_quaternion_multiply(const BgFP64Quaternion* multiplicand, const double multipier, BgFP64Quaternion* product) { - const double s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3); - const double x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3); - const double x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1); - const double x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2); - - result->s0 = s0; - result->x1 = x1; - result->x2 = x2; - result->x3 = x3; + product->s0 = multiplicand->s0 * multipier; + product->x1 = multiplicand->x1 * multipier; + product->x2 = multiplicand->x2 * multipier; + product->x3 = multiplicand->x3 * multipier; } +// ================== Division ================== // + +static inline void bg_fp32_quaternion_divide(const BgFP32Quaternion* dividend, const float divisor, BgFP32Quaternion* quotient) +{ + bg_fp32_quaternion_multiply(dividend, 1.0f / divisor, quotient); +} + +static inline void bg_fp64_quaternion_divide(const BgFP64Quaternion* dividend, const double divisor, BgFP64Quaternion* quotient) +{ + bg_fp64_quaternion_multiply(dividend, 1.0 / divisor, quotient); +} + +// ================== Product =================== // + +void bg_fp32_quaternion_get_product(const BgFP32Quaternion* left, const BgFP32Quaternion* right, BgFP32Quaternion* product); + +void bg_fp64_quaternion_get_product(const BgFP64Quaternion* left, const BgFP64Quaternion* right, BgFP64Quaternion* product); + #endif // _GEOMETRY_QUATERNION_H_ diff --git a/basic-geometry/vector2.c b/basic-geometry/vector2.c index bcd0cc2..5d82337 100644 --- a/basic-geometry/vector2.c +++ b/basic-geometry/vector2.c @@ -1,5 +1,41 @@ #include "vector2.h" +// =============== Normalization ================ // + +int bg_fp32_vector2_normalize(BgFP32Vector2* vector) +{ + const float square_modulus = bg_fp32_vector2_get_square_modulus(vector); + + if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) { + return 1; + } + + if (square_modulus <= BG_FP32_SQUARE_EPSYLON) { + bg_fp32_vector2_reset(vector); + return 0; + } + + bg_fp32_vector2_multiply(vector, sqrtf(1.0f / square_modulus), vector); + return 1; +} + +int bg_fp64_vector2_normalize(BgFP64Vector2* vector) +{ + const double square_modulus = bg_fp64_vector2_get_square_modulus(vector); + + if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) { + return 1; + } + + if (square_modulus <= BG_FP64_SQUARE_EPSYLON) { + bg_fp64_vector2_reset(vector); + return 0; + } + + bg_fp64_vector2_multiply(vector, sqrt(1.0 / square_modulus), vector); + return 1; +} + // =================== Angle ==================== // float bg_fp32_vector2_get_angle(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2, const angle_unit_t unit) @@ -20,7 +56,7 @@ float bg_fp32_vector2_get_angle(const BgFP32Vector2* vector1, const BgFP32Vector return 0.0f; } - const float cosine = bg_fp32_vector2_dot_product(vector1, vector2) / sqrtf(square_modulus1 * square_modulus2); + const float cosine = bg_fp32_vector2_scalar_product(vector1, vector2) / sqrtf(square_modulus1 * square_modulus2); if (cosine >= 1.0f - BG_FP32_EPSYLON) { return 0.0f; @@ -51,7 +87,7 @@ double bg_fp64_vector2_get_angle(const BgFP64Vector2* vector1, const BgFP64Vecto return 0.0; } - const double cosine = bg_fp64_vector2_dot_product(vector1, vector2) / sqrt(square_modulus1 * square_modulus2); + const double cosine = bg_fp64_vector2_scalar_product(vector1, vector2) / sqrt(square_modulus1 * square_modulus2); if (cosine >= 1.0 - BG_FP64_EPSYLON) { return 0.0; diff --git a/basic-geometry/vector2.h b/basic-geometry/vector2.h index c7e169d..eae428b 100644 --- a/basic-geometry/vector2.h +++ b/basic-geometry/vector2.h @@ -150,58 +150,56 @@ static inline int bg_fp64_vector2_is_unit(const BgFP64Vector2* vector) // ==================== Add ===================== // -static inline void bg_fp32_vector2_add(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2, BgFP32Vector2* result) +static inline void bg_fp32_vector2_add(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2, BgFP32Vector2* sum) { - result->x1 = vector1->x1 + vector2->x1; - result->x2 = vector1->x2 + vector2->x2; + sum->x1 = vector1->x1 + vector2->x1; + sum->x2 = vector1->x2 + vector2->x2; } -static inline void bg_fp64_vector2_add(const BgFP64Vector2* vector1, const BgFP64Vector2* vector2, BgFP64Vector2* result) +static inline void bg_fp64_vector2_add(const BgFP64Vector2* vector1, const BgFP64Vector2* vector2, BgFP64Vector2* sum) { - result->x1 = vector1->x1 + vector2->x1; - result->x2 = vector1->x2 + vector2->x2; + sum->x1 = vector1->x1 + vector2->x1; + sum->x2 = vector1->x2 + vector2->x2; } // ================ Subtraction ================= // -static inline void bg_fp32_vector2_subtract(const BgFP32Vector2* minuend, const BgFP32Vector2* subtrahend, BgFP32Vector2* result) +static inline void bg_fp32_vector2_subtract(const BgFP32Vector2* minuend, const BgFP32Vector2* subtrahend, BgFP32Vector2* difference) { - result->x1 = minuend->x1 - subtrahend->x1; - result->x2 = minuend->x2 - subtrahend->x2; + difference->x1 = minuend->x1 - subtrahend->x1; + difference->x2 = minuend->x2 - subtrahend->x2; } -static inline void bg_fp64_vector2_subtract(const BgFP64Vector2* minuend, const BgFP64Vector2* subtrahend, BgFP64Vector2* result) +static inline void bg_fp64_vector2_subtract(const BgFP64Vector2* minuend, const BgFP64Vector2* subtrahend, BgFP64Vector2* difference) { - result->x1 = minuend->x1 - subtrahend->x1; - result->x2 = minuend->x2 - subtrahend->x2; + difference->x1 = minuend->x1 - subtrahend->x1; + difference->x2 = minuend->x2 - subtrahend->x2; } // =============== Multiplication =============== // -static inline void bg_fp32_vector2_multiply(const BgFP32Vector2* multiplicand, const float multiplier, BgFP32Vector2* result) +static inline void bg_fp32_vector2_multiply(const BgFP32Vector2* multiplicand, const float multiplier, BgFP32Vector2* product) { - result->x1 = multiplicand->x1 * multiplier; - result->x2 = multiplicand->x2 * multiplier; + product->x1 = multiplicand->x1 * multiplier; + product->x2 = multiplicand->x2 * multiplier; } -static inline void bg_fp64_vector2_multiply(const BgFP64Vector2* multiplicand, const double multiplier, BgFP64Vector2* result) +static inline void bg_fp64_vector2_multiply(const BgFP64Vector2* multiplicand, const double multiplier, BgFP64Vector2* product) { - result->x1 = multiplicand->x1 * multiplier; - result->x2 = multiplicand->x2 * multiplier; + product->x1 = multiplicand->x1 * multiplier; + product->x2 = multiplicand->x2 * multiplier; } // ================== Division ================== // -static inline void bg_fp32_vector2_divide(const BgFP32Vector2* dividend, const float divisor, BgFP32Vector2* result) +static inline void bg_fp32_vector2_divide(const BgFP32Vector2* dividend, const float divisor, BgFP32Vector2* quotient) { - result->x1 = dividend->x1 / divisor; - result->x2 = dividend->x2 / divisor; + bg_fp32_vector2_multiply(dividend, 1.0f / divisor, quotient); } -static inline void bg_fp64_vector2_divide(const BgFP64Vector2* dividend, const double divisor, BgFP64Vector2* result) +static inline void bg_fp64_vector2_divide(const BgFP64Vector2* dividend, const double divisor, BgFP64Vector2* quotient) { - result->x1 = dividend->x1 / divisor; - result->x2 = dividend->x2 / divisor; + bg_fp64_vector2_multiply(dividend, 1.0 / divisor, quotient); } // ================ Append scaled =============== // @@ -248,12 +246,12 @@ static inline void bg_fp64_vector2_get_mean3(const BgFP64Vector2* vector1, const // =============== Scalar Product =============== // -static inline float bg_fp32_vector2_dot_product(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2) +static inline float bg_fp32_vector2_scalar_product(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2) { return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2; } -static inline double bg_fp64_vector2_dot_product(const BgFP64Vector2* vector1, const BgFP64Vector2* vector2) +static inline double bg_fp64_vector2_scalar_product(const BgFP64Vector2* vector1, const BgFP64Vector2* vector2) { return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2; } @@ -272,39 +270,9 @@ static inline double bg_fp64_vector2_cross_product(const BgFP64Vector2* vector1, // =============== Normalization ================ // -static inline int bg_fp32_vector2_normalize(BgFP32Vector2* vector) -{ - const float square_modulus = bg_fp32_vector2_get_square_modulus(vector); +int bg_fp32_vector2_normalize(BgFP32Vector2* vector); - if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) { - return 1; - } - - if (square_modulus <= BG_FP32_SQUARE_EPSYLON) { - bg_fp32_vector2_reset(vector); - return 0; - } - - bg_fp32_vector2_divide(vector, sqrtf(square_modulus), vector); - return 1; -} - -static inline int bg_fp64_vector2_normalize(BgFP64Vector2* vector) -{ - const double square_modulus = bg_fp64_vector2_get_square_modulus(vector); - - if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) { - return 1; - } - - if (square_modulus <= BG_FP64_SQUARE_EPSYLON) { - bg_fp64_vector2_reset(vector); - return 0; - } - - bg_fp64_vector2_divide(vector, sqrt(square_modulus), vector); - return 1; -} +int bg_fp64_vector2_normalize(BgFP64Vector2* vector); // =============== Get Normalized =============== // diff --git a/basic-geometry/vector3.c b/basic-geometry/vector3.c index 029097b..a98ae39 100644 --- a/basic-geometry/vector3.c +++ b/basic-geometry/vector3.c @@ -1,5 +1,41 @@ #include "vector3.h" +// =============== Normalization ================ // + +int bg_fp32_vector3_normalize(BgFP32Vector3* vector) +{ + const float square_modulus = bg_fp32_vector3_get_square_modulus(vector); + + if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) { + return 1; + } + + if (square_modulus <= BG_FP32_SQUARE_EPSYLON) { + bg_fp32_vector3_reset(vector); + return 0; + } + + bg_fp32_vector3_multiply(vector, sqrtf(1.0f / square_modulus), vector); + return 1; +} + +int bg_fp64_vector3_normalize(BgFP64Vector3* vector) +{ + const double square_modulus = bg_fp64_vector3_get_square_modulus(vector); + + if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) { + return 1; + } + + if (square_modulus <= BG_FP64_SQUARE_EPSYLON) { + bg_fp64_vector3_reset(vector); + return 0; + } + + bg_fp64_vector3_multiply(vector, sqrt(1.0 / square_modulus), vector); + return 1; +} + // =================== Angle ==================== // float bg_fp32_vector3_get_angle(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, const angle_unit_t unit) @@ -20,7 +56,7 @@ float bg_fp32_vector3_get_angle(const BgFP32Vector3* vector1, const BgFP32Vector return 0.0f; } - const float cosine = bg_fp32_vector3_dot_product(vector1, vector2) / sqrtf(square_modulus1 * square_modulus2); + const float cosine = bg_fp32_vector3_scalar_product(vector1, vector2) / sqrtf(square_modulus1 * square_modulus2); if (cosine >= 1.0f - BG_FP32_EPSYLON) { return 0.0f; @@ -51,7 +87,7 @@ double bg_fp64_vector3_get_angle(const BgFP64Vector3* vector1, const BgFP64Vecto return 0.0; } - const double cosine = bg_fp64_vector3_dot_product(vector1, vector2) / sqrt(square_modulus1 * square_modulus2); + const double cosine = bg_fp64_vector3_scalar_product(vector1, vector2) / sqrt(square_modulus1 * square_modulus2); if (cosine >= 1.0 - BG_FP64_EPSYLON) { return 0.0; diff --git a/basic-geometry/vector3.h b/basic-geometry/vector3.h index 290dcce..33af716 100644 --- a/basic-geometry/vector3.h +++ b/basic-geometry/vector3.h @@ -164,66 +164,62 @@ static inline int bg_fp64_vector3_is_unit(const BgFP64Vector3* vector) // ==================== Add ===================== // -static inline void bg_fp32_vector3_add(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, BgFP32Vector3* result) +static inline void bg_fp32_vector3_add(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, BgFP32Vector3* sum) { - result->x1 = vector1->x1 + vector2->x1; - result->x2 = vector1->x2 + vector2->x2; - result->x3 = vector1->x3 + vector2->x3; + sum->x1 = vector1->x1 + vector2->x1; + sum->x2 = vector1->x2 + vector2->x2; + sum->x3 = vector1->x3 + vector2->x3; } -static inline void bg_fp64_vector3_add(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, BgFP64Vector3* result) +static inline void bg_fp64_vector3_add(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, BgFP64Vector3* sum) { - result->x1 = vector1->x1 + vector2->x1; - result->x2 = vector1->x2 + vector2->x2; - result->x3 = vector1->x3 + vector2->x3; + sum->x1 = vector1->x1 + vector2->x1; + sum->x2 = vector1->x2 + vector2->x2; + sum->x3 = vector1->x3 + vector2->x3; } // ================ Subtraction ================= // -static inline void bg_fp32_vector3_subtract(const BgFP32Vector3* minuend, const BgFP32Vector3* subtrahend, BgFP32Vector3* result) +static inline void bg_fp32_vector3_subtract(const BgFP32Vector3* minuend, const BgFP32Vector3* subtrahend, BgFP32Vector3* difference) { - result->x1 = minuend->x1 - subtrahend->x1; - result->x2 = minuend->x2 - subtrahend->x2; - result->x3 = minuend->x3 - subtrahend->x3; + difference->x1 = minuend->x1 - subtrahend->x1; + difference->x2 = minuend->x2 - subtrahend->x2; + difference->x3 = minuend->x3 - subtrahend->x3; } -static inline void bg_fp64_vector3_subtract(const BgFP64Vector3* minuend, const BgFP64Vector3* subtrahend, BgFP64Vector3* result) +static inline void bg_fp64_vector3_subtract(const BgFP64Vector3* minuend, const BgFP64Vector3* subtrahend, BgFP64Vector3* difference) { - result->x1 = minuend->x1 - subtrahend->x1; - result->x2 = minuend->x2 - subtrahend->x2; - result->x3 = minuend->x3 - subtrahend->x3; + difference->x1 = minuend->x1 - subtrahend->x1; + difference->x2 = minuend->x2 - subtrahend->x2; + difference->x3 = minuend->x3 - subtrahend->x3; } // =============== Multiplication =============== // -static inline void bg_fp32_vector3_multiply(const BgFP32Vector3* multiplicand, const float multiplier, BgFP32Vector3* result) +static inline void bg_fp32_vector3_multiply(const BgFP32Vector3* multiplicand, const float multiplier, BgFP32Vector3* product) { - result->x1 = multiplicand->x1 * multiplier; - result->x2 = multiplicand->x2 * multiplier; - result->x3 = multiplicand->x3 * multiplier; + product->x1 = multiplicand->x1 * multiplier; + product->x2 = multiplicand->x2 * multiplier; + product->x3 = multiplicand->x3 * multiplier; } -static inline void bg_fp64_vector3_multiply(const BgFP64Vector3* multiplicand, const double multiplier, BgFP64Vector3* result) +static inline void bg_fp64_vector3_multiply(const BgFP64Vector3* multiplicand, const double multiplier, BgFP64Vector3* product) { - result->x1 = multiplicand->x1 * multiplier; - result->x2 = multiplicand->x2 * multiplier; - result->x3 = multiplicand->x3 * multiplier; + product->x1 = multiplicand->x1 * multiplier; + product->x2 = multiplicand->x2 * multiplier; + product->x3 = multiplicand->x3 * multiplier; } // ================== Division ================== // -static inline void bg_fp32_vector3_divide(const BgFP32Vector3* dividend, const float divisor, BgFP32Vector3* result) +static inline void bg_fp32_vector3_divide(const BgFP32Vector3* dividend, const float divisor, BgFP32Vector3* quotient) { - result->x1 = dividend->x1 / divisor; - result->x2 = dividend->x2 / divisor; - result->x3 = dividend->x3 / divisor; + bg_fp32_vector3_multiply(dividend, 1.0f / divisor, quotient); } -static inline void bg_fp64_vector3_divide(const BgFP64Vector3* dividend, const double divisor, BgFP64Vector3* result) +static inline void bg_fp64_vector3_divide(const BgFP64Vector3* dividend, const double divisor, BgFP64Vector3* quotient) { - result->x1 = dividend->x1 / divisor; - result->x2 = dividend->x2 / divisor; - result->x3 = dividend->x3 / divisor; + bg_fp64_vector3_multiply(dividend, 1.0 / divisor, quotient); } // ================ Append scaled =============== // @@ -276,12 +272,12 @@ static inline void bg_fp64_vector3_get_mean3(const BgFP64Vector3* vector1, const // =============== Scalar Product =============== // -static inline float bg_fp32_vector3_dot_product(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2) +static inline float bg_fp32_vector3_scalar_product(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2) { return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2 + vector1->x3 * vector2->x3; } -static inline double bg_fp64_vector3_dot_product(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2) +static inline double bg_fp64_vector3_scalar_product(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2) { return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2 + vector1->x3 * vector2->x3; } @@ -306,30 +302,32 @@ static inline double bg_fp64_vector3_triple_product(const BgFP64Vector3* vector1 static inline void bg_fp32_vector3_cross_product(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, BgFP32Vector3* result) { - bg_fp32_vector3_set_values( - vector1->x2 * vector2->x3 - vector1->x3 * vector2->x2, - vector1->x3 * vector2->x1 - vector1->x1 * vector2->x3, - vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1, - result - ); + const float x1 = vector1->x2 * vector2->x3 - vector1->x3 * vector2->x2; + const float x2 = vector1->x3 * vector2->x1 - vector1->x1 * vector2->x3; + const float x3 = vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1; + + result->x1 = x1; + result->x2 = x2; + result->x3 = x3; } static inline void bg_fp64_vector3_cross_product(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, BgFP64Vector3* result) { - bg_fp64_vector3_set_values( - vector1->x2 * vector2->x3 - vector1->x3 * vector2->x2, - vector1->x3 * vector2->x1 - vector1->x1 * vector2->x3, - vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1, - result - ); + const double x1 = vector1->x2 * vector2->x3 - vector1->x3 * vector2->x2; + const double x2 = vector1->x3 * vector2->x1 - vector1->x1 * vector2->x3; + const double x3 = vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1; + + result->x1 = x1; + result->x2 = x2; + result->x3 = x3; } // ============ Double Cross Product ============ // static inline void bg_fp32_vector3_double_cross_product(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, const BgFP32Vector3* vector3, BgFP32Vector3* result) { - const float ac = bg_fp32_vector3_dot_product(vector1, vector3); - const float ab = bg_fp32_vector3_dot_product(vector1, vector2); + const float ac = bg_fp32_vector3_scalar_product(vector1, vector3); + const float ab = bg_fp32_vector3_scalar_product(vector1, vector2); result->x1 = vector2->x1 * ac - vector3->x1 * ab; result->x2 = vector2->x2 * ac - vector3->x2 * ab; @@ -338,8 +336,8 @@ static inline void bg_fp32_vector3_double_cross_product(const BgFP32Vector3* vec static inline void bg_fp64_vector3_double_cross(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, const BgFP64Vector3* vector3, BgFP64Vector3* result) { - const double ac = bg_fp64_vector3_dot_product(vector1, vector3); - const double ab = bg_fp64_vector3_dot_product(vector1, vector2); + const double ac = bg_fp64_vector3_scalar_product(vector1, vector3); + const double ab = bg_fp64_vector3_scalar_product(vector1, vector2); result->x1 = vector2->x1 * ac - vector3->x1 * ab; result->x2 = vector2->x2 * ac - vector3->x2 * ab; @@ -348,39 +346,9 @@ static inline void bg_fp64_vector3_double_cross(const BgFP64Vector3* vector1, co // =============== Normalization ================ // -static inline int bg_fp32_vector3_normalize(BgFP32Vector3* vector) -{ - const float square_modulus = bg_fp32_vector3_get_square_modulus(vector); +int bg_fp32_vector3_normalize(BgFP32Vector3* vector); - if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) { - return 1; - } - - if (square_modulus <= BG_FP32_SQUARE_EPSYLON) { - bg_fp32_vector3_reset(vector); - return 0; - } - - bg_fp32_vector3_divide(vector, sqrtf(square_modulus), vector); - return 1; -} - -static inline int bg_fp64_vector3_normalize(BgFP64Vector3* vector) -{ - const double square_modulus = bg_fp64_vector3_get_square_modulus(vector); - - if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) { - return 1; - } - - if (square_modulus <= BG_FP64_SQUARE_EPSYLON) { - bg_fp64_vector3_reset(vector); - return 0; - } - - bg_fp64_vector3_divide(vector, sqrt(square_modulus), vector); - return 1; -} +int bg_fp64_vector3_normalize(BgFP64Vector3* vector); // =============== Get Normalized =============== // diff --git a/basic-geometry/versor.c b/basic-geometry/versor.c index 00c3936..cae0c0a 100644 --- a/basic-geometry/versor.c +++ b/basic-geometry/versor.c @@ -7,9 +7,24 @@ const BgFP32Versor BG_FP32_IDLE_VERSOR = { 1.0f, 0.0f, 0.0f, 0.0f }; const BgFP64Versor BG_FP64_IDLE_VERSOR = { 1.0, 0.0, 0.0, 0.0 }; -void __bg_fp32_versor_normalize(const float square_modulus, __BgFP32DarkTwinVersor* twin) +// ==================== Set ===================== // + +void bg_fp32_versor_set_values(const float s0, const float x1, const float x2, const float x3, BgFP32Versor* versor) { - if (square_modulus <= BG_FP32_SQUARE_EPSYLON || (twin->s0 * twin->s0) >= (1.0f - BG_FP32_TWO_EPSYLON) * square_modulus) { + __BgFP32DarkTwinVersor* twin = (__BgFP32DarkTwinVersor*)versor; + + twin->s0 = s0; + twin->x1 = x1; + twin->x2 = x2; + twin->x3 = x3; + + const float square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + + if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) { + return; + } + + if (square_modulus <= BG_FP32_SQUARE_EPSYLON) { twin->s0 = 1.0f; twin->x1 = 0.0f; twin->x2 = 0.0f; @@ -17,17 +32,30 @@ void __bg_fp32_versor_normalize(const float square_modulus, __BgFP32DarkTwinVers return; } - const float modulus = sqrtf(square_modulus); + const float multiplier = sqrtf(1.0f / square_modulus); - twin->s0 /= modulus; - twin->x1 /= modulus; - twin->x2 /= modulus; - twin->x3 /= modulus; + twin->s0 *= multiplier; + twin->x1 *= multiplier; + twin->x2 *= multiplier; + twin->x3 *= multiplier; } -void __bg_fp64_versor_normalize(const double square_modulus, __BgFP64DarkTwinVersor* twin) +void bg_fp64_versor_set_values(const double s0, const double x1, const double x2, const double x3, BgFP64Versor* versor) { - if (square_modulus <= BG_FP64_SQUARE_EPSYLON || (twin->s0 * twin->s0) >= (1.0 - BG_FP64_TWO_EPSYLON) * square_modulus) { + __BgFP64DarkTwinVersor* twin = (__BgFP64DarkTwinVersor*)versor; + + twin->s0 = s0; + twin->x1 = x1; + twin->x2 = x2; + twin->x3 = x3; + + const double square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + + if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) { + return; + } + + if (square_modulus <= BG_FP64_SQUARE_EPSYLON) { twin->s0 = 1.0; twin->x1 = 0.0; twin->x2 = 0.0; @@ -35,12 +63,12 @@ void __bg_fp64_versor_normalize(const double square_modulus, __BgFP64DarkTwinVer return; } - const double modulus = sqrt(square_modulus); + const double multiplier = sqrt(1.0 / square_modulus); - twin->s0 /= modulus; - twin->x1 /= modulus; - twin->x2 /= modulus; - twin->x3 /= modulus; + twin->s0 *= multiplier; + twin->x1 *= multiplier; + twin->x2 *= multiplier; + twin->x3 *= multiplier; } // =============== Set Crude Turn =============== // @@ -108,11 +136,11 @@ void bg_fp32_versor_get_rotation(const BgFP32Versor* versor, BgFP32Rotation3* re result->radians = 2.0f * acosf(versor->s0 / sqrtf(versor->s0 * versor->s0 + square_vector)); - const float vector_modulus = sqrtf(square_vector); + const float multiplier = sqrtf(1.0f / square_vector); - result->axis.x1 = versor->x1 / vector_modulus; - result->axis.x2 = versor->x2 / vector_modulus; - result->axis.x3 = versor->x3 / vector_modulus; + result->axis.x1 = versor->x1 * multiplier; + result->axis.x2 = versor->x2 * multiplier; + result->axis.x3 = versor->x3 * multiplier; } void bg_fp64_versor_get_rotation(const BgFP64Versor* versor, BgFP64Rotation3* result) @@ -130,9 +158,264 @@ void bg_fp64_versor_get_rotation(const BgFP64Versor* versor, BgFP64Rotation3* re result->radians = 2.0 * acos(versor->s0 / sqrt(versor->s0 * versor->s0 + square_vector)); - const double vector_modulus = sqrt(square_vector); + const double multiplier = sqrt(1.0 / square_vector); - result->axis.x1 = versor->x1 / vector_modulus; - result->axis.x2 = versor->x2 / vector_modulus; - result->axis.x3 = versor->x3 / vector_modulus; + result->axis.x1 = versor->x1 * multiplier; + result->axis.x2 = versor->x2 * multiplier; + result->axis.x3 = versor->x3 * multiplier; } + +// ================ Combination ================= // + +void bg_fp32_versor_combine(const BgFP32Versor* second, const BgFP32Versor* first, BgFP32Versor* result) +{ + const float s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3); + const float x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3); + const float x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1); + const float x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2); + + const float square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + + __BgFP32DarkTwinVersor* twin = (__BgFP32DarkTwinVersor*)result; + + twin->s0 = s0; + twin->x1 = x1; + twin->x2 = x2; + twin->x3 = x3; + + if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) { + return; + } + + if (square_modulus <= BG_FP32_SQUARE_EPSYLON) { + twin->s0 = 1.0f; + twin->x1 = 0.0f; + twin->x2 = 0.0f; + twin->x3 = 0.0f; + return; + } + + const float multiplier = sqrtf(1.0f / square_modulus); + + twin->s0 *= multiplier; + twin->x1 *= multiplier; + twin->x2 *= multiplier; + twin->x3 *= multiplier; +} + +void bg_fp64_versor_combine(const BgFP64Versor* second, const BgFP64Versor* first, BgFP64Versor* result) +{ + const double s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3); + const double x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3); + const double x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1); + const double x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2); + + const double square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + + __BgFP64DarkTwinVersor* twin = (__BgFP64DarkTwinVersor*)result; + + twin->s0 = s0; + twin->x1 = x1; + twin->x2 = x2; + twin->x3 = x3; + + if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) { + return; + } + + if (square_modulus <= BG_FP64_SQUARE_EPSYLON) { + twin->s0 = 1.0; + twin->x1 = 0.0; + twin->x2 = 0.0; + twin->x3 = 0.0; + return; + } + + const double multiplier = sqrt(1.0 / square_modulus); + + twin->s0 *= multiplier; + twin->x1 *= multiplier; + twin->x2 *= multiplier; + twin->x3 *= multiplier; +} + +// =========== Make Rotation Matrix3x3 ========== // + +void bg_fp32_versor_get_rotation_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix) +{ + const float s0s0 = versor->s0 * versor->s0; + const float x1x1 = versor->x1 * versor->x1; + const float x2x2 = versor->x2 * versor->x2; + const float x3x3 = versor->x3 * versor->x3; + + const float s0x1 = 2.0f * versor->s0 * versor->x1; + const float s0x2 = 2.0f * versor->s0 * versor->x2; + const float s0x3 = 2.0f * versor->s0 * versor->x3; + + const float x1x2 = 2.0f * versor->x1 * versor->x2; + const float x1x3 = 2.0f * versor->x1 * versor->x3; + const float x2x3 = 2.0f * versor->x2 * versor->x3; + + matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); + matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); + matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); + + matrix->r1c2 = x1x2 - s0x3; + matrix->r2c3 = x2x3 - s0x1; + matrix->r3c1 = x1x3 - s0x2; + + matrix->r2c1 = x1x2 + s0x3; + matrix->r3c2 = x2x3 + s0x1; + matrix->r1c3 = x1x3 + s0x2; +} + +void bg_fp64_versor_get_rotation_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix) +{ + const double s0s0 = versor->s0 * versor->s0; + const double x1x1 = versor->x1 * versor->x1; + const double x2x2 = versor->x2 * versor->x2; + const double x3x3 = versor->x3 * versor->x3; + + const double s0x1 = 2.0 * versor->s0 * versor->x1; + const double s0x2 = 2.0 * versor->s0 * versor->x2; + const double s0x3 = 2.0 * versor->s0 * versor->x3; + + const double x1x2 = 2.0 * versor->x1 * versor->x2; + const double x1x3 = 2.0 * versor->x1 * versor->x3; + const double x2x3 = 2.0 * versor->x2 * versor->x3; + + matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); + matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); + matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); + + matrix->r1c2 = x1x2 - s0x3; + matrix->r2c3 = x2x3 - s0x1; + matrix->r3c1 = x1x3 - s0x2; + + matrix->r2c1 = x1x2 + s0x3; + matrix->r3c2 = x2x3 + s0x1; + matrix->r1c3 = x1x3 + s0x2; +} + +// =========== Make Reverse Matrix3x3 =========== // + +void bg_fp32_versor_get_reverse_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix) +{ + const float s0s0 = versor->s0 * versor->s0; + const float x1x1 = versor->x1 * versor->x1; + const float x2x2 = versor->x2 * versor->x2; + const float x3x3 = versor->x3 * versor->x3; + + const float s0x1 = 2.0f * versor->s0 * versor->x1; + const float s0x2 = 2.0f * versor->s0 * versor->x2; + const float s0x3 = 2.0f * versor->s0 * versor->x3; + + const float x1x2 = 2.0f * versor->x1 * versor->x2; + const float x1x3 = 2.0f * versor->x1 * versor->x3; + const float x2x3 = 2.0f * versor->x2 * versor->x3; + + matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); + matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); + matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); + + matrix->r1c2 = x1x2 + s0x3; + matrix->r2c3 = x2x3 + s0x1; + matrix->r3c1 = x1x3 + s0x2; + + matrix->r2c1 = x1x2 - s0x3; + matrix->r3c2 = x2x3 - s0x1; + matrix->r1c3 = x1x3 - s0x2; +} + +void bg_fp64_versor_get_reverse_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix) +{ + const double s0s0 = versor->s0 * versor->s0; + const double x1x1 = versor->x1 * versor->x1; + const double x2x2 = versor->x2 * versor->x2; + const double x3x3 = versor->x3 * versor->x3; + + const double s0x1 = 2.0 * versor->s0 * versor->x1; + const double s0x2 = 2.0 * versor->s0 * versor->x2; + const double s0x3 = 2.0 * versor->s0 * versor->x3; + + const double x1x2 = 2.0 * versor->x1 * versor->x2; + const double x1x3 = 2.0 * versor->x1 * versor->x3; + const double x2x3 = 2.0 * versor->x2 * versor->x3; + + matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); + matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); + matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); + + matrix->r1c2 = x1x2 + s0x3; + matrix->r2c3 = x2x3 + s0x1; + matrix->r3c1 = x1x3 + s0x2; + + matrix->r2c1 = x1x2 - s0x3; + matrix->r3c2 = x2x3 - s0x1; + matrix->r1c3 = x1x3 - s0x2; +} + +// ================ Turn Vector ================= // + +void bg_fp32_versor_turn(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result) +{ + const float tx1 = 2.0f * (versor->x2 * vector->x3 - versor->x3 * vector->x2); + const float tx2 = 2.0f * (versor->x3 * vector->x1 - versor->x1 * vector->x3); + const float tx3 = 2.0f * (versor->x1 * vector->x2 - versor->x2 * vector->x1); + + const float x1 = (vector->x1 + tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2); + const float x2 = (vector->x2 + tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3); + const float x3 = (vector->x3 + tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1); + + result->x1 = x1; + result->x2 = x2; + result->x3 = x3; +} + +void bg_fp64_versor_turn(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result) +{ + const double tx1 = 2.0 * (versor->x2 * vector->x3 - versor->x3 * vector->x2); + const double tx2 = 2.0 * (versor->x3 * vector->x1 - versor->x1 * vector->x3); + const double tx3 = 2.0 * (versor->x1 * vector->x2 - versor->x2 * vector->x1); + + const double x1 = (vector->x1 + tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2); + const double x2 = (vector->x2 + tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3); + const double x3 = (vector->x3 + tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1); + + result->x1 = x1; + result->x2 = x2; + result->x3 = x3; +} + +// ============== Turn Vector Back ============== // + +void bg_fp32_versor_turn_back(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result) +{ + const float tx1 = 2.0f * (versor->x2 * vector->x3 - versor->x3 * vector->x2); + const float tx2 = 2.0f * (versor->x3 * vector->x1 - versor->x1 * vector->x3); + const float tx3 = 2.0f * (versor->x1 * vector->x2 - versor->x2 * vector->x1); + + const float x1 = (vector->x1 - tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2); + const float x2 = (vector->x2 - tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3); + const float x3 = (vector->x3 - tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1); + + result->x1 = x1; + result->x2 = x2; + result->x3 = x3; +} + +void bg_fp64_versor_turn_back(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result) +{ + const double tx1 = 2.0 * (versor->x2 * vector->x3 - versor->x3 * vector->x2); + const double tx2 = 2.0 * (versor->x3 * vector->x1 - versor->x1 * vector->x3); + const double tx3 = 2.0 * (versor->x1 * vector->x2 - versor->x2 * vector->x1); + + const double x1 = (vector->x1 - tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2); + const double x2 = (vector->x2 - tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3); + const double x3 = (vector->x3 - tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1); + + result->x1 = x1; + result->x2 = x2; + result->x3 = x3; +} + diff --git a/basic-geometry/versor.h b/basic-geometry/versor.h index 517cf10..913d919 100644 --- a/basic-geometry/versor.h +++ b/basic-geometry/versor.h @@ -58,45 +58,9 @@ static inline void bg_fp64_versor_reset(BgFP64Versor* versor) // ==================== Set ===================== // -void __bg_fp32_versor_normalize(const float square_modulus, __BgFP32DarkTwinVersor* twin); +void bg_fp32_versor_set_values(const float s0, const float x1, const float x2, const float x3, BgFP32Versor* versor); -void __bg_fp64_versor_normalize(const double square_modulus, __BgFP64DarkTwinVersor* twin); - -static inline void bg_fp32_versor_set_values(const float s0, const float x1, const float x2, const float x3, BgFP32Versor* versor) -{ - __BgFP32DarkTwinVersor* twin = (__BgFP32DarkTwinVersor*)versor; - - twin->s0 = s0; - twin->x1 = x1; - twin->x2 = x2; - twin->x3 = x3; - - const float square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) { - return; - } - - __bg_fp32_versor_normalize(square_modulus, (__BgFP32DarkTwinVersor*)versor); -} - -static inline void bg_fp64_versor_set_values(const double s0, const double x1, const double x2, const double x3, BgFP64Versor* versor) -{ - __BgFP64DarkTwinVersor* twin = (__BgFP64DarkTwinVersor*)versor; - - twin->s0 = s0; - twin->x1 = x1; - twin->x2 = x2; - twin->x3 = x3; - - const double square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) { - return; - } - - __bg_fp64_versor_normalize(square_modulus, twin); -} +void bg_fp64_versor_set_values(const double s0, const double x1, const double x2, const double x3, BgFP64Versor* versor); // ==================== Copy ==================== // @@ -152,24 +116,24 @@ static inline void bg_fp64_versor_set_rotation(const BgFP64Rotation3* rotation, // =============== Square modulus =============== // -static inline int bg_fp32_versor_get_square_modulus(const BgFP32Versor* versor) +static inline float bg_fp32_versor_get_square_modulus(const BgFP32Versor* versor) { return (versor->s0 * versor->s0 + versor->x1 * versor->x1) + (versor->x2 * versor->x2 + versor->x3 * versor->x3); } -static inline int bg_fp64_versor_get_square_modulus(const BgFP64Versor* versor) +static inline double bg_fp64_versor_get_square_modulus(const BgFP64Versor* versor) { return (versor->s0 * versor->s0 + versor->x1 * versor->x1) + (versor->x2 * versor->x2 + versor->x3 * versor->x3); } // =================== Modulus ================== // -static inline int bg_fp32_versor_get_modulus(const BgFP32Versor* versor) +static inline float bg_fp32_versor_get_modulus(const BgFP32Versor* versor) { return sqrtf(bg_fp32_versor_get_square_modulus(versor)); } -static inline int bg_fp64_versor_get_modulus(const BgFP64Versor* versor) +static inline double bg_fp64_versor_get_modulus(const BgFP64Versor* versor) { return sqrt(bg_fp64_versor_get_square_modulus(versor)); } @@ -274,51 +238,9 @@ static inline void bg_fp64_versor_set_inverted_fp32(const BgFP32Versor* versor, // ================ Combination ================= // -static inline void bg_fp32_versor_combine(const BgFP32Versor* second, const BgFP32Versor* first, BgFP32Versor* result) -{ - const float s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3); - const float x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3); - const float x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1); - const float x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2); +void bg_fp32_versor_combine(const BgFP32Versor* second, const BgFP32Versor* first, BgFP32Versor* result); - const float square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - __BgFP32DarkTwinVersor* twin = (__BgFP32DarkTwinVersor*)result; - - twin->s0 = s0; - twin->x1 = x1; - twin->x2 = x2; - twin->x3 = x3; - - if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) { - return; - } - - __bg_fp32_versor_normalize(square_modulus, twin); -} - -static inline void bg_fp64_versor_combine(const BgFP64Versor* second, const BgFP64Versor* first, BgFP64Versor* result) -{ - const double s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3); - const double x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3); - const double x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1); - const double x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2); - - const double square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - __BgFP64DarkTwinVersor* twin = (__BgFP64DarkTwinVersor*)result; - - twin->s0 = s0; - twin->x1 = x1; - twin->x2 = x2; - twin->x3 = x3; - - if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) { - return; - } - - __bg_fp64_versor_normalize(square_modulus, twin); -} +void bg_fp64_versor_combine(const BgFP64Versor* second, const BgFP64Versor* first, BgFP64Versor* result); // ================= Rotation3 ================== // @@ -328,330 +250,26 @@ void bg_fp64_versor_get_rotation(const BgFP64Versor* versor, BgFP64Rotation3* re // =========== Make Rotation Matrix3x3 ========== // -static inline void bg_fp32_versor_get_rotation_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix) -{ - const float s0s0 = versor->s0 * versor->s0; - const float x1x1 = versor->x1 * versor->x1; - const float x2x2 = versor->x2 * versor->x2; - const float x3x3 = versor->x3 * versor->x3; +void bg_fp32_versor_get_rotation_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix); - const float s0x1 = 2.0f * versor->s0 * versor->x1; - const float s0x2 = 2.0f * versor->s0 * versor->x2; - const float s0x3 = 2.0f * versor->s0 * versor->x3; - - const float x1x2 = 2.0f * versor->x1 * versor->x2; - const float x1x3 = 2.0f * versor->x1 * versor->x3; - const float x2x3 = 2.0f * versor->x2 * versor->x3; - - matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); - matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); - matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); - - matrix->r1c2 = x1x2 - s0x3; - matrix->r2c3 = x2x3 - s0x1; - matrix->r3c1 = x1x3 - s0x2; - - matrix->r2c1 = x1x2 + s0x3; - matrix->r3c2 = x2x3 + s0x1; - matrix->r1c3 = x1x3 + s0x2; -} - -static inline void bg_fp64_versor_get_rotation_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix) -{ - const double s0s0 = versor->s0 * versor->s0; - const double x1x1 = versor->x1 * versor->x1; - const double x2x2 = versor->x2 * versor->x2; - const double x3x3 = versor->x3 * versor->x3; - - const double s0x1 = 2.0 * versor->s0 * versor->x1; - const double s0x2 = 2.0 * versor->s0 * versor->x2; - const double s0x3 = 2.0 * versor->s0 * versor->x3; - - const double x1x2 = 2.0 * versor->x1 * versor->x2; - const double x1x3 = 2.0 * versor->x1 * versor->x3; - const double x2x3 = 2.0 * versor->x2 * versor->x3; - - matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); - matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); - matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); - - matrix->r1c2 = x1x2 - s0x3; - matrix->r2c3 = x2x3 - s0x1; - matrix->r3c1 = x1x3 - s0x2; - - matrix->r2c1 = x1x2 + s0x3; - matrix->r3c2 = x2x3 + s0x1; - matrix->r1c3 = x1x3 + s0x2; -} +void bg_fp64_versor_get_rotation_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix); // =========== Make Reverse Matrix3x3 =========== // -static inline void bg_fp32_versor_get_reverse_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix) -{ - const float s0s0 = versor->s0 * versor->s0; - const float x1x1 = versor->x1 * versor->x1; - const float x2x2 = versor->x2 * versor->x2; - const float x3x3 = versor->x3 * versor->x3; +void bg_fp32_versor_get_reverse_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix); - const float s0x1 = 2.0f * versor->s0 * versor->x1; - const float s0x2 = 2.0f * versor->s0 * versor->x2; - const float s0x3 = 2.0f * versor->s0 * versor->x3; - - const float x1x2 = 2.0f * versor->x1 * versor->x2; - const float x1x3 = 2.0f * versor->x1 * versor->x3; - const float x2x3 = 2.0f * versor->x2 * versor->x3; - - matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); - matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); - matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); - - matrix->r1c2 = x1x2 + s0x3; - matrix->r2c3 = x2x3 + s0x1; - matrix->r3c1 = x1x3 + s0x2; - - matrix->r2c1 = x1x2 - s0x3; - matrix->r3c2 = x2x3 - s0x1; - matrix->r1c3 = x1x3 - s0x2; -} - -static inline void bg_fp64_versor_get_reverse_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix) -{ - const double s0s0 = versor->s0 * versor->s0; - const double x1x1 = versor->x1 * versor->x1; - const double x2x2 = versor->x2 * versor->x2; - const double x3x3 = versor->x3 * versor->x3; - - const double s0x1 = 2.0 * versor->s0 * versor->x1; - const double s0x2 = 2.0 * versor->s0 * versor->x2; - const double s0x3 = 2.0 * versor->s0 * versor->x3; - - const double x1x2 = 2.0 * versor->x1 * versor->x2; - const double x1x3 = 2.0 * versor->x1 * versor->x3; - const double x2x3 = 2.0 * versor->x2 * versor->x3; - - matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); - matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); - matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); - - matrix->r1c2 = x1x2 + s0x3; - matrix->r2c3 = x2x3 + s0x1; - matrix->r3c1 = x1x3 + s0x2; - - matrix->r2c1 = x1x2 - s0x3; - matrix->r3c2 = x2x3 - s0x1; - matrix->r1c3 = x1x3 - s0x2; -} +void bg_fp64_versor_get_reverse_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix); // ================ Turn Vector ================= // -static inline void bg_fp32_versor_turn(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result) -{ - const float tx1 = 2.0f * (versor->x2 * vector->x3 - versor->x3 * vector->x2); - const float tx2 = 2.0f * (versor->x3 * vector->x1 - versor->x1 * vector->x3); - const float tx3 = 2.0f * (versor->x1 * vector->x2 - versor->x2 * vector->x1); +void bg_fp32_versor_turn(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result); - const float x1 = (vector->x1 + tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2); - const float x2 = (vector->x2 + tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3); - const float x3 = (vector->x3 + tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1); - - result->x1 = x1; - result->x2 = x2; - result->x3 = x3; -} - -static inline void bg_fp64_versor_turn(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result) -{ - const double tx1 = 2.0 * (versor->x2 * vector->x3 - versor->x3 * vector->x2); - const double tx2 = 2.0 * (versor->x3 * vector->x1 - versor->x1 * vector->x3); - const double tx3 = 2.0 * (versor->x1 * vector->x2 - versor->x2 * vector->x1); - - const double x1 = (vector->x1 + tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2); - const double x2 = (vector->x2 + tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3); - const double x3 = (vector->x3 + tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1); - - result->x1 = x1; - result->x2 = x2; - result->x3 = x3; -} - -// ================ Turn2 Vector ================ // - -static inline void bg_fp32_versor_turn2(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result) -{ - const float s0s0 = versor->s0 * versor->s0; - const float x1x1 = versor->x1 * versor->x1; - const float x2x2 = versor->x2 * versor->x2; - const float x3x3 = versor->x3 * versor->x3; - - const float s0x1 = 2.0f * versor->s0 * versor->x1; - const float s0x2 = 2.0f * versor->s0 * versor->x2; - const float s0x3 = 2.0f * versor->s0 * versor->x3; - - const float x1x2 = 2.0f * versor->x1 * versor->x2; - const float x1x3 = 2.0f * versor->x1 * versor->x3; - const float x2x3 = 2.0f * versor->x2 * versor->x3; - - const float r2c1 = x1x2 + s0x3; - const float r3c2 = x2x3 + s0x1; - const float r1c3 = x1x3 + s0x2; - - const float r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); - const float r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); - const float r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); - - const float r1c2 = x1x2 - s0x3; - const float r2c3 = x2x3 - s0x1; - const float r3c1 = x1x3 - s0x2; - - const float x1 = r1c1 * vector->x1 + r1c2 * vector->x2 + r1c3 * vector->x3; - const float x2 = r2c1 * vector->x1 + r2c2 * vector->x2 + r2c3 * vector->x3; - const float x3 = r3c1 * vector->x1 + r3c2 * vector->x2 + r3c3 * vector->x3; - - result->x1 = x1; - result->x2 = x2; - result->x3 = x3; -} - -static inline void bg_fp64_versor_turn2(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result) -{ - const double s0s0 = versor->s0 * versor->s0; - const double x1x1 = versor->x1 * versor->x1; - const double x2x2 = versor->x2 * versor->x2; - const double x3x3 = versor->x3 * versor->x3; - - const double s0x1 = 2.0f * versor->s0 * versor->x1; - const double s0x2 = 2.0f * versor->s0 * versor->x2; - const double s0x3 = 2.0f * versor->s0 * versor->x3; - - const double x1x2 = 2.0f * versor->x1 * versor->x2; - const double x1x3 = 2.0f * versor->x1 * versor->x3; - const double x2x3 = 2.0f * versor->x2 * versor->x3; - - const double r2c1 = x1x2 + s0x3; - const double r3c2 = x2x3 + s0x1; - const double r1c3 = x1x3 + s0x2; - - const double r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); - const double r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); - const double r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); - - const double r1c2 = x1x2 - s0x3; - const double r2c3 = x2x3 - s0x1; - const double r3c1 = x1x3 - s0x2; - - const double x1 = r1c1 * vector->x1 + r1c2 * vector->x2 + r1c3 * vector->x3; - const double x2 = r2c1 * vector->x1 + r2c2 * vector->x2 + r2c3 * vector->x3; - const double x3 = r3c1 * vector->x1 + r3c2 * vector->x2 + r3c3 * vector->x3; - - result->x1 = x1; - result->x2 = x2; - result->x3 = x3; -} +void bg_fp64_versor_turn(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result); // ============== Turn Vector Back ============== // -static inline void bg_fp32_versor_turn_back(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result) -{ - const float tx1 = 2.0f * (versor->x2 * vector->x3 - versor->x3 * vector->x2); - const float tx2 = 2.0f * (versor->x3 * vector->x1 - versor->x1 * vector->x3); - const float tx3 = 2.0f * (versor->x1 * vector->x2 - versor->x2 * vector->x1); +void bg_fp32_versor_turn_back(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result); - const float x1 = (vector->x1 - tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2); - const float x2 = (vector->x2 - tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3); - const float x3 = (vector->x3 - tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1); - - result->x1 = x1; - result->x2 = x2; - result->x3 = x3; -} - -static inline void bg_fp64_versor_turn_back(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result) -{ - const double tx1 = 2.0 * (versor->x2 * vector->x3 - versor->x3 * vector->x2); - const double tx2 = 2.0 * (versor->x3 * vector->x1 - versor->x1 * vector->x3); - const double tx3 = 2.0 * (versor->x1 * vector->x2 - versor->x2 * vector->x1); - - const double x1 = (vector->x1 - tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2); - const double x2 = (vector->x2 - tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3); - const double x3 = (vector->x3 - tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1); - - result->x1 = x1; - result->x2 = x2; - result->x3 = x3; -} - -// ============== Turn Vector Back2 ============= // - -static inline void bg_fp32_versor_turn_back2(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result) -{ - const float s0s0 = versor->s0 * versor->s0; - const float x1x1 = versor->x1 * versor->x1; - const float x2x2 = versor->x2 * versor->x2; - const float x3x3 = versor->x3 * versor->x3; - - const float s0x1 = 2.0f * versor->s0 * versor->x1; - const float s0x2 = 2.0f * versor->s0 * versor->x2; - const float s0x3 = 2.0f * versor->s0 * versor->x3; - - const float x1x2 = 2.0f * versor->x1 * versor->x2; - const float x1x3 = 2.0f * versor->x1 * versor->x3; - const float x2x3 = 2.0f * versor->x2 * versor->x3; - - const float r1c2 = x1x2 + s0x3; - const float r2c3 = x2x3 + s0x1; - const float r3c1 = x1x3 + s0x2; - - const float r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); - const float r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); - const float r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); - - const float r2c1 = x1x2 - s0x3; - const float r3c2 = x2x3 - s0x1; - const float r1c3 = x1x3 - s0x2; - - const float x1 = r1c1 * vector->x1 + r1c2 * vector->x2 + r1c3 * vector->x3; - const float x2 = r2c1 * vector->x1 + r2c2 * vector->x2 + r2c3 * vector->x3; - const float x3 = r3c1 * vector->x1 + r3c2 * vector->x2 + r3c3 * vector->x3; - - result->x1 = x1; - result->x2 = x2; - result->x3 = x3; -} - -static inline void bg_fp64_versor_turn_back2(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result) -{ - const double s0s0 = versor->s0 * versor->s0; - const double x1x1 = versor->x1 * versor->x1; - const double x2x2 = versor->x2 * versor->x2; - const double x3x3 = versor->x3 * versor->x3; - - const double s0x1 = 2.0f * versor->s0 * versor->x1; - const double s0x2 = 2.0f * versor->s0 * versor->x2; - const double s0x3 = 2.0f * versor->s0 * versor->x3; - - const double x1x2 = 2.0f * versor->x1 * versor->x2; - const double x1x3 = 2.0f * versor->x1 * versor->x3; - const double x2x3 = 2.0f * versor->x2 * versor->x3; - - const double r1c2 = x1x2 + s0x3; - const double r2c3 = x2x3 + s0x1; - const double r3c1 = x1x3 + s0x2; - - const double r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); - const double r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); - const double r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); - - const double r2c1 = x1x2 - s0x3; - const double r3c2 = x2x3 - s0x1; - const double r1c3 = x1x3 - s0x2; - - const double x1 = r1c1 * vector->x1 + r1c2 * vector->x2 + r1c3 * vector->x3; - const double x2 = r2c1 * vector->x1 + r2c2 * vector->x2 + r2c3 * vector->x3; - const double x3 = r3c1 * vector->x1 + r3c2 * vector->x2 + r3c3 * vector->x3; - - result->x1 = x1; - result->x2 = x2; - result->x3 = x3; -} +void bg_fp64_versor_turn_back(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result); #endif