feat: updates and bugfixes (#30)

* feat: refactor Fixed4x4 to support affine and full transformations
- added TransformPoint and InverseTransformPoint methods
- renamed SRT & TRS methods

* feat: added ToAngularVelocity conversion method to FixedQuanternion

Author: mrdav30

src/FixedMathSharp/Extensions/Fixed4x4.Extensions.cs

@@ -1,5 +1,4 @@
-using System.Numerics;
-using System.Runtime.CompilerServices;
+using System.Runtime.CompilerServices;
 
 namespace FixedMathSharp
 {
@@ -41,6 +40,18 @@ public static Fixed4x4 SetGlobalScale(this ref Fixed4x4 matrix, Vector3d globalS
             return matrix = Fixed4x4.SetGlobalScale(matrix, globalScale);
         }
 
+        /// <inheritdoc cref="Fixed4x4.TransformPoint(Fixed4x4, Vector3d)" />
+        public static Vector3d TransformPoint(this Fixed4x4 matrix, Vector3d point)
+        {
+            return Fixed4x4.TransformPoint(matrix, point);
+        }
+
+        /// <inheritdoc cref="Fixed4x4.InverseTransformPoint(Fixed4x4, Vector3d)" />
+        public static Vector3d InverseTransformPoint(this Fixed4x4 matrix, Vector3d point)
+        {
+            return Fixed4x4.InverseTransformPoint(matrix, point);
+        }
+
         #endregion
     }
 }

src/FixedMathSharp/Extensions/FixedQuaternion.Extensions.cs

