Реорганизация методов для версоров и тангентов

basic-geometry/tangent.c

@@ -3,3 +3,35 @@
 const BgcTangentFP32 BGC_IDLE_TANGENT_FP32 = { 1.0f, 0.0f };
 
 const BgcTangentFP64 BGC_IDLE_TANGENT_FP64 = { 1.0, 0.0 };
+
+void _bgc_tangent_normalize_fp32(const float square_modulus, _BgcDarkTwinTangentFP32* twin)
+{
+    // (square_modulus != square_modulus) is true when square_modulus is NaN
+
+    if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) {
+        twin->cos = 1.0f;
+        twin->sin = 0.0f;
+        return;
+    }
+
+    const float multiplier = sqrtf(1.0f / square_modulus);
+
+    twin->cos *= multiplier;
+    twin->sin *= multiplier;
+}
+
+void _bgc_tangent_normalize_fp64(const double square_modulus, _BgcDarkTwinTangentFP64* twin)
+{
+    // (square_modulus != square_modulus) is true when square_modulus is NaN
+
+    if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) {
+        twin->cos = 1.0;
+        twin->sin = 0.0;
+        return;
+    }
+
+    const double multiplier = sqrt(1.0 / square_modulus);
+
+    twin->cos *= multiplier;
+    twin->sin *= multiplier;
+}

basic-geometry/tangent.h

@@ -55,54 +55,40 @@ inline void bgc_tangent_reset_fp64(BgcTangentFP64* tangent)
 
 // ==================== Set ===================== //
 
+void _bgc_tangent_normalize_fp32(const float square_modulus, _BgcDarkTwinTangentFP32* twin);
+
+void _bgc_tangent_normalize_fp64(const double square_modulus, _BgcDarkTwinTangentFP64* twin);
+
 inline void bgc_tangent_set_values_fp32(const float x1, const float x2, BgcTangentFP32* tangent)
 {
-    const float square_module = x1 * x1 + x2 * x2;
+    const float square_modulus = x1 * x1 + x2 * x2;
 
     _BgcDarkTwinTangentFP32* twin = (_BgcDarkTwinTangentFP32*)tangent;
 
     twin->cos = x1;
     twin->sin = x2;
 
-    if (1.0f - BGC_TWO_EPSYLON_FP32 <= square_module && square_module <= 1.0f + BGC_TWO_EPSYLON_FP32) {
+    if (1.0f - BGC_TWO_EPSYLON_FP32 <= square_modulus && square_modulus <= 1.0f + BGC_TWO_EPSYLON_FP32) {
         return;
     }
 
-    if (square_module <= BGC_SQUARE_EPSYLON_FP32) {
-        twin->cos = 1.0f;
-        twin->sin = 0.0f;
-        return;
-    }
-
-    const float multiplier = sqrtf(1.0f / square_module);
-
-    twin->cos = x1 * multiplier;
-    twin->sin = x2 * multiplier;
+    _bgc_tangent_normalize_fp32(square_modulus, twin);
 }
 
 inline void bgc_tangent_set_values_fp64(const double x1, const double x2, BgcTangentFP64* tangent)
 {
-    const double square_module = x1 * x1 + x2 * x2;
+    const double square_modulus = x1 * x1 + x2 * x2;
 
     _BgcDarkTwinTangentFP64* twin = (_BgcDarkTwinTangentFP64*)tangent;
 
     twin->cos = x1;
     twin->sin = x2;
 
-    if (1.0 - BGC_TWO_EPSYLON_FP64 <= square_module && square_module <= 1.0 + BGC_TWO_EPSYLON_FP64) {
+    if (1.0 - BGC_TWO_EPSYLON_FP64 <= square_modulus && square_modulus <= 1.0 + BGC_TWO_EPSYLON_FP64) {
         return;
     }
 
-    if (square_module <= BGC_SQUARE_EPSYLON_FP64) {
-        twin->cos = 1.0;
-        twin->sin = 0.0;
-        return;
-    }
-
-    const double multiplier = sqrt(1.0 / square_module);
-
-    twin->cos = x1 * multiplier;
-    twin->sin = x2 * multiplier;
+    _bgc_tangent_normalize_fp64(square_modulus, twin);
 }
 
 // ==================== Copy ==================== //

basic-geometry/versor.c

@@ -7,6 +7,49 @@ const BgcVersorFP32 BGC_IDLE_VERSOR_FP32 = { 1.0f, 0.0f, 0.0f, 0.0f };
 
 const BgcVersorFP64 BGC_IDLE_VERSOR_FP64 = { 1.0, 0.0, 0.0, 0.0 };
 
+// =============== Normalization ================ //
+
+void _bgc_versor_normalize_fp32(const float square_modulus, _BgcDarkTwinVersorFP32* twin)
+{
+    // (square_modulus != square_modulus) is true when square_modulus is NaN
+
+    if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) {
+        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 _bgc_versor_normalize_fp64(const double square_modulus, _BgcDarkTwinVersorFP64* twin)
+{
+    // (square_modulus != square_modulus) is true when square_modulus is NaN
+
+    if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) {
+        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;
+}
+
 // =============== Set Crude Turn =============== //
 
 void bgc_versor_set_crude_turn_fp32(const float x1, const float x2, const float x3, const float angle, const BgcAngleUnitEnum unit, BgcVersorFP32* result)

basic-geometry/versor.h

@@ -58,6 +58,10 @@ inline void bgc_versor_reset_fp64(BgcVersorFP64* versor)
 
 // ==================== Set ===================== //
 
+void _bgc_versor_normalize_fp32(const float square_modulus, _BgcDarkTwinVersorFP32* twin);
+
+void _bgc_versor_normalize_fp64(const double square_modulus, _BgcDarkTwinVersorFP64* twin);
+