From 5fd14e4627926d13a77469769087359246e2640b Mon Sep 17 00:00:00 2001
From: Andrey Pokidov <andrey.pokidov@gmail.com>
Date: Wed, 27 Nov 2024 16:49:58 +0700
Subject: [PATCH] =?UTF-8?q?=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=B5?=
 =?UTF-8?q?=D0=BD=D0=B8=D0=B5=20swap=20=D1=84=D1=83=D0=BD=D0=BA=D1=86?=
 =?UTF-8?q?=D0=B8=D0=B9=20=D0=B4=D0=BB=D1=8F=20=D0=B2=D0=B5=D0=BA=D1=82?=
 =?UTF-8?q?=D0=BE=D1=80=D0=BE=D0=B2,=20=D0=BA=D0=B2=D0=B0=D1=82=D0=B5?=
 =?UTF-8?q?=D1=80=D0=BD=D0=B8=D0=BE=D0=BD=D0=BE=D0=B2=20=D0=B8=20=D0=BC?=
 =?UTF-8?q?=D0=B0=D1=82=D1=80=D0=B8=D1=86=20/=20Swap=20functions=20have=20?=
 =?UTF-8?q?been=20added=20for=20vectors,=20quaternions=20and=20matrixes?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 basic-geometry-dev/main.c   |  1 +
 basic-geometry/matrix2x2.h  | 44 ++++++++++++++++++++
 basic-geometry/matrix2x3.h  | 62 ++++++++++++++++++++++++++++
 basic-geometry/matrix3x2.h  | 80 +++++++++++++++++++++++++++++++++++++
 basic-geometry/matrix3x3.h  | 80 +++++++++++++++++++++++++++++++++++++
 basic-geometry/quaternion.h | 38 ++++++++++++++++++
 basic-geometry/vector2.h    | 26 ++++++++++++
 basic-geometry/vector3.h    | 32 +++++++++++++++
 basic-geometry/versor.c     | 10 -----
 basic-geometry/versor.h     |  2 -
 10 files changed, 363 insertions(+), 12 deletions(-)

diff --git a/basic-geometry-dev/main.c b/basic-geometry-dev/main.c
index 2126a87..4cdb30f 100644
--- a/basic-geometry-dev/main.c
+++ b/basic-geometry-dev/main.c
@@ -236,6 +236,7 @@ int main()
             bg_fp32_versor_combine(&versors1[i], &versors2[i], &results[i]);
             bg_fp32_versor_get_rotation_matrix(&versors1[i], &matrixes[i]);
             bg_fp32_matrix3x3_right_product(&matrixes[i], &vectors[i], &vectors[i]);
+            //bg_fp32_versor_turn(&results[i], &vectors[i], &vectors[i]);
         }
     }
 
diff --git a/basic-geometry/matrix2x2.h b/basic-geometry/matrix2x2.h
index 8c7f5ac..f28f19a 100644
--- a/basic-geometry/matrix2x2.h
+++ b/basic-geometry/matrix2x2.h
@@ -105,6 +105,50 @@ static inline void bg_fp64_matrix2x2_copy(const BgFP64Matrix2x2* from, BgFP64Mat
     to->r2c2 = from->r2c2;
 }
 
+// ==================== Swap ==================== //
+
+static inline void bg_fp32_matrix2x2_swap(BgFP32Matrix2x2* matrix1, BgFP32Matrix2x2* matrix2)
+{
+    const float r1c1 = matrix2->r1c1;
+    const float r1c2 = matrix2->r1c2;
+
+    const float r2c1 = matrix2->r2c1;
+    const float r2c2 = matrix2->r2c2;
+
+    matrix2->r1c1 = matrix1->r1c1;
+    matrix2->r1c2 = matrix1->r1c2;
+
+    matrix2->r2c1 = matrix1->r2c1;
+    matrix2->r2c2 = matrix1->r2c2;
+
+    matrix1->r1c1 = r1c1;
+    matrix1->r1c2 = r1c2;
+
+    matrix1->r2c1 = r2c1;
+    matrix1->r2c2 = r2c2;
+}
+
+static inline void bg_fp64_matrix2x2_swap(BgFP64Matrix2x2* matrix1, BgFP64Matrix2x2* matrix2)
+{
+    const double r1c1 = matrix2->r1c1;
+    const double r1c2 = matrix2->r1c2;
+
+    const double r2c1 = matrix2->r2c1;
+    const double r2c2 = matrix2->r2c2;
+
+    matrix2->r1c1 = matrix1->r1c1;
+    matrix2->r1c2 = matrix1->r1c2;
+
+    matrix2->r2c1 = matrix1->r2c1;
+    matrix2->r2c2 = matrix1->r2c2;
+
+    matrix1->r1c1 = r1c1;
+    matrix1->r1c2 = r1c2;
+
+    matrix1->r2c1 = r2c1;
+    matrix1->r2c2 = r2c2;
+}
+
 // ============= Copy to twin type ============== //
 
 static inline void bg_fp32_matrix2x2_set_from_fp64(const BgFP64Matrix2x2* from, BgFP32Matrix2x2* to)
