refactor: quat and vector normalization strategy (#34)

mrdav30 · web-flow · commit 0cf844f43896 · 2025-03-11T00:40:35.000-04:00
diff --git a/src/FixedMathSharp/Numerics/FixedQuaternion.cs b/src/FixedMathSharp/Numerics/FixedQuaternion.cs
@@ -182,19 +182,48 @@ public FixedQuaternion Rotated(Fixed64 sin, Fixed64 cos, Vector3d? axis = null)
 
         #region Quaternion Operations
 
+        /// <summary>
+        /// Checks if this vector has been normalized by checking if the magnitude is close to 1.
+        /// </summary>
+        public bool IsNormalized()
+        {
+            Fixed64 mag = GetMagnitude(this);
+            return FixedMath.Abs(mag - Fixed64.One) <= Fixed64.Epsilon;
+        }
+
+        public static Fixed64 GetMagnitude(FixedQuaternion q)
+        {
+            Fixed64 mag = (q.x * q.x) + (q.y * q.y) + (q.z * q.z) + (q.w * q.w);
+            // If rounding error caused the final magnitude to be slightly above 1, clamp it
+            if (mag > Fixed64.One && mag <= Fixed64.One + Fixed64.Epsilon)
+                return Fixed64.One;
+
+            return mag != Fixed64.Zero ? FixedMath.Sqrt(mag) : Fixed64.Zero;
+        }
+
         /// <summary>
         /// Normalizes the quaternion to a unit quaternion.
         /// </summary>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static FixedQuaternion GetNormalized(FixedQuaternion q)
         {
-            Fixed64 mag = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
-            if (mag > Fixed64.Zero && mag != Fixed64.One)
-            {
-                Fixed64 invMagnitude = Fixed64.One / FixedMath.Sqrt(mag);
-                return new FixedQuaternion(q.x * invMagnitude, q.y * invMagnitude, q.z * invMagnitude, q.w * invMagnitude);
-            }
-            return q;
+            Fixed64 mag = GetMagnitude(q);
+
+            // If magnitude is zero, return identity quaternion (to avoid divide by zero)
+            if (mag == Fixed64.Zero)
+                return new FixedQuaternion(Fixed64.Zero, Fixed64.Zero, Fixed64.Zero, Fixed64.One);
+
+            // If already normalized, return as-is
+            if (mag == Fixed64.One)
+                return q;
+
+            // Normalize it exactly
+            return new FixedQuaternion(
+                q.x / mag,
+                q.y / mag,
+                q.z / mag,
+                q.w / mag
+            );
         }
 
         /// <summary>
@@ -343,7 +372,7 @@ public static FixedQuaternion FromAxisAngle(Vector3d axis, Fixed64 angle)
         public static FixedQuaternion FromEulerAnglesInDegrees(Fixed64 pitch, Fixed64 yaw, Fixed64 roll)
         {
             // Convert input angles from degrees to radians
-            pitch = FixedMath.DegToRad(pitch); 
+            pitch = FixedMath.DegToRad(pitch);
             yaw = FixedMath.DegToRad(yaw);
             roll = FixedMath.DegToRad(roll);
 
@@ -432,8 +461,8 @@ public static Vector3d QuaternionLog(FixedQuaternion q)
         /// - Finally, it divides by `deltaTime` to compute the angular velocity.
         /// </remarks>
         public static Vector3d ToAngularVelocity(
-            FixedQuaternion currentRotation, 
-            FixedQuaternion previousRotation, 
+            FixedQuaternion currentRotation,
+            FixedQuaternion previousRotation,
             Fixed64 deltaTime)
         {
             FixedQuaternion rotationDelta = currentRotation * previousRotation.Inverse();
diff --git a/src/FixedMathSharp/Numerics/Vector2d.cs b/src/FixedMathSharp/Numerics/Vector2d.cs
@@ -335,12 +335,23 @@ public Vector2d Normalize()
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public Vector2d Normalize(out Fixed64 mag)
         {
-            mag = Magnitude;
-            if (mag > Fixed64.Zero && mag != Fixed64.One)
+            mag = GetMagnitude(this);
+
+            // If magnitude is zero, return a zero vector to avoid divide-by-zero errors
+            if (mag == Fixed64.Zero)
             {
-                x /= mag;
-                y /= mag;
+                x = Fixed64.Zero;
+                y = Fixed64.Zero;
+                return this;
             }
+
+            // If already normalized, return as-is
+            if (mag == Fixed64.One)
+                return this;
+
+            x /= mag;
+            y /= mag;
+
             return this;
         }
 
@@ -604,13 +615,19 @@ public Fixed64 SqrDistance(Vector2d other)
         public static Vector2d GetNormalized(Vector2d value)
         {
             Fixed64 mag = GetMagnitude(value);
-            if (mag > Fixed64.Zero && mag != Fixed64.One)
-            {
-                Fixed64 xM = value.x / mag;
-                Fixed64 yM = value.y / mag;
-                return new Vector2d(xM, yM);
-            }
-            return value;
+
+            if (mag == Fixed64.Zero)
+                return new Vector2d(Fixed64.Zero, Fixed64.Zero);
+
+            // If already normalized, return as-is
+            if (mag == Fixed64.One)
+                return value;
+
+            // Normalize it exactly
+            return new Vector2d(
+                value.x / mag,
+                value.y / mag
+            );
         }
 
         /// <summary>
@@ -621,8 +638,13 @@ public static Vector2d GetNormalized(Vector2d value)
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static Fixed64 GetMagnitude(Vector2d vector)
         {
-            Fixed64 temp1 = (vector.x * vector.x) + (vector.y * vector.y);
-            return temp1.Abs() > Fixed64.Zero ? FixedMath.Sqrt(temp1) : Fixed64.Zero;
+            Fixed64 mag = (vector.x * vector.x) + (vector.y * vector.y);
+
+            // If rounding error pushed magnitude slightly above 1, clamp it
+            if (mag > Fixed64.One && mag <= Fixed64.One + Fixed64.Epsilon)
+                return Fixed64.One;
+
+            return mag.Abs() > Fixed64.Zero ? FixedMath.Sqrt(mag) : Fixed64.Zero;
         }
 
         /// <summary>
diff --git a/src/FixedMathSharp/Numerics/Vector3d.cs b/src/FixedMathSharp/Numerics/Vector3d.cs
@@ -374,17 +374,26 @@ public Vector3d Normalize()
         /// If the vector is zero-length or already normalized, no operation is performed, but the original magnitude will still be output.
         /// </remarks>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public Vector3d Normalize(out Fixed64 m)
+        public Vector3d Normalize(out Fixed64 mag)
         {
-            Fixed64 mag = Magnitude;
-            if (mag > Fixed64.Zero && mag != Fixed64.One)
+            mag = GetMagnitude(this);
+
+            // If magnitude is zero, return a zero vector to avoid divide-by-zero errors
+            if (mag == Fixed64.Zero)
             {
-                x /= mag;
-                y /= mag;
-                z /= mag;
+                x = Fixed64.Zero;
+                y = Fixed64.Zero;
+                z = Fixed64.Zero;
+                return this;
             }
 
-            m = mag;
+            // If already normalized, return as-is
+            if (mag == Fixed64.One)
+                return this;
+
+            x /= mag;
+            y /= mag;
+            z /= mag;
 
             return this;
         }
@@ -394,7 +403,7 @@ public Vector3d Normalize(out Fixed64 m)
         /// </summary>
         public bool IsNormalized()
         {
-            return Magnitude.Round() - Fixed64.One == Fixed64.Zero;
+            return FixedMath.Abs(Magnitude - Fixed64.One) <= Fixed64.Epsilon;
         }
 
         /// <summary>
@@ -561,14 +570,21 @@ public static Vector3d Slerp(Vector3d start, Vector3d end, Fixed64 percent)
         public static Vector3d GetNormalized(Vector3d value)
         {
             Fixed64 mag = GetMagnitude(value);
-            if (mag > Fixed64.Zero && mag != Fixed64.One)
-            {
-                Fixed64 xM = value.x / mag;
-                Fixed64 yM = value.y / mag;
-                Fixed64 zM = value.z / mag;
-                return new Vector3d(xM, yM, zM);
-            }
-            return value;
+
+            // If magnitude is zero, return a zero vector to avoid divide-by-zero errors
+            if (mag == Fixed64.Zero)
+                return new Vector3d(Fixed64.Zero, Fixed64.Zero, Fixed64.Zero);
+
+            // If already normalized, return as-is
+            if (mag == Fixed64.One)
+                return value;
+
+            // Normalize it exactly           
+            return new Vector3d(
+                value.x / mag,
+                value.y / mag,
+                value.z / mag
+            );
         }
 
         /// <summary>
@@ -579,8 +595,13 @@ public static Vector3d GetNormalized(Vector3d value)
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static Fixed64 GetMagnitude(Vector3d vector)
         {
-            Fixed64 temp1 = (vector.x * vector.x) + (vector.y * vector.y) + (vector.z * vector.z);
-            return temp1 != Fixed64.Zero ? FixedMath.Sqrt(temp1) : Fixed64.Zero;
+            Fixed64 mag = (vector.x * vector.x) + (vector.y * vector.y) + (vector.z * vector.z);
+
+            // If rounding error pushed magnitude slightly above 1, clamp it
+            if (mag > Fixed64.One && mag <= Fixed64.One + Fixed64.Epsilon)
+                return Fixed64.One;
+
+            return mag != Fixed64.Zero ? FixedMath.Sqrt(mag) : Fixed64.Zero;
         }
 
         /// <summary>
@@ -1010,7 +1031,7 @@ public static Vector3d InverseRotate(Vector3d source, Vector3d position, FixedQu
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static Vector3d operator *(Fixed4x4 matrix, Vector3d point)
         {
-            if(matrix.IsAffine)
+            if (matrix.IsAffine)
             {
                 return new Vector3d(
                     matrix.m00 * point.x + matrix.m01 * point.y + matrix.m02 * point.z + matrix.m03 + matrix.m30,