@@ -1,10 +1,18 @@
-using FixedMathSharp;
-using System.Runtime.CompilerServices;
+using System.Runtime.CompilerServices;
 
 namespace FixedMathSharp
 {
     public static partial class FixedQuaternionExtensions
     {
+        /// <inheritdoc cref="FixedQuaternion.ToAngularVelocity" />
+        public static Vector3d ToAngularVelocity(
+            this FixedQuaternion currentRotation,
+            FixedQuaternion previousRotation,
+            Fixed64 deltaTime)
+        {
+            return FixedQuaternion.ToAngularVelocity(currentRotation, previousRotation, deltaTime);
+        }
+
         #region Equality
 
         /// <summary>

src/FixedMathSharp/Numerics/Fixed4x4.cs

@@ -1,7 +1,6 @@
 using System;
 using System.Linq;
 
-
 #if NET8_0_OR_GREATER
 using System.Text.Json.Serialization;
 #endif

src/FixedMathSharp/Numerics/FixedCurve.cs

@@ -385,6 +385,63 @@ public static FixedQuaternion FromEulerAngles(Fixed64 pitch, Fixed64 yaw, Fixed6
             return GetNormalized(new FixedQuaternion(x, y, z, w));
         }
 
+        /// <summary>
+        /// Computes the logarithm of a quaternion, which represents the rotational displacement.
+        /// This is useful for interpolation and angular velocity calculations.
+        /// </summary>
+        /// <param name="q">The quaternion to compute the logarithm of.</param>
+        /// <returns>A Vector3d representing the logarithm of the quaternion (axis-angle representation).</returns>
+        /// <remarks>
+        /// The logarithm of a unit quaternion is given by:
+        /// log(q) = (θ * v̂), where:
+        /// - θ = 2 * acos(w) is the rotation angle.
+        /// - v̂ = (x, y, z) / ||(x, y, z)|| is the unit vector representing the axis of rotation.
+        /// If the quaternion is close to identity, the function returns a zero vector to avoid numerical instability.
+        /// </remarks>
+        public static Vector3d QuaternionLog(FixedQuaternion q)
+        {
+            // Ensure the quaternion is normalized
+            q = q.Normal;
+
+            // Extract vector part
+            Vector3d v = new Vector3d(q.x, q.y, q.z);
+            Fixed64 vLength = v.Magnitude;
+
+            // If rotation is very small, avoid division by zero
+            if (vLength < Fixed64.FromRaw(0x00001000L)) // Small epsilon
+                return Vector3d.Zero;
+
+            // Compute angle (theta = 2 * acos(w))
+            Fixed64 theta = Fixed64.Two * FixedMath.Acos(q.w);
+
+            // Convert to angular velocity
+            return (v / vLength) * theta;
+        }
+
+        /// <summary>
+        /// Computes the angular velocity required to move from `previousRotation` to `currentRotation` over a given time step.
+        /// </summary>
+        /// <param name="currentRotation">The current orientation as a quaternion.</param>
+        /// <param name="previousRotation">The previous orientation as a quaternion.</param>
+        /// <param name="deltaTime">The time step over which the rotation occurs.</param>
+        /// <returns>A Vector3d representing the angular velocity (in radians per second).</returns>
+        /// <remarks>
+        /// This function calculates the change in rotation over `deltaTime` and converts it into angular velocity.
+        /// - First, it computes the relative rotation: `rotationDelta = currentRotation * previousRotation.Inverse()`.
+        /// - Then, it applies `QuaternionLog(rotationDelta)` to extract the axis-angle representation.
+        /// - Finally, it divides by `deltaTime` to compute the angular velocity.
+        /// </remarks>
+        public static Vector3d ToAngularVelocity(
+            FixedQuaternion currentRotation, 
+            FixedQuaternion previousRotation, 
+            Fixed64 deltaTime)
+        {
+            FixedQuaternion rotationDelta = currentRotation * previousRotation.Inverse();
+            Vector3d angularDisplacement = QuaternionLog(rotationDelta);
+
+            return angularDisplacement / deltaTime; // Convert to angular velocity
+        }
+
         /// <summary>
         /// Performs a simple linear interpolation between the components of the input quaternions
         /// </summary>

src/FixedMathSharp/Numerics/FixedQuaternion.cs

@@ -997,12 +997,26 @@ public static Vector3d InverseRotate(Vector3d source, Vector3d position, FixedQu
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static Vector3d operator *(Fixed4x4 matrix, Vector3d vector)
+        public static Vector3d operator *(Fixed4x4 matrix, Vector3d point)
         {
+            if(matrix.IsAffine)
+            {
+                return new Vector3d(
+                    matrix.m00 * point.x + matrix.m01 * point.y + matrix.m02 * point.z + matrix.m03 + matrix.m30,
+                    matrix.m10 * point.x + matrix.m11 * point.y + matrix.m12 * point.z + matrix.m13 + matrix.m31,
+                    matrix.m20 * point.x + matrix.m21 * point.y + matrix.m22 * point.z + matrix.m23 + matrix.m32
+                );
+            }
+
+            // Full 4×4 transformation
+            Fixed64 w = matrix.m03 * point.x + matrix.m13 * point.y + matrix.m23 * point.z + matrix.m33;
+            if (w == Fixed64.Zero) w = Fixed64.One;  // Prevent divide-by-zero
+
             return new Vector3d(
-                matrix.m00 * vector.x + matrix.m01 * vector.y + matrix.m02 * vector.z + matrix.m03,
-                matrix.m10 * vector.x + matrix.m11 * vector.y + matrix.m12 * vector.z + matrix.m13,
-                matrix.m20 * vector.x + matrix.m21 * vector.y + matrix.m22 * vector.z + matrix.m23);
+                (matrix.m00 * point.x + matrix.m01 * point.y + matrix.m02 * point.z + matrix.m03 + matrix.m30) / w,
+                (matrix.m10 * point.x + matrix.m11 * point.y + matrix.m12 * point.z + matrix.m13 + matrix.m31) / w,
+                (matrix.m20 * point.x + matrix.m21 * point.y + matrix.m22 * point.z + matrix.m23 + matrix.m32) / w
+            );
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]

src/FixedMathSharp/Numerics/Vector3d.cs

@@ -214,7 +214,7 @@ public void FixedMatrix4x4_TRS_CreatesCorrectTransformationMatrix()
             var rotation = FixedQuaternion.FromEulerAnglesInDegrees((Fixed64)30, (Fixed64)45, (Fixed64)60);
             var scale = new Vector3d(2, 3, 4);
 
-            var trsMatrix = Fixed4x4.SRT(translation, rotation, scale);
+            var trsMatrix = Fixed4x4.ScaleRotateTranslate(translation, rotation, scale);
 
             // Instead of direct equality, compare the decomposed components
             Assert.True(Fixed4x4.Decompose(trsMatrix, out var decomposedScale, out var decomposedRotation, out var decomposedTranslation));
@@ -279,7 +279,7 @@ public void TransformPoint_WorldToLocal_ReturnsCorrectResult()
             var rotation = FixedQuaternion.FromEulerAnglesInDegrees(-(Fixed64)20, (Fixed64)35, (Fixed64)50);
             var scale = new Vector3d(1, 2, 1.5);
 
-            var transformMatrix = Fixed4x4.SRT(translation, rotation, scale);
+            var transformMatrix = Fixed4x4.ScaleRotateTranslate(translation, rotation, scale);
 
             var worldPoint = new Vector3d(10, 15, -2);
             var localPoint = Fixed4x4.InverseTransformPoint(transformMatrix, worldPoint);
@@ -296,7 +296,7 @@ public void InverseTransformPoint_LocalToWorld_ReturnsCorrectResult()
             var rotation = FixedQuaternion.FromEulerAnglesInDegrees((Fixed64)45, -(Fixed64)30, (Fixed64)90);
             var scale = new Vector3d(1.2, 0.8, 1.5);
 
-            var transformMatrix = Fixed4x4.SRT(translation, rotation, scale);
+            var transformMatrix = Fixed4x4.ScaleRotateTranslate(translation, rotation, scale);
 
             var localPoint = new Vector3d(2, 3, -1);
             var worldPoint = Fixed4x4.TransformPoint(transformMatrix, localPoint);
@@ -313,7 +313,7 @@ public void TransformPoint_InverseTransformPoint_RoundTripConsistency()
             var rotation = FixedQuaternion.FromEulerAnglesInDegrees(-(Fixed64)45, (Fixed64)30, (Fixed64)90);
             var scale = new Vector3d(1.5, 2.5, 3.0);
 
-            var transformMatrix = Fixed4x4.SRT(translation, rotation, scale);
+            var transformMatrix = Fixed4x4.ScaleRotateTranslate(translation, rotation, scale);
 
             var originalPoint = new Vector3d(3, 5, 7);
             var transformedPoint = Fixed4x4.TransformPoint(transformMatrix, originalPoint);
@@ -330,7 +330,7 @@ public void Fixed4x4_Serialization_RoundTripMaintainsData()
             var rotation = FixedQuaternion.FromEulerAnglesInDegrees(Fixed64.Zero, FixedMath.PiOver2, Fixed64.Zero);
             var scale = new Vector3d(1, 1, 1);
 
-            var original4x4 = Fixed4x4.SRT(translation, rotation, scale);
+            var original4x4 = Fixed4x4.ScaleRotateTranslate(translation, rotation, scale);
 
             // Serialize the Fixed4x4 object
 #if NET48_OR_GREATER

tests/FixedMathSharp.Tests/Fixed4x4.Tests.cs

@@ -6,7 +6,6 @@
 #if NET8_0_OR_GREATER
 using System.Text.Json;
 using System.Text.Json.Serialization;
-
 #endif
 
 using Xunit;
@@ -197,6 +196,33 @@ public void FixedQuaternion_ToMatrix_WorksCorrectly()
             Assert.True(result == expected, $"ToMatrix returned {result}, expected Identity matrix.");
         }
 
+        [Fact]
+        public void FixedQuaternion_ToAngularVelocity_WorksCorrectly()
+        {
+            var prevRotation = FixedQuaternion.Identity;
+            var currentRotation = FixedQuaternion.FromAxisAngle(Vector3d.Up, FixedMath.PiOver4); // Rotated 45 degrees around Y-axis
+            var deltaTime = new Fixed64(2); // Assume 2 seconds elapsed
+
+            var angularVelocity = FixedQuaternion.ToAngularVelocity(currentRotation, prevRotation, deltaTime);
+
+            var expected = new Vector3d(Fixed64.Zero, FixedMath.PiOver4 / deltaTime, Fixed64.Zero); // Expect ω = θ / dt
+            Assert.True(angularVelocity.FuzzyEqual(expected, new Fixed64(0.0001)),
+                $"ToAngularVelocity returned {angularVelocity}, expected {expected}");
+        }
+
+        [Fact]
+        public void FixedQuaternion_ToAngularVelocity_ZeroForNoRotation()
+        {
+            var prevRotation = FixedQuaternion.Identity;
+            var currentRotation = FixedQuaternion.Identity;
+            var deltaTime = Fixed64.One;
+
+            var angularVelocity = FixedQuaternion.ToAngularVelocity(currentRotation, prevRotation, deltaTime);
+
+            Assert.True(angularVelocity.FuzzyEqual(Vector3d.Zero),
+                $"ToAngularVelocity should return zero for no rotation, but got {angularVelocity}");
+        }
+
         [Fact]
         public void FixedQuanternion_Serialization_RoundTripMaintainsData()
         {
@@ -336,6 +362,27 @@ public void FixedQuaternion_LookRotation_WorksCorrectly()
             Assert.True(result.FuzzyEqual(expected), $"Look rotation returned {result}, expected {expected}.");
         }
 
+        [Fact]
+        public void FixedQuaternion_QuaternionLog_WorksCorrectly()
+        {
+            var quaternion = FixedQuaternion.FromAxisAngle(Vector3d.Up, FixedMath.PiOver4); // 45-degree rotation around Y-axis
+            var logResult = FixedQuaternion.QuaternionLog(quaternion);
+
+            var expected = new Vector3d(Fixed64.Zero, FixedMath.PiOver4, Fixed64.Zero); // Expect log(q) = θ * axis
+            Assert.True(logResult.FuzzyEqual(expected, new Fixed64(0.0001)),
+                $"QuaternionLog returned {logResult}, expected {expected}");
+        }
+
+        [Fact]
+        public void FixedQuaternion_QuaternionLog_ReturnsZeroForIdentity()
+        {
+            var identity = FixedQuaternion.Identity;
+            var logResult = FixedQuaternion.QuaternionLog(identity);
+
+            Assert.True(logResult.FuzzyEqual(Vector3d.Zero),
+                $"QuaternionLog of Identity should be (0,0,0), but got {logResult}");
+        }
+
         #endregion
 
         #region Test: Operators