diff --git a/basic-geometry/matrix2x3.h b/basic-geometry/matrix2x3.h
index e3fa0fd..2f4e3a3 100644
--- a/basic-geometry/matrix2x3.h
+++ b/basic-geometry/matrix2x3.h
@@ -57,6 +57,68 @@ static inline void bg_fp64_matrix2x3_copy(const BgFP64Matrix2x3* from, BgFP64Mat
     to->r3c2 = from->r3c2;
 }
 
+// ==================== Swap ==================== //
+
+static inline void bg_fp32_matrix2x3_swap(BgFP32Matrix2x3* matrix1, BgFP32Matrix2x3* matrix2)
+{
+    const float r1c1 = matrix2->r1c1;
+    const float r1c2 = matrix2->r1c2;
+
+    const float r2c1 = matrix2->r2c1;
+    const float r2c2 = matrix2->r2c2;
+
+    const float r3c1 = matrix2->r3c1;
+    const float r3c2 = matrix2->r3c2;
+
+    matrix2->r1c1 = matrix1->r1c1;
+    matrix2->r1c2 = matrix1->r1c2;
+
+    matrix2->r2c1 = matrix1->r2c1;
+    matrix2->r2c2 = matrix1->r2c2;
+
+    matrix2->r3c1 = matrix1->r3c1;
+    matrix2->r3c2 = matrix1->r3c2;
+
+    matrix1->r1c1 = r1c1;
+    matrix1->r1c2 = r1c2;
+
+    matrix1->r2c1 = r2c1;
+    matrix1->r2c2 = r2c2;
+
+    matrix1->r3c1 = r3c1;
+    matrix1->r3c2 = r3c2;
+}
+
+static inline void bg_fp64_matrix2x3_swap(BgFP64Matrix2x3* matrix1, BgFP64Matrix2x3* matrix2)
+{
+    const double r1c1 = matrix2->r1c1;
+    const double r1c2 = matrix2->r1c2;
+
+    const double r2c1 = matrix2->r2c1;
+    const double r2c2 = matrix2->r2c2;
+
+    const double r3c1 = matrix2->r3c1;
+    const double r3c2 = matrix2->r3c2;
+
+    matrix2->r1c1 = matrix1->r1c1;
+    matrix2->r1c2 = matrix1->r1c2;
+
+    matrix2->r2c1 = matrix1->r2c1;
+    matrix2->r2c2 = matrix1->r2c2;
+
+    matrix2->r3c1 = matrix1->r3c1;
+    matrix2->r3c2 = matrix1->r3c2;
+
+    matrix1->r1c1 = r1c1;
+    matrix1->r1c2 = r1c2;
+
+    matrix1->r2c1 = r2c1;
+    matrix1->r2c2 = r2c2;
+
+    matrix1->r3c1 = r3c1;
+    matrix1->r3c2 = r3c2;
+}
+
 // ============= Copy to twin type ============== //
 
 static inline void bg_fp32_matrix2x3_set_from_fp64(const BgFP64Matrix2x3* from, BgFP32Matrix2x3* to)
diff --git a/basic-geometry/matrix3x2.h b/basic-geometry/matrix3x2.h
index 4f51e77..c40caff 100644
--- a/basic-geometry/matrix3x2.h
+++ b/basic-geometry/matrix3x2.h
@@ -29,6 +29,86 @@ static inline void bg_fp64_matrix3x2_reset(BgFP64Matrix3x2* matrix)
     matrix->r2c3 = 0.0;
 }
 
