Добавление swap функций для векторов, кватернионов и матриц / Swap functions have been added for vectors, quaternions and matrixes

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)

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)

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)

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)

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)

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)

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)

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)

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);