From 23fcdc2c28daf6b3971f0379b1dbba407cb65576 Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Thu, 21 Nov 2024 14:12:51 +0700 Subject: [PATCH] =?UTF-8?q?=D0=98=D0=B7=D0=BC=D0=B5=D0=BD=D0=B5=D0=BD?= =?UTF-8?q?=D0=B8=D0=B5=20=D0=BF=D0=BE=D0=BB=D0=B5=D0=B9=20=D0=B2=D0=B5?= =?UTF-8?q?=D1=80=D1=81=D0=BE=D1=80=D0=B0=20=D0=BD=D0=B0=20=D0=BA=D0=BE?= =?UTF-8?q?=D0=BD=D1=81=D1=82=D0=B0=D0=BD=D1=82=D0=BD=D1=8B=D0=B5=20/=20Ma?= =?UTF-8?q?king=20versor's=20fields=20constant?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Geometry.workspace.layout | 1 - dev/geometry-dev.depend | 26 +- dev/geometry-dev.layout | 4 +- dev/main.c | 2 +- src/geometry.layout | 172 +++++++------ src/quaternion.c | 8 +- src/quaternion.h | 8 +- src/versor.c | 92 +++---- src/versor.h | 517 +++++++++++++++++++------------------- test/geometry-test.layout | 18 +- 10 files changed, 423 insertions(+), 425 deletions(-) diff --git a/Geometry.workspace.layout b/Geometry.workspace.layout index 4111ddb..e46768a 100644 --- a/Geometry.workspace.layout +++ b/Geometry.workspace.layout @@ -2,5 +2,4 @@ - diff --git a/dev/geometry-dev.depend b/dev/geometry-dev.depend index 3b4ed7f..bb3e634 100644 --- a/dev/geometry-dev.depend +++ b/dev/geometry-dev.depend @@ -1,5 +1,5 @@ # depslib dependency file v1.0 -1731995770 source:/home/andrey/Projects/Private/C/Geometry/dev/main.c +1732169779 source:/home/andrey/Projects/Private/C/Geometry/dev/main.c @@ -21,18 +21,18 @@ "quaternion.h" "versor.h" -1730476002 /home/andrey/Projects/Private/C/Geometry/src/basis.h +1732094620 /home/andrey/Projects/Private/C/Geometry/src/basis.h -1730392488 /home/andrey/Projects/Private/C/Geometry/src/angle.h +1732096335 /home/andrey/Projects/Private/C/Geometry/src/angle.h "basis.h" -1731673023 /home/andrey/Projects/Private/C/Geometry/src/vector2.h +1732095049 /home/andrey/Projects/Private/C/Geometry/src/vector2.h "basis.h" "angle.h" -1731673331 /home/andrey/Projects/Private/C/Geometry/src/vector3.h +1732095097 /home/andrey/Projects/Private/C/Geometry/src/vector3.h "basis.h" "angle.h" @@ -42,12 +42,12 @@ "angle.h" -1731673308 /home/andrey/Projects/Private/C/Geometry/src/matrix2x2.h +1732095748 /home/andrey/Projects/Private/C/Geometry/src/matrix2x2.h "angle.h" "vector2.h" "matrixes.h" -1731906002 /home/andrey/Projects/Private/C/Geometry/src/matrix3x3.h +1732095414 /home/andrey/Projects/Private/C/Geometry/src/matrix3x3.h "vector3.h" "matrixes.h" @@ -61,12 +61,12 @@ "vector2.h" "matrix2x2.h" -1730355414 /home/andrey/Projects/Private/C/Geometry/src/rotation3.h +1732095748 /home/andrey/Projects/Private/C/Geometry/src/rotation3.h "basis.h" "angle.h" "vector3.h" -1731995858 /home/andrey/Projects/Private/C/Geometry/src/versor.h +1732169837 /home/andrey/Projects/Private/C/Geometry/src/versor.h "basis.h" "vector3.h" @@ -107,19 +107,19 @@ "versor.h" "affine_map3.h" -1731906002 /home/andrey/Projects/Private/C/Geometry/src/matrixes.h +1732094620 /home/andrey/Projects/Private/C/Geometry/src/matrixes.h -1731906002 /home/andrey/Projects/Private/C/Geometry/src/matrix2x3.h +1732095530 /home/andrey/Projects/Private/C/Geometry/src/matrix2x3.h "vector2.h" "vector3.h" "matrixes.h" -1731906002 /home/andrey/Projects/Private/C/Geometry/src/matrix3x2.h +1732095530 /home/andrey/Projects/Private/C/Geometry/src/matrix3x2.h "vector2.h" "vector3.h" "matrixes.h" -1731407834 /home/andrey/Projects/Private/C/Geometry/src/quaternion.h +1732095234 /home/andrey/Projects/Private/C/Geometry/src/quaternion.h "basis.h" "matrix3x3.h" diff --git a/dev/geometry-dev.layout b/dev/geometry-dev.layout index 0efaadb..31c2b44 100644 --- a/dev/geometry-dev.layout +++ b/dev/geometry-dev.layout @@ -2,9 +2,9 @@ - + - + diff --git a/dev/main.c b/dev/main.c index 21c1e22..a1aedea 100644 --- a/dev/main.c +++ b/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("(%f, %f, %f, %f)\n", versor->s0, versor->x1, versor->x2, versor->x3); } void print_vector(const BgFP32Vector3* vector) diff --git a/src/geometry.layout b/src/geometry.layout index 360585a..5dc2255 100644 --- a/src/geometry.layout +++ b/src/geometry.layout @@ -2,9 +2,29 @@ - + - + + + + + + + + + + + + + + + + + + + + + @@ -12,49 +32,9 @@ - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + @@ -62,49 +42,79 @@ + + + + + + + + + + + + + + + - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/quaternion.c b/src/quaternion.c index cd27f12..a937eb3 100644 --- a/src/quaternion.c +++ b/src/quaternion.c @@ -2,7 +2,7 @@ // ============ Make Rotation Matrix ============ // -void bg_fp32_quaternion_make_rotation_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix) +void bg_fp32_quaternion_get_rotation_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix) { const float s0s0 = quaternion->s0 * quaternion->s0; const float x1x1 = quaternion->x1 * quaternion->x1; @@ -40,7 +40,7 @@ void bg_fp32_quaternion_make_rotation_matrix(const BgFP32Quaternion* quaternion, matrix->r1c3 = corrector2 * (x1x3 + s0x2); } -void bg_fp64_quaternion_make_rotation_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix) +void bg_fp64_quaternion_get_rotation_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix) { const double s0s0 = quaternion->s0 * quaternion->s0; const double x1x1 = quaternion->x1 * quaternion->x1; @@ -80,7 +80,7 @@ void bg_fp64_quaternion_make_rotation_matrix(const BgFP64Quaternion* quaternion, // ============ Make Reverse Matrix ============= // -void bg_fp32_quaternion_make_reverse_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix) +void bg_fp32_quaternion_get_reverse_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix) { const float s0s0 = quaternion->s0 * quaternion->s0; const float x1x1 = quaternion->x1 * quaternion->x1; @@ -118,7 +118,7 @@ void bg_fp32_quaternion_make_reverse_matrix(const BgFP32Quaternion* quaternion, matrix->r1c3 = corrector2 * (x1x3 - s0x2); } -void bg_fp64_quaternion_make_reverse_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix) +void bg_fp64_quaternion_get_reverse_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix) { const double s0s0 = quaternion->s0 * quaternion->s0; const double x1x1 = quaternion->x1 * quaternion->x1; diff --git a/src/quaternion.h b/src/quaternion.h index c9827b4..a44207a 100644 --- a/src/quaternion.h +++ b/src/quaternion.h @@ -158,15 +158,15 @@ static inline void bg_fp64_quaternion_set_conjugate_fp32(const BgFP32Quaternion* // ============ Make Rotation Matrix ============ // -void bg_fp32_quaternion_make_rotation_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix); +void bg_fp32_quaternion_get_rotation_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix); -void bg_fp64_quaternion_make_rotation_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix); +void bg_fp64_quaternion_get_rotation_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix); // ============ Make Reverse Matrix ============= // -void bg_fp32_quaternion_make_reverse_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix); +void bg_fp32_quaternion_get_reverse_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix); -void bg_fp64_quaternion_make_reverse_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix); +void bg_fp64_quaternion_get_reverse_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix); // ==================== Add ===================== // diff --git a/src/versor.c b/src/versor.c index cb33bdd..896e259 100644 --- a/src/versor.c +++ b/src/versor.c @@ -7,51 +7,45 @@ 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_module, BgFP32Versor* versor) +void __bg_fp32_versor_normalize(const float square_module, __BgFP32VersorDarkTwin* twin) { - if (square_module <= BG_FP32_SQUARE_EPSYLON) { - bg_fp32_versor_reset(versor); - return; - } - - if ((versor->_x1 * versor->_x1 + versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3) <= BG_FP32_SQUARE_EPSYLON * square_module) { - bg_fp32_versor_reset(versor); + if (square_module <= BG_FP32_SQUARE_EPSYLON || (twin->x1 * twin->x1 + twin->x2 * twin->x2 + twin->x3 * twin->x3) <= BG_FP32_SQUARE_EPSYLON * square_module) { + twin->s0 = 1.0f; + twin->x1 = 0.0f; + twin->x2 = 0.0f; + twin->x3 = 0.0f; return; } const float module = sqrtf(square_module); - versor->_s0 /= module; - versor->_x1 /= module; - versor->_x2 /= module; - versor->_x3 /= module; + twin->s0 /= module; + twin->x1 /= module; + twin->x2 /= module; + twin->x3 /= module; } -void __bg_fp64_versor_normalize(const double square_module, BgFP64Versor* versor) +void __bg_fp64_versor_normalize(const double square_module, __BgFP64VersorDarkTwin* twin) { - if (square_module <= BG_FP64_SQUARE_EPSYLON) { - bg_fp64_versor_reset(versor); - return; - } - - const double square_vector = versor->_x1 * versor->_x1 + versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3; - - if (square_vector <= BG_FP64_SQUARE_EPSYLON * square_module) { - bg_fp64_versor_reset(versor); + if (square_module <= BG_FP64_SQUARE_EPSYLON || (twin->x1 * twin->x1 + twin->x2 * twin->x2 + twin->x3 * twin->x3) <= BG_FP64_SQUARE_EPSYLON * square_module) { + twin->s0 = 1.0; + twin->x1 = 0.0; + twin->x2 = 0.0; + twin->x3 = 0.0; return; } const double module = sqrt(square_module); - versor->_s0 /= module; - versor->_x1 /= module; - versor->_x2 /= module; - versor->_x3 /= module; + twin->s0 /= module; + twin->x1 /= module; + twin->x2 /= module; + twin->x3 /= module; } -// ==================== Make ==================== // +// =============== Set Crude Turn =============== // -void bg_fp32_versor_make_from_crude_turn(const float x1, const float x2, const float x3, const float angle, const angle_unit_t unit, BgFP32Versor* result) +void bg_fp32_versor_set_crude_turn(const float x1, const float x2, const float x3, const float angle, const angle_unit_t unit, BgFP32Versor* result) { const float square_vector = x1 * x1 + x2 * x2 + x3 * x3; @@ -71,16 +65,10 @@ void bg_fp32_versor_make_from_crude_turn(const float x1, const float x2, const f const float multiplier = sine / sqrtf(square_vector); - bg_fp32_versor_set_values( - cosf(half_angle), - x1 * multiplier, - x2 * multiplier, - x3 * multiplier, - result - ); + bg_fp32_versor_set_values(cosf(half_angle), x1 * multiplier, x2 * multiplier, x3 * multiplier, result); } -void bg_fp64_versor_make_from_crude_turn(const double x1, const double x2, const double x3, const double angle, const angle_unit_t unit, BgFP64Versor* result) +void bg_fp64_versor_set_crude_turn(const double x1, const double x2, const double x3, const double angle, const angle_unit_t unit, BgFP64Versor* result) { const double square_vector = x1 * x1 + x2 * x2 + x3 * x3; @@ -100,13 +88,7 @@ void bg_fp64_versor_make_from_crude_turn(const double x1, const double x2, const const double multiplier = sine / sqrt(square_vector); - bg_fp64_versor_set_values( - cos(half_angle), - x1 * multiplier, - x2 * multiplier, - x3 * multiplier, - result - ); + bg_fp64_versor_set_values(cos(half_angle), x1 * multiplier, x2 * multiplier, x3 * multiplier, result); } // ================= Rotation3 ================== // @@ -117,20 +99,20 @@ void bg_fp32_versor_get_rotation(const BgFP32Versor* versor, BgFP32Rotation3* re return; } - if (versor->_s0 <= -(1.0f - BG_FP32_EPSYLON) || 1.0f - BG_FP32_EPSYLON <= versor->_s0) { + if (versor->s0 <= -(1.0f - BG_FP32_EPSYLON) || 1.0f - BG_FP32_EPSYLON <= versor->s0) { bg_fp32_rotation_reset(result); return; } - const float square_vector = versor->_x1 * versor->_x1 + versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3; + const float square_vector = versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3; - result->radians = 2.0f * acosf(versor->_s0 / sqrtf(versor->_s0 * versor->_s0 + square_vector)); + result->radians = 2.0f * acosf(versor->s0 / sqrtf(versor->s0 * versor->s0 + square_vector)); const float vector_module = sqrtf(square_vector); - result->axis.x1 = versor->_x1 / vector_module; - result->axis.x2 = versor->_x2 / vector_module; - result->axis.x3 = versor->_x3 / vector_module; + result->axis.x1 = versor->x1 / vector_module; + result->axis.x2 = versor->x2 / vector_module; + result->axis.x3 = versor->x3 / vector_module; } void bg_fp64_versor_get_rotation(const BgFP64Versor* versor, BgFP64Rotation3* result) @@ -139,18 +121,18 @@ void bg_fp64_versor_get_rotation(const BgFP64Versor* versor, BgFP64Rotation3* re return; } - if (versor->_s0 <= -(1.0 - BG_FP64_EPSYLON) || 1.0 - BG_FP64_EPSYLON <= versor->_s0) { + if (versor->s0 <= -(1.0 - BG_FP64_EPSYLON) || 1.0 - BG_FP64_EPSYLON <= versor->s0) { bg_fp64_rotation_reset(result); return; } - const double square_vector = versor->_x1 * versor->_x1 + versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3; + const double square_vector = versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3; - result->radians = 2.0 * acos(versor->_s0 / sqrt(versor->_s0 * versor->_s0 + square_vector)); + result->radians = 2.0 * acos(versor->s0 / sqrt(versor->s0 * versor->s0 + square_vector)); const double vector_module = sqrt(square_vector); - result->axis.x1 = versor->_x1 / vector_module; - result->axis.x2 = versor->_x2 / vector_module; - result->axis.x3 = versor->_x3 / vector_module; + result->axis.x1 = versor->x1 / vector_module; + result->axis.x2 = versor->x2 / vector_module; + result->axis.x3 = versor->x3 / vector_module; } diff --git a/src/versor.h b/src/versor.h index c2d244e..312c8f9 100644 --- a/src/versor.h +++ b/src/versor.h @@ -8,14 +8,28 @@ #include "rotation3.h" #include "matrix3x3.h" +// =================== Types ==================== // + typedef struct { - float _s0, _x1, _x2, _x3; + const float s0, x1, x2, x3; } BgFP32Versor; typedef struct { - double _s0, _x1, _x2, _x3; + const double s0, x1, x2, x3; } BgFP64Versor; +// ================= Dark Twins ================= // + +typedef struct { + float s0, x1, x2, x3; +} __BgFP32VersorDarkTwin; + +typedef struct { + double s0, x1, x2, x3; +} __BgFP64VersorDarkTwin; + +// ================= Constants ================== // + extern const BgFP32Versor BG_FP32_IDLE_VERSOR; extern const BgFP64Versor BG_FP64_IDLE_VERSOR; @@ -23,116 +37,59 @@ extern const BgFP64Versor BG_FP64_IDLE_VERSOR; static inline void bg_fp32_versor_reset(BgFP32Versor* versor) { - versor->_s0 = 1.0f; - versor->_x1 = 0.0f; - versor->_x2 = 0.0f; - versor->_x3 = 0.0f; + __BgFP32VersorDarkTwin* twin = (__BgFP32VersorDarkTwin*)versor; + + twin->s0 = 1.0f; + twin->x1 = 0.0f; + twin->x2 = 0.0f; + twin->x3 = 0.0f; } static inline void bg_fp64_versor_reset(BgFP64Versor* versor) { - versor->_s0 = 1.0; - versor->_x1 = 0.0; - versor->_x2 = 0.0; - versor->_x3 = 0.0; -} + __BgFP64VersorDarkTwin* twin = (__BgFP64VersorDarkTwin*)versor; -// ============== Get Scalar Part =============== // - -static inline float bg_fp32_versor_get_scalar(const BgFP32Versor* versor) -{ - return versor->_s0; -} - -static inline double bg_fp64_versor_get_scalar(const BgFP64Versor* versor) -{ - return versor->_s0; -} - -// ============== Get Vector Part =============== // - -static inline void bg_fp32_versor_get_vector_part(const BgFP32Versor* versor, BgFP32Vector3 * result) -{ - result->x1 = versor->_x1; - result->x2 = versor->_x2; - result->x3 = versor->_x3; -} - -static inline void bg_fp64_versor_get_vector_part(const BgFP64Versor* versor, BgFP64Vector3 * result) -{ - result->x1 = versor->_x1; - result->x2 = versor->_x2; - result->x3 = versor->_x3; -} - -// =================== Get x1 =================== // - -static inline float bg_fp32_versor_get_x1(const BgFP32Versor* versor) -{ - return versor->_x1; -} - -static inline double bg_fp64_versor_get_x1(const BgFP64Versor* versor) -{ - return versor->_x1; -} - -// =================== Get x2 =================== // - -static inline float bg_fp32_versor_get_x2(const BgFP32Versor* versor) -{ - return versor->_x2; -} - -static inline double bg_fp64_versor_get_x2(const BgFP64Versor* versor) -{ - return versor->_x2; -} - -// =================== Get x3 =================== // - - -static inline float bg_fp32_versor_get_x3(const BgFP32Versor* versor) -{ - return versor->_x3; -} - -static inline double bg_fp64_versor_get_x3(const BgFP64Versor* versor) -{ - return versor->_x3; + twin->s0 = 1.0; + twin->x1 = 0.0; + twin->x2 = 0.0; + twin->x3 = 0.0; } // ==================== Set ===================== // -void __bg_fp32_versor_normalize(const float square_module, BgFP32Versor* versor); +void __bg_fp32_versor_normalize(const float square_module, __BgFP32VersorDarkTwin* twin); -void __bg_fp64_versor_normalize(const double square_module, BgFP64Versor* versor); +void __bg_fp64_versor_normalize(const double square_module, __BgFP64VersorDarkTwin* twin); -static inline void bg_fp32_versor_set_values(const float s0, const float x1, const float x2, const float x3, BgFP32Versor* result) +static inline void bg_fp32_versor_set_values(const float s0, const float x1, const float x2, const float x3, BgFP32Versor* versor) { - result->_s0 = s0; - result->_x1 = x1; - result->_x2 = x2; - result->_x3 = x3; + __BgFP32VersorDarkTwin* twin = (__BgFP32VersorDarkTwin*)versor; + + twin->s0 = s0; + twin->x1 = x1; + twin->x2 = x2; + twin->x3 = x3; const float square_module = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); if (square_module < 1.0f - BG_FP32_TWO_EPSYLON || 1.0f + BG_FP32_TWO_EPSYLON < square_module) { - __bg_fp32_versor_normalize(square_module, result); + __bg_fp32_versor_normalize(square_module, (__BgFP32VersorDarkTwin*)versor); } } -static inline void bg_fp64_versor_set_values(const double s0, const double x1, const double x2, const double x3, BgFP64Versor* result) +static inline void bg_fp64_versor_set_values(const double s0, const double x1, const double x2, const double x3, BgFP64Versor* versor) { - result->_s0 = s0; - result->_x1 = x1; - result->_x2 = x2; - result->_x3 = x3; + __BgFP64VersorDarkTwin* twin = (__BgFP64VersorDarkTwin*)versor; + + twin->s0 = s0; + twin->x1 = x1; + twin->x2 = x2; + twin->x3 = x3; const double square_module = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); if (square_module < 1.0 - BG_FP64_TWO_EPSYLON || 1.0 + BG_FP64_TWO_EPSYLON < square_module) { - __bg_fp64_versor_normalize(square_module, result); + __bg_fp64_versor_normalize(square_module, twin); } } @@ -140,56 +97,64 @@ static inline void bg_fp64_versor_set_values(const double s0, const double x1, c static inline void bg_fp32_versor_copy(const BgFP32Versor* from, BgFP32Versor* to) { - to->_s0 = from->_s0; - to->_x1 = from->_x1; - to->_x2 = from->_x2; - to->_x3 = from->_x3; + __BgFP32VersorDarkTwin* twin = (__BgFP32VersorDarkTwin*)to; + + twin->s0 = from->s0; + twin->x1 = from->x1; + twin->x2 = from->x2; + twin->x3 = from->x3; } static inline void bg_fp64_versor_copy(const BgFP64Versor* from, BgFP64Versor* to) { - to->_s0 = from->_s0; - to->_x1 = from->_x1; - to->_x2 = from->_x2; - to->_x3 = from->_x3; + __BgFP64VersorDarkTwin* twin = (__BgFP64VersorDarkTwin*)to; + + twin->s0 = from->s0; + twin->x1 = from->x1; + twin->x2 = from->x2; + twin->x3 = from->x3; } -// ==================== Make ==================== // +// =============== Set Crude Turn =============== // -void bg_fp32_versor_make_from_crude_turn(const float x1, const float x2, const float x3, const float angle, const angle_unit_t unit, BgFP32Versor* result); +void bg_fp32_versor_set_crude_turn(const float x1, const float x2, const float x3, const float angle, const angle_unit_t unit, BgFP32Versor* result); -void bg_fp64_versor_make_from_crude_turn(const double x1, const double x2, const double x3, const double angle, const angle_unit_t unit, BgFP64Versor* result); +void bg_fp64_versor_set_crude_turn(const double x1, const double x2, const double x3, const double angle, const angle_unit_t unit, BgFP64Versor* result); -static inline void bg_fp32_versor_make_from_turn(const BgFP32Vector3* axis, const float angle, const angle_unit_t unit, BgFP32Versor* result) +// ================== Set Turn ================== // + +static inline void bg_fp32_versor_set_turn(const BgFP32Vector3* axis, const float angle, const angle_unit_t unit, BgFP32Versor* result) { - bg_fp32_versor_make_from_crude_turn(axis->x1, axis->x2, axis->x3, angle, unit, result); + bg_fp32_versor_set_crude_turn(axis->x1, axis->x2, axis->x3, angle, unit, result); } -static inline void bg_fp64_versor_make_from_turn(const BgFP64Vector3* axis, const double angle, const angle_unit_t unit, BgFP64Versor* result) +static inline void bg_fp64_versor_set_turn(const BgFP32Vector3* axis, const double angle, const angle_unit_t unit, BgFP64Versor* result) { - bg_fp64_versor_make_from_crude_turn(axis->x1, axis->x2, axis->x3, angle, unit, result); + bg_fp64_versor_set_crude_turn(axis->x1, axis->x2, axis->x3, angle, unit, result); } -static inline void bg_fp32_versor_make_from_rotation(const BgFP32Rotation3* rotation, BgFP32Versor* result) +// ================ Set Rotation ================ // + +static inline void bg_fp32_versor_set_rotation(const BgFP32Rotation3* rotation, BgFP32Versor* result) { - bg_fp32_versor_make_from_crude_turn(rotation->axis.x1, rotation->axis.x2, rotation->axis.x3, rotation->radians, BG_ANGLE_UNIT_RADIANS, result); + bg_fp32_versor_set_crude_turn(rotation->axis.x1, rotation->axis.x2, rotation->axis.x3, rotation->radians, BG_ANGLE_UNIT_RADIANS, result); } -static inline void bg_fp64_versor_make_from_rotation(const BgFP64Rotation3* rotation, BgFP64Versor* result) +static inline void bg_fp64_versor_set_rotation(const BgFP64Rotation3* rotation, BgFP64Versor* result) { - bg_fp64_versor_make_from_crude_turn(rotation->axis.x1, rotation->axis.x2, rotation->axis.x3, rotation->radians, BG_ANGLE_UNIT_RADIANS, result); + bg_fp64_versor_set_crude_turn(rotation->axis.x1, rotation->axis.x2, rotation->axis.x3, rotation->radians, BG_ANGLE_UNIT_RADIANS, result); } // ================= Comparison ================= // static inline int bg_fp32_versor_is_idle(const BgFP32Versor* versor) { - return 1.0f - BG_FP32_EPSYLON <= versor->_s0 || versor->_s0 <= -(1.0 - BG_FP32_EPSYLON); + return 1.0f - BG_FP32_EPSYLON <= versor->s0 || versor->s0 <= -(1.0 - BG_FP32_EPSYLON); } static inline int bg_fp64_versor_is_idle(const BgFP64Versor* versor) { - return 1.0 - BG_FP64_EPSYLON <= versor->_s0 || versor->_s0 <= -(1.0 - BG_FP64_EPSYLON); + return 1.0 - BG_FP64_EPSYLON <= versor->s0 || versor->s0 <= -(1.0 - BG_FP64_EPSYLON); } // ============= Copy to twin type ============== // @@ -197,10 +162,10 @@ static inline int bg_fp64_versor_is_idle(const BgFP64Versor* versor) static inline void bg_fp32_versor_set_from_fp64(const BgFP64Versor* versor, BgFP32Versor* result) { bg_fp32_versor_set_values( - (float) versor->_s0, - (float) versor->_x1, - (float) versor->_x2, - (float) versor->_x3, + (float) versor->s0, + (float) versor->x1, + (float) versor->x2, + (float) versor->x3, result ); } @@ -208,10 +173,10 @@ static inline void bg_fp32_versor_set_from_fp64(const BgFP64Versor* versor, BgFP static inline void bg_fp64_versor_set_from_fp32(const BgFP32Versor* versor, BgFP64Versor* result) { bg_fp64_versor_set_values( - versor->_s0, - versor->_x1, - versor->_x2, - versor->_x3, + versor->s0, + versor->x1, + versor->x2, + versor->x3, result ); } @@ -220,73 +185,105 @@ static inline void bg_fp64_versor_set_from_fp32(const BgFP32Versor* versor, BgFP static inline void bg_fp32_versor_invert(BgFP32Versor* versor) { - versor->_x1 = -versor->_x1; - versor->_x2 = -versor->_x2; - versor->_x3 = -versor->_x3; + __BgFP32VersorDarkTwin* twin = (__BgFP32VersorDarkTwin*)versor; + twin->x1 = -versor->x1; + twin->x2 = -versor->x2; + twin->x3 = -versor->x3; } static inline void bg_fp64_versor_invert(BgFP64Versor* versor) { - versor->_x1 = -versor->_x1; - versor->_x2 = -versor->_x2; - versor->_x3 = -versor->_x3; + __BgFP64VersorDarkTwin* twin = (__BgFP64VersorDarkTwin*)versor; + twin->x1 = -versor->x1; + twin->x2 = -versor->x2; + twin->x3 = -versor->x3; } -// ================ Make Inverted =============== // +// ================ Set Inverted ================ // -static inline void bg_fp32_versor_set_inverted(const BgFP32Versor* versor, BgFP32Versor* result) +static inline void bg_fp32_versor_set_inverted(const BgFP32Versor* versor, BgFP32Versor* to) { - result->_s0 = versor->_s0; - result->_x1 = -versor->_x1; - result->_x2 = -versor->_x2; - result->_x3 = -versor->_x3; + __BgFP32VersorDarkTwin* twin = (__BgFP32VersorDarkTwin*)to; + twin->s0 = versor->s0; + twin->x1 = -versor->x1; + twin->x2 = -versor->x2; + twin->x3 = -versor->x3; } -static inline void bg_fp64_versor_set_inverted(const BgFP64Versor* versor, BgFP64Versor* result) +static inline void bg_fp64_versor_set_inverted(const BgFP64Versor* versor, BgFP64Versor* to) { - result->_s0 = versor->_s0; - result->_x1 = -versor->_x1; - result->_x2 = -versor->_x2; - result->_x3 = -versor->_x3; + __BgFP64VersorDarkTwin* twin = (__BgFP64VersorDarkTwin*)to; + twin->s0 = versor->s0; + twin->x1 = -versor->x1; + twin->x2 = -versor->x2; + twin->x3 = -versor->x3; +} + +// ================ Set Inverted ================ // + +static inline void bg_fp32_versor_set_inverted_fp64(const BgFP64Versor* versor, BgFP32Versor* to) +{ + bg_fp32_versor_set_values( + (float) versor->s0, + (float) -versor->x1, + (float) -versor->x2, + (float) -versor->x3, + to + ); +} + +static inline void bg_fp64_versor_set_inverted_fp32(const BgFP32Versor* versor, BgFP64Versor* to) +{ + bg_fp64_versor_set_values( + versor->s0, + -versor->x1, + -versor->x2, + -versor->x3, + to + ); } // ================ 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); + 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_module = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - result->_s0 = s0; - result->_x1 = x1; - result->_x2 = x2; - result->_x3 = x3; + __BgFP32VersorDarkTwin* twin = (__BgFP32VersorDarkTwin*)result; + + twin->s0 = s0; + twin->x1 = x1; + twin->x2 = x2; + twin->x3 = x3; if (square_module < 1.0f - BG_FP32_TWO_EPSYLON || 1.0f + BG_FP32_TWO_EPSYLON < square_module) { - __bg_fp32_versor_normalize(square_module, result); + __bg_fp32_versor_normalize(square_module, 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 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_module = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - result->_s0 = s0; - result->_x1 = x1; - result->_x2 = x2; - result->_x3 = x3; + __BgFP64VersorDarkTwin* twin = (__BgFP64VersorDarkTwin*)result; + + twin->s0 = s0; + twin->x1 = x1; + twin->x2 = x2; + twin->x3 = x3; if (square_module < 1.0 - BG_FP64_TWO_EPSYLON || 1.0 + BG_FP64_TWO_EPSYLON < square_module) { - __bg_fp64_versor_normalize(square_module, result); + __bg_fp64_versor_normalize(square_module, twin); } } @@ -298,20 +295,20 @@ void bg_fp64_versor_get_rotation(const BgFP64Versor* versor, BgFP64Rotation3* re // =========== Make Rotation Matrix3x3 ========== // -static inline void bg_fp32_versor_make_rotation_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix) +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; + 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 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 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); @@ -326,20 +323,20 @@ static inline void bg_fp32_versor_make_rotation_matrix(const BgFP32Versor* verso matrix->r1c3 = x1x3 + s0x2; } -static inline void bg_fp64_versor_make_rotation_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix) +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 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 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; + 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); @@ -356,20 +353,20 @@ static inline void bg_fp64_versor_make_rotation_matrix(const BgFP64Versor* verso // =========== Make Reverse Matrix3x3 =========== // -static inline void bg_fp32_versor_make_reverse_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix) +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; + 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 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 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); @@ -384,20 +381,20 @@ static inline void bg_fp32_versor_make_reverse_matrix(const BgFP32Versor* versor matrix->r1c3 = x1x3 - s0x2; } -static inline void bg_fp64_versor_make_reverse_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix) +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 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 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; + 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); @@ -416,13 +413,13 @@ static inline void bg_fp64_versor_make_reverse_matrix(const BgFP64Versor* versor 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); + 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); + 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; @@ -431,13 +428,13 @@ static inline void bg_fp32_versor_turn(const BgFP32Versor* versor, const BgFP32V 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 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); + 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; @@ -448,18 +445,18 @@ static inline void bg_fp64_versor_turn(const BgFP64Versor* versor, const BgFP64V 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 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 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 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; @@ -484,18 +481,18 @@ static inline void bg_fp32_versor_turn2(const BgFP32Versor* versor, const BgFP32 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 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 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 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; @@ -522,13 +519,13 @@ static inline void bg_fp64_versor_turn2(const BgFP64Versor* versor, const BgFP64 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); + 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); + 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; @@ -537,13 +534,13 @@ static inline void bg_fp32_versor_turn_back(const BgFP32Versor* versor, const Bg 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 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); + 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; @@ -554,18 +551,18 @@ static inline void bg_fp64_versor_turn_back(const BgFP64Versor* versor, const Bg 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 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 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 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; @@ -590,18 +587,18 @@ static inline void bg_fp32_versor_turn_back2(const BgFP32Versor* versor, const B 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 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 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 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; diff --git a/test/geometry-test.layout b/test/geometry-test.layout index dd47169..e6201cb 100644 --- a/test/geometry-test.layout +++ b/test/geometry-test.layout @@ -2,14 +2,24 @@ - + - + - + - + + + + + + + + + + +