+// ==================== Copy ==================== //
+
+static inline void bg_fp32_matrix3x2_copy(const BgFP32Matrix3x2* from, BgFP32Matrix3x2* to)
+{
+    to->r1c1 = from->r1c1;
+    to->r1c2 = from->r1c2;
+    to->r1c3 = from->r1c3;
+
+    to->r2c1 = from->r2c1;
+    to->r2c2 = from->r2c2;
+    to->r2c3 = from->r2c3;
+}
+
+static inline void bg_fp64_matrix3x2_copy(const BgFP64Matrix3x2* from, BgFP64Matrix3x2* to)
+{
+    to->r1c1 = from->r1c1;
+    to->r1c2 = from->r1c2;
+    to->r1c3 = from->r1c3;
+
+    to->r2c1 = from->r2c1;
+    to->r2c2 = from->r2c2;
+    to->r2c3 = from->r2c3;
+}
+
+// ==================== Swap ==================== //
+
+static inline void bg_fp32_matrix3x2_swap(BgFP32Matrix3x2* matrix1, BgFP32Matrix3x2* matrix2)
+{
+    const float r1c1 = matrix2->r1c1;
+    const float r1c2 = matrix2->r1c2;
+    const float r1c3 = matrix2->r1c3;
+
+    const float r2c1 = matrix2->r2c1;
+    const float r2c2 = matrix2->r2c2;
+    const float r2c3 = matrix2->r2c3;
+
+    matrix2->r1c1 = matrix1->r1c1;
+    matrix2->r1c2 = matrix1->r1c2;
+    matrix2->r1c3 = matrix1->r1c3;
+
+    matrix2->r2c1 = matrix1->r2c1;
+    matrix2->r2c2 = matrix1->r2c2;
+    matrix2->r2c3 = matrix1->r2c3;
+
+    matrix1->r1c1 = r1c1;
+    matrix1->r1c2 = r1c2;
+    matrix1->r1c3 = r1c3;
+
+    matrix1->r2c1 = r2c1;
+    matrix1->r2c2 = r2c2;
+    matrix1->r2c3 = r2c3;
+}
+
+static inline void bg_fp64_matrix3x2_swap(BgFP64Matrix3x2* matrix1, BgFP64Matrix3x2* matrix2)
+{
+    const double r1c1 = matrix2->r1c1;
+    const double r1c2 = matrix2->r1c2;
+    const double r1c3 = matrix2->r1c3;
+
+    const double r2c1 = matrix2->r2c1;
+    const double r2c2 = matrix2->r2c2;
+    const double r2c3 = matrix2->r2c3;
+
+    matrix2->r1c1 = matrix1->r1c1;
+    matrix2->r1c2 = matrix1->r1c2;
+    matrix2->r1c3 = matrix1->r1c3;
+
+    matrix2->r2c1 = matrix1->r2c1;
+    matrix2->r2c2 = matrix1->r2c2;
+    matrix2->r2c3 = matrix1->r2c3;
+
+    matrix1->r1c1 = r1c1;
+    matrix1->r1c2 = r1c2;
+    matrix1->r1c3 = r1c3;
+
+    matrix1->r2c1 = r2c1;
+    matrix1->r2c2 = r2c2;
+    matrix1->r2c3 = r2c3;
+}
+
 // ============= Set from twin type ============= //
 
 static inline void bg_fp32_matrix3x2_set_from_fp64(const BgFP64Matrix3x2* from, BgFP32Matrix3x2* to)
diff --git a/basic-geometry/matrix3x3.h b/basic-geometry/matrix3x3.h
index b940c3d..1207cbd 100644
--- a/basic-geometry/matrix3x3.h
+++ b/basic-geometry/matrix3x3.h
@@ -132,6 +132,86 @@ static inline void bg_fp64_matrix3x3_copy(const BgFP64Matrix3x3* from, BgFP64Mat
     to->r3c3 = from->r3c3;
 }
 
