feat: helper methods (#32)

* feat: add method to extract up direction from 4x4 matrix
- cleanup unneccessary extensions

* feat: add method to clamp Vector3d by magnitude

Author: mrdav30

src/FixedMathSharp/Extensions/Fixed4x4.Extensions.cs

@@ -6,38 +6,29 @@ public static class Fixed4x4Extensions
     {
         #region Extraction, and Setters
 
-        /// <inheritdoc cref="Fixed4x4.ExtractScale(Fixed4x4)" />
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static Vector3d ExtractScale(this Fixed4x4 matrix)
-        {
-            return Fixed4x4.ExtractScale(matrix);
-        }
-
         /// <inheritdoc cref="Fixed4x4.ExtractLossyScale(Fixed4x4)" />
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static Vector3d ExtractLossyScale(this Fixed4x4 matrix)
         {
             return Fixed4x4.ExtractLossyScale(matrix);
         }
 
-        /// <inheritdoc cref="Fixed4x4.ExtractTranslation(Fixed4x4)" />
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static Vector3d ExtractTranslation(this Fixed4x4 matrix)
+        /// <inheritdoc cref="Fixed4x4.SetGlobalScale(Fixed4x4, Vector3d)" />
+        public static Fixed4x4 SetGlobalScale(this ref Fixed4x4 matrix, Vector3d globalScale)
         {
-            return Fixed4x4.ExtractTranslation(matrix);
+            return matrix = Fixed4x4.SetGlobalScale(matrix, globalScale);
         }
 
-        /// <inheritdoc cref="Fixed4x4.ExtractRotation(Fixed4x4)" />
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static FixedQuaternion ExtractRotation(this Fixed4x4 matrix)
+        /// <inheritdoc cref="Fixed4x4.SetTranslation(Fixed4x4, Vector3d)" />
+        public static Fixed4x4 SetTranslation(this ref Fixed4x4 matrix, Vector3d position)
         {
-            return Fixed4x4.ExtractRotation(matrix);
+            return matrix = Fixed4x4.SetTranslation(matrix, position);
         }
 
-        /// <inheritdoc cref="Fixed4x4.SetGlobalScale(Fixed4x4, Vector3d)" />
-        public static Fixed4x4 SetGlobalScale(this ref Fixed4x4 matrix, Vector3d globalScale)
+        /// <inheritdoc cref="Fixed4x4.SetRotation(Fixed4x4, FixedQuaternion)" />
+        public static Fixed4x4 SetRotation(this ref Fixed4x4 matrix, FixedQuaternion rotation)
         {
-            return matrix = Fixed4x4.SetGlobalScale(matrix, globalScale);
+            return matrix = Fixed4x4.SetRotation(matrix, rotation);
         }
 
         /// <inheritdoc cref="Fixed4x4.TransformPoint(Fixed4x4, Vector3d)" />

src/FixedMathSharp/Extensions/Vector3d.Extensions.cs

@@ -20,6 +20,11 @@ public static Vector3d ClampOneInPlace(this Vector3d v)
             return v;
         }
 
+        public static Vector3d ClampMagnitude(this Vector3d value, Fixed64 maxMagnitude)
+        {
+            return Vector3d.ClampMagnitude(value, maxMagnitude);
+        }
+
         /// <summary>
         /// Checks if the distance between two vectors is less than or equal to a specified factor.
         /// </summary>

src/FixedMathSharp/Numerics/Fixed4x4.cs

@@ -63,17 +63,16 @@ public Fixed4x4(
 
         public readonly bool IsAffine => (m33 == Fixed64.One) && (m03 == Fixed64.Zero && m13 == Fixed64.Zero && m23 == Fixed64.Zero);
 
-        /// <summary>
-        /// Gets or sets the translation component of this matrix.
-        /// </summary>
-        /// <returns>
-        /// The translation component of the current instance.
-        /// </returns>
-        public readonly Vector3d Translation => this.ExtractTranslation();
+        /// <inheritdoc cref="ExtractTranslation(Fixed4x4)" />
+        public readonly Vector3d Translation => ExtractTranslation(this);
+
+        public readonly Vector3d Up => ExtractUp(this);
 
-        public readonly Vector3d Scale => this.ExtractScale();
+        /// <inheritdoc cref="ExtractScale(Fixed4x4)" />
+        public readonly Vector3d Scale => ExtractScale(this);
 
-        public readonly FixedQuaternion Rotation => this.ExtractRotation();
+        /// <inheritdoc cref="ExtractRotation(Fixed4x4)" />
+        public readonly FixedQuaternion Rotation => ExtractRotation(this);
 
         /// <summary>
         /// Calculates the determinant of a 4x4 matrix.
@@ -275,6 +274,19 @@ public static Vector3d ExtractTranslation(Fixed4x4 matrix)
             return new Vector3d(matrix.m30, matrix.m31, matrix.m32);
         }
 
+        /// <summary>
+        /// Extracts the up direction from the 4x4 matrix.
+        /// </summary>
+        /// <remarks>
+        /// This is the surface normal if the matrix represents ground orientation.
+        /// </remarks>
+        /// <param name="matrix"></param>
+        /// <returns>A <see cref="Vector3d"/> representing the up direction.</returns>
+        public static Vector3d ExtractUp(Fixed4x4 matrix)
+        {
+            return new Vector3d(matrix.m10, matrix.m11, matrix.m12).Normalize();
+        }
+
         /// <summary>
         /// Extracts the scaling factors from the matrix by calculating the magnitudes of the basis vectors (non-lossy).
         /// </summary>

src/FixedMathSharp/Numerics/Vector3d.cs

@@ -622,6 +622,17 @@ public static Vector3d Clamp(Vector3d value, Vector3d min, Vector3d max)
             );
         }
 