+// ==================== Swap ==================== //
+
+static inline void bg_fp32_matrix3x3_swap(BgFP32Matrix3x3* matrix1, BgFP32Matrix3x3* matrix2)
+{
+    const float r1c1 = matrix2->r1c1;
+    const float r1c2 = matrix2->r1c2;
+    const float r1c3 = matrix2->r1c3;
+
+    const float r2c1 = matrix2->r2c1;
+    const float r2c2 = matrix2->r2c2;
+    const float r2c3 = matrix2->r2c3;
+
+    const float r3c1 = matrix2->r3c1;
+    const float r3c2 = matrix2->r3c2;
+    const float r3c3 = matrix2->r3c3;
+
+    matrix2->r1c1 = matrix1->r1c1;
+    matrix2->r1c2 = matrix1->r1c2;
+    matrix2->r1c3 = matrix1->r1c3;
+
+    matrix2->r2c1 = matrix1->r2c1;
+    matrix2->r2c2 = matrix1->r2c2;
+    matrix2->r2c3 = matrix1->r2c3;
+
+    matrix2->r3c1 = matrix1->r3c1;
+    matrix2->r3c2 = matrix1->r3c2;
+    matrix2->r3c3 = matrix1->r3c3;
+
+    matrix1->r1c1 = r1c1;
+    matrix1->r1c2 = r1c2;
+    matrix1->r1c3 = r1c3;
+
+    matrix1->r2c1 = r2c1;
+    matrix1->r2c2 = r2c2;
+    matrix1->r2c3 = r2c3;
+
+    matrix1->r3c1 = r3c1;
+    matrix1->r3c2 = r3c2;
+    matrix1->r3c3 = r3c3;
+}
+
+static inline void bg_fp64_matrix3x3_swap(BgFP64Matrix3x3* matrix1, BgFP64Matrix3x3* matrix2)
+{
+    const double r1c1 = matrix2->r1c1;
+    const double r1c2 = matrix2->r1c2;
+    const double r1c3 = matrix2->r1c3;
+
+    const double r2c1 = matrix2->r2c1;
+    const double r2c2 = matrix2->r2c2;
+    const double r2c3 = matrix2->r2c3;
+
+    const double r3c1 = matrix2->r3c1;
+    const double r3c2 = matrix2->r3c2;
+    const double r3c3 = matrix2->r3c3;
+
+    matrix2->r1c1 = matrix1->r1c1;
+    matrix2->r1c2 = matrix1->r1c2;
+    matrix2->r1c3 = matrix1->r1c3;
+
+    matrix2->r2c1 = matrix1->r2c1;
+    matrix2->r2c2 = matrix1->r2c2;
+    matrix2->r2c3 = matrix1->r2c3;
+
+    matrix2->r3c1 = matrix1->r3c1;
+    matrix2->r3c2 = matrix1->r3c2;
+    matrix2->r3c3 = matrix1->r3c3;
+
+    matrix1->r1c1 = r1c1;
+    matrix1->r1c2 = r1c2;
+    matrix1->r1c3 = r1c3;
+
+    matrix1->r2c1 = r2c1;
+    matrix1->r2c2 = r2c2;
+    matrix1->r2c3 = r2c3;
+
+    matrix1->r3c1 = r3c1;
+    matrix1->r3c2 = r3c2;
+    matrix1->r3c3 = r3c3;
+}
+
 // ============= Set from twin type ============= //
 
 static inline void bg_fp32_matrix3x3_set_from_fp64(const BgFP64Matrix3x3* from, BgFP32Matrix3x3* to)
diff --git a/basic-geometry/quaternion.h b/basic-geometry/quaternion.h
index 9fb324d..49f0b05 100644
--- a/basic-geometry/quaternion.h
+++ b/basic-geometry/quaternion.h
@@ -87,6 +87,44 @@ static inline void bg_fp64_quaternion_copy(const BgFP64Quaternion* from, BgFP64Q
     to->x3 = from->x3;
 }
 
+// ==================== Swap ==================== //
+
+static inline void bg_fp32_quaternion_swap(BgFP32Quaternion* quarternion1, BgFP32Quaternion* quarternion2)
+{
+    const float s0 = quarternion2->s0;
+    const float x1 = quarternion2->x1;
+    const float x2 = quarternion2->x2;
+    const float x3 = quarternion2->x3;
+
+    quarternion2->s0 = quarternion1->s0;
+    quarternion2->x1 = quarternion1->x1;
+    quarternion2->x2 = quarternion1->x2;
+    quarternion2->x3 = quarternion1->x3;
+
+    quarternion1->s0 = s0;
+    quarternion1->x1 = x1;
+    quarternion1->x2 = x2;
+    quarternion1->x3 = x3;
+}
+
+static inline void bg_fp64_quaternion_swap(BgFP64Quaternion* quarternion1, BgFP64Quaternion* quarternion2)
+{
+    const double s0 = quarternion2->s0;
+    const double x1 = quarternion2->x1;
+    const double x2 = quarternion2->x2;
+    const double x3 = quarternion2->x3;
+
+    quarternion2->s0 = quarternion1->s0;
+    quarternion2->x1 = quarternion1->x1;
+    quarternion2->x2 = quarternion1->x2;
+    quarternion2->x3 = quarternion1->x3;
+
+    quarternion1->s0 = s0;
+    quarternion1->x1 = x1;
+    quarternion1->x2 = x2;
+    quarternion1->x3 = x3;
+}
+
 // ============= Copy to twin type ============== //
 
 static inline void bg_fp32_quaternion_set_from_fp64(const BgFP64Quaternion* quaternion, BgFP32Quaternion* result)