+        public static Vector3d ClampMagnitude(Vector3d value, Fixed64 maxMagnitude)
+        {
+            Fixed64 magnitudeSqr = value.SqrMagnitude;
+            if (magnitudeSqr > maxMagnitude * maxMagnitude)
+            {
+                Fixed64 magnitude = FixedMath.Sqrt(magnitudeSqr); // Get actual magnitude
+                return (value / magnitude) * maxMagnitude; // Scale vector to max magnitude
+            }
+            return value;
+        }
+
         /// <summary>
         /// Determines if two vectors are exactly parallel by checking if their cross product is zero.
         /// </summary>

tests/FixedMathSharp.Tests/Fixed4x4.Tests.cs

@@ -38,9 +38,7 @@ public void FixedMatrix4x4_CreateTranslation_WorksCorrectly()
             var matrix = Fixed4x4.CreateTranslation(translation);
 
             // Extract the translation to verify
-            var extractedTranslation = matrix.ExtractTranslation();
-
-            Assert.Equal(translation, extractedTranslation);
+            Assert.Equal(translation, matrix.Translation);
         }
 
         [Fact]
@@ -50,9 +48,7 @@ public void FixedMatrix4x4_CreateScale_WorksCorrectly()
             var matrix = Fixed4x4.CreateScale(scale);
 
             // Extract the scale to verify
-            var extractedScale = matrix.ExtractScale();
-
-            Assert.Equal(scale, extractedScale);
+            Assert.Equal(scale, matrix.Scale);
         }
 
         [Fact]
@@ -84,14 +80,10 @@ public void FixedMatrix4x4_SetTransform_WorksCorrectly()
             matrix.SetTransform(translation, rotation, scale);
 
             // Extract and validate translation, scale, and rotation
-            var extractedTranslation = matrix.ExtractTranslation();
-            var extractedScale = matrix.ExtractScale();
-            var extractedRotation = matrix.ExtractRotation();
-
-            Assert.Equal(translation, extractedTranslation);
-            Assert.Equal(scale, extractedScale);
-            Assert.True(extractedRotation.FuzzyEqual(rotation, new Fixed64(0.0001)),
-                $"Extracted rotation {extractedRotation} does not match expected {rotation}.");
+            Assert.Equal(translation, matrix.Translation);
+            Assert.Equal(scale, matrix.Scale);
+            Assert.True(matrix.Rotation.FuzzyEqual(rotation, new Fixed64(0.0001)),
+                $"Extracted rotation {matrix.Rotation} does not match expected {rotation}.");
         }
 
         [Fact]
@@ -248,9 +240,7 @@ public void FixedMatrix4x4_SetGlobalScale_WorksWithoutRotation()
             matrix.SetGlobalScale(globalScale);
 
             // Extract the final scale
-            var extractedScale = matrix.ExtractScale();
-
-            Assert.Equal(globalScale, extractedScale);
+            Assert.Equal(globalScale, matrix.Scale);
         }
 
         [Fact]