diff --git a/basic-geometry/vector2.h b/basic-geometry/vector2.h
index e753c68..dda4d10 100644
--- a/basic-geometry/vector2.h
+++ b/basic-geometry/vector2.h
@@ -58,6 +58,32 @@ static inline void bg_fp64_vector2_copy(const BgFP64Vector2* from, BgFP64Vector2
     to->x2 = from->x2;
 }
 
+// ==================== Swap ==================== //
+
+static inline void bg_fp32_vector2_swap(BgFP32Vector2* vector1, BgFP32Vector2* vector2)
+{
+    const float x1 = vector2->x1;
+    const float x2 = vector2->x2;
+
+    vector2->x1 = vector1->x1;
+    vector2->x2 = vector1->x2;
+
+    vector1->x1 = x1;
+    vector1->x2 = x2;
+}
+
+static inline void bg_fp64_vector2_swap(BgFP64Vector2* vector1, BgFP64Vector2* vector2)
+{
+    const double x1 = vector2->x1;
+    const double x2 = vector2->x2;
+
+    vector2->x1 = vector1->x1;
+    vector2->x2 = vector1->x2;
+
+    vector1->x1 = x1;
+    vector1->x2 = x2;
+}
+
 // ============= Copy to twin type ============== //
 
 static inline void bg_fp32_vector2_set_from_fp64(const BgFP64Vector2* from, BgFP32Vector2* to)
diff --git a/basic-geometry/vector3.h b/basic-geometry/vector3.h
index 8c94245..d821919 100644
--- a/basic-geometry/vector3.h
+++ b/basic-geometry/vector3.h
@@ -66,6 +66,38 @@ static inline void bg_fp64_vector3_copy(const BgFP64Vector3* from, BgFP64Vector3
     to->x3 = from->x3;
 }
 
+// ==================== Swap ==================== //
+
+static inline void bg_fp32_vector3_swap(BgFP32Vector3* vector1, BgFP32Vector3* vector2)
+{
+    const float x1 = vector2->x1;
+    const float x2 = vector2->x2;
+    const float x3 = vector2->x3;
+
+    vector2->x1 = vector1->x1;
+    vector2->x2 = vector1->x2;
+    vector2->x3 = vector1->x3;
+
+    vector1->x1 = x1;
+    vector1->x2 = x2;
+    vector1->x3 = x3;
+}
+
+static inline void bg_fp64_vector3_swap(BgFP64Vector3* vector1, BgFP64Vector3* vector2)
+{
+    const double x1 = vector2->x1;
+    const double x2 = vector2->x2;
+    const double x3 = vector2->x3;
+
+    vector2->x1 = vector1->x1;
+    vector2->x2 = vector1->x2;
+    vector2->x3 = vector1->x3;
+
+    vector1->x1 = x1;
+    vector1->x2 = x2;
+    vector1->x3 = x3;
+}
+
 // ============= Copy to twin type ============== //
 
 static inline void bg_fp32_vector3_set_from_fp64(const BgFP64Vector3* from, BgFP32Vector3* to)
diff --git a/basic-geometry/versor.c b/basic-geometry/versor.c
index 0ff44cf..9a459a5 100644
--- a/basic-geometry/versor.c
+++ b/basic-geometry/versor.c
@@ -7,16 +7,6 @@ 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)
-{
-    const float multiplier = sqrtf(1.0f / square_modulus);
-
-    twin->s0 *= multiplier;
-    twin->x1 *= multiplier;
-    twin->x2 *= multiplier;
-    twin->x3 *= multiplier;
-}
-
 // =============== Set Crude Turn =============== //
 
 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)
diff --git a/basic-geometry/versor.h b/basic-geometry/versor.h
index 8e60b83..fdedb73 100644
--- a/basic-geometry/versor.h
+++ b/basic-geometry/versor.h
@@ -408,8 +408,6 @@ static inline void bg_fp64_versor_set_inverted_fp32(const BgFP32Versor* versor,
 
 // ================ Combination ================= //
 
-__declspec(noinline) void __bg_fp32_versor_normalize(const float square_modulus, __BgFP32DarkTwinVersor* twin);
-
 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);