diff --git a/src/FixedMathSharp/Extensions/Fixed4x4.Extensions.cs b/src/FixedMathSharp/Extensions/Fixed4x4.Extensions.cs index 4b52aa8..7e67086 100644 --- a/src/FixedMathSharp/Extensions/Fixed4x4.Extensions.cs +++ b/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); } + /// + public static Vector3d TransformPoint(this Fixed4x4 matrix, Vector3d point) + { + return Fixed4x4.TransformPoint(matrix, point); + } + + /// + public static Vector3d InverseTransformPoint(this Fixed4x4 matrix, Vector3d point) + { + return Fixed4x4.InverseTransformPoint(matrix, point); + } + #endregion } } \ No newline at end of file diff --git a/src/FixedMathSharp/Extensions/FixedQuaternion.Extensions.cs b/src/FixedMathSharp/Extensions/FixedQuaternion.Extensions.cs index abef9c4..c09c958 100644 --- a/src/FixedMathSharp/Extensions/FixedQuaternion.Extensions.cs +++ b/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 { + /// + public static Vector3d ToAngularVelocity( + this FixedQuaternion currentRotation, + FixedQuaternion previousRotation, + Fixed64 deltaTime) + { + return FixedQuaternion.ToAngularVelocity(currentRotation, previousRotation, deltaTime); + } + #region Equality /// diff --git a/src/FixedMathSharp/Numerics/Fixed4x4.cs b/src/FixedMathSharp/Numerics/Fixed4x4.cs index 6105843..b614f1d 100644 --- a/src/FixedMathSharp/Numerics/Fixed4x4.cs +++ b/src/FixedMathSharp/Numerics/Fixed4x4.cs @@ -1,5 +1,4 @@ using System; -using System.Numerics; using System.Runtime.CompilerServices; namespace FixedMathSharp @@ -62,23 +61,33 @@ public Fixed4x4( #region Properties and Methods (Instance) + public readonly bool IsAffine => (m33 == Fixed64.One) && (m03 == Fixed64.Zero && m13 == Fixed64.Zero && m23 == Fixed64.Zero); + /// /// Gets or sets the translation component of this matrix. /// /// /// The translation component of the current instance. /// - public Vector3d Translation => this.ExtractTranslation(); + public readonly Vector3d Translation => this.ExtractTranslation(); - public Vector3d Scale => this.ExtractScale(); + public readonly Vector3d Scale => this.ExtractScale(); - public FixedQuaternion Rotation => this.ExtractRotation(); + public readonly FixedQuaternion Rotation => this.ExtractRotation(); /// /// Calculates the determinant of a 4x4 matrix. /// public Fixed64 GetDeterminant() { + if (IsAffine) + { + return m00 * (m11 * m22 - m12 * m21) + - m01 * (m10 * m22 - m12 * m20) + + m02 * (m10 * m21 - m11 * m20); + } + + // Process as full 4x4 matrix Fixed64 minor0 = m22 * m33 - m23 * m32; Fixed64 minor1 = m21 * m33 - m23 * m31; Fixed64 minor2 = m21 * m32 - m22 * m31; @@ -89,7 +98,6 @@ public Fixed64 GetDeterminant() - m01 * (m10 * minor0 - m12 * cofactor0 + m13 * cofactor1) + m02 * (m10 * minor1 - m11 * cofactor0 + m13 * cofactor2) - m03 * (m10 * minor2 - m11 * cofactor1 + m12 * cofactor2); - } /// @@ -182,7 +190,7 @@ public static Fixed4x4 CreateScale(Vector3d scale) /// - Uses a normalized rotation matrix to maintain numerical stability. /// - Applies non-uniform scaling to the rotation before setting translation. /// - Preferred when ensuring transformations remain mathematically correct. - /// - If the rotation is already normalized and combined transformations are needed, consider using . + /// - If the rotation is already normalized and combined transformations are needed, consider using . /// /// The translation vector. /// The scale vector. @@ -213,7 +221,7 @@ public static Fixed4x4 CreateTransform(Vector3d translation, FixedQuaternion rot /// - Then rotation is applied so that rotation is not affected by non-uniform scaling. /// - Finally, translation moves the object to its correct world position. /// - public static Fixed4x4 SRT(Vector3d translation, FixedQuaternion rotation, Vector3d scale) + public static Fixed4x4 ScaleRotateTranslate(Vector3d translation, FixedQuaternion rotation, Vector3d scale) { // Create translation matrix Fixed4x4 translationMatrix = CreateTranslation(translation); @@ -237,7 +245,7 @@ public static Fixed4x4 SRT(Vector3d translation, FixedQuaternion rotation, Vecto /// - Example use cases include **animation systems**, **hierarchical transformations**, and **UI transformations**. /// - If you need to apply world-space transformations, use instead. /// - public static Fixed4x4 TRS(Vector3d translation, FixedQuaternion rotation, Vector3d scale) + public static Fixed4x4 TranslateRotateScale(Vector3d translation, FixedQuaternion rotation, Vector3d scale) { // Create translation matrix Fixed4x4 translationMatrix = CreateTranslation(translation); @@ -300,10 +308,15 @@ public static FixedQuaternion ExtractRotation(Fixed4x4 matrix) { Vector3d scale = ExtractScale(matrix); + // prevent divide by zero exception + Fixed64 scaleX = scale.x == Fixed64.Zero ? Fixed64.One : scale.x; + Fixed64 scaleY = scale.y == Fixed64.Zero ? Fixed64.One : scale.y; + Fixed64 scaleZ = scale.z == Fixed64.Zero ? Fixed64.One : scale.z; + Fixed4x4 normalizedMatrix = new Fixed4x4( - matrix.m00 / scale.x, matrix.m01 / scale.y, matrix.m02 / scale.z, Fixed64.Zero, - matrix.m10 / scale.x, matrix.m11 / scale.y, matrix.m12 / scale.z, Fixed64.Zero, - matrix.m20 / scale.x, matrix.m21 / scale.y, matrix.m22 / scale.z, Fixed64.Zero, + matrix.m00 / scaleX, matrix.m01 / scaleY, matrix.m02 / scaleZ, Fixed64.Zero, + matrix.m10 / scaleX, matrix.m11 / scaleY, matrix.m12 / scaleZ, Fixed64.Zero, + matrix.m20 / scaleX, matrix.m21 / scaleY, matrix.m22 / scaleZ, Fixed64.Zero, Fixed64.Zero, Fixed64.Zero, Fixed64.Zero, Fixed64.One ); @@ -327,43 +340,31 @@ public static bool Decompose( // Extract scale by calculating the magnitudes of the basis vectors scale = ExtractScale(matrix); - // Extract translation - Fixed4x4 normalizedMatrix = ApplyScaleToRotation(matrix, new Vector3d(Fixed64.One / scale.x, Fixed64.One / scale.y, Fixed64.One / scale.z)); - translation = ExtractTranslation(normalizedMatrix); - - // Extract scale from the basis vectors - Vector3d basisX = new Vector3d(matrix.m00, matrix.m01, matrix.m02); - Vector3d basisY = new Vector3d(matrix.m10, matrix.m11, matrix.m12); - Vector3d basisZ = new Vector3d(matrix.m20, matrix.m21, matrix.m22); - - scale = new Vector3d(basisX.Magnitude, basisY.Magnitude, basisZ.Magnitude); + // prevent divide by zero exception + scale = new Vector3d( + scale.x == Fixed64.Zero ? Fixed64.One : scale.x, + scale.y == Fixed64.Zero ? Fixed64.One : scale.y, + scale.z == Fixed64.Zero ? Fixed64.One : scale.z); - // Normalize the basis vectors to isolate rotation - if (scale.x > Fixed64.Zero) basisX /= scale.x; - if (scale.y > Fixed64.Zero) basisY /= scale.y; - if (scale.z > Fixed64.Zero) basisZ /= scale.z; + // normalize rotation and scaling + Fixed4x4 normalizedMatrix = ApplyScaleToRotation(matrix, Vector3d.One / scale); - // Construct the normalized rotation matrix - Fixed4x4 normalizedRotationMatrix = new Fixed4x4( - basisX.x, basisX.y, basisX.z, Fixed64.Zero, - basisY.x, basisY.y, basisY.z, Fixed64.Zero, - basisZ.x, basisZ.y, basisZ.z, Fixed64.Zero, - Fixed64.Zero, Fixed64.Zero, Fixed64.Zero, Fixed64.One - ); + // Extract translation + translation = new Vector3d(normalizedMatrix.m30, normalizedMatrix.m31, normalizedMatrix.m32); // Check the determinant to ensure correct handedness - Fixed64 determinant = normalizedRotationMatrix.GetDeterminant(); + Fixed64 determinant = normalizedMatrix.GetDeterminant(); if (determinant < Fixed64.Zero) { // Adjust for left-handed coordinate system by flipping one of the axes scale.x = -scale.x; - normalizedRotationMatrix.m00 = -normalizedRotationMatrix.m00; - normalizedRotationMatrix.m01 = -normalizedRotationMatrix.m01; - normalizedRotationMatrix.m02 = -normalizedRotationMatrix.m02; + normalizedMatrix.m00 = -normalizedMatrix.m00; + normalizedMatrix.m01 = -normalizedMatrix.m01; + normalizedMatrix.m02 = -normalizedMatrix.m02; } // Extract the rotation component from the orthogonalized matrix - rotation = FixedQuaternion.FromMatrix(normalizedRotationMatrix); + rotation = FixedQuaternion.FromMatrix(normalizedMatrix); return true; } @@ -480,18 +481,21 @@ public static Fixed4x4 SetRotation(Fixed4x4 matrix, FixedQuaternion rotation) { Fixed3x3 rotationMatrix = rotation.ToMatrix3x3(); + Vector3d scale = ExtractScale(matrix); + // Apply rotation to the upper-left 3x3 matrix - matrix.m00 = rotationMatrix.m00; - matrix.m01 = rotationMatrix.m01; - matrix.m02 = rotationMatrix.m02; - matrix.m10 = rotationMatrix.m10; - matrix.m11 = rotationMatrix.m11; - matrix.m12 = rotationMatrix.m12; + matrix.m00 = rotationMatrix.m00 * scale.x; + matrix.m01 = rotationMatrix.m01 * scale.x; + matrix.m02 = rotationMatrix.m02 * scale.x; - matrix.m20 = rotationMatrix.m20; - matrix.m21 = rotationMatrix.m21; - matrix.m22 = rotationMatrix.m22; + matrix.m10 = rotationMatrix.m10 * scale.y; + matrix.m11 = rotationMatrix.m11 * scale.y; + matrix.m12 = rotationMatrix.m12 * scale.y; + + matrix.m20 = rotationMatrix.m20 * scale.z; + matrix.m21 = rotationMatrix.m21 * scale.z; + matrix.m22 = rotationMatrix.m22 * scale.z; return matrix; } @@ -536,13 +540,63 @@ public static Fixed4x4 NormalizeRotationMatrix(Fixed4x4 matrix) /// multiplied by a sign based on the element's position. /// After computing all cofactors, the result is transposed to get the inverse matrix. /// - public static bool Invert(Fixed4x4 matrix, out Fixed4x4? result) + public static bool Invert(Fixed4x4 matrix, out Fixed4x4 result) { + if (!matrix.IsAffine) + return FullInvert(matrix, out result); + Fixed64 det = matrix.GetDeterminant(); if (det == Fixed64.Zero) { - result = null; + result = Identity; + return false; + } + + Fixed64 invDet = Fixed64.One / det; + + // Invert the 3×3 upper-left rotation/scale matrix + result = new Fixed4x4( + (matrix.m11 * matrix.m22 - matrix.m12 * matrix.m21) * invDet, + (matrix.m02 * matrix.m21 - matrix.m01 * matrix.m22) * invDet, + (matrix.m01 * matrix.m12 - matrix.m02 * matrix.m11) * invDet, Fixed64.Zero, + + (matrix.m12 * matrix.m20 - matrix.m10 * matrix.m22) * invDet, + (matrix.m00 * matrix.m22 - matrix.m02 * matrix.m20) * invDet, + (matrix.m02 * matrix.m10 - matrix.m00 * matrix.m12) * invDet, Fixed64.Zero, + + (matrix.m10 * matrix.m21 - matrix.m11 * matrix.m20) * invDet, + (matrix.m01 * matrix.m20 - matrix.m00 * matrix.m21) * invDet, + (matrix.m00 * matrix.m11 - matrix.m01 * matrix.m10) * invDet, Fixed64.Zero, + + Fixed64.Zero, Fixed64.Zero, Fixed64.Zero, Fixed64.One // Ensure homogeneous coordinate stays valid + ); + + Fixed3x3 rotationScaleInverse = new Fixed3x3( + result.m00, result.m01, result.m02, + result.m10, result.m11, result.m12, + result.m20, result.m21, result.m22 + ); + + // Correct translation component + Vector3d transformedTranslation = new Vector3d(matrix.m30, matrix.m31, matrix.m32); + transformedTranslation = -Fixed3x3.TransformDirection(rotationScaleInverse, transformedTranslation); + + result.m30 = transformedTranslation.x; + result.m31 = transformedTranslation.y; + result.m32 = transformedTranslation.z; + result.m33 = Fixed64.One; + + return true; + } + + private static bool FullInvert(Fixed4x4 matrix, out Fixed4x4 result) + { + Fixed64 det = matrix.GetDeterminant(); + + if (det == Fixed64.Zero) + { + result = Fixed4x4.Identity; return false; } @@ -598,16 +652,37 @@ public static bool Invert(Fixed4x4 matrix, out Fixed4x4? result) /// /// Transforms a point from local space to world space using this transformation matrix. /// + /// + /// This is the same as doing ` a * b` + /// /// The transformation matrix. /// The local-space point. /// The transformed point in world space. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3d TransformPoint(Fixed4x4 matrix, Vector3d point) { + if (matrix.IsAffine) + { + return new Vector3d( + matrix.m00 * point.x + matrix.m01 * point.y + matrix.m02 * point.z + matrix.m30, + matrix.m10 * point.x + matrix.m11 * point.y + matrix.m12 * point.z + matrix.m31, + matrix.m20 * point.x + matrix.m21 * point.y + matrix.m22 * point.z + matrix.m32 + ); + } + + return FullTransformPoint(matrix, point); + } + + private static Vector3d FullTransformPoint(Fixed4x4 matrix, Vector3d point) + { + // Full 4×4 transformation (needed for perspective projections) + 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 * point.x + matrix.m01 * point.y + matrix.m02 * point.z + matrix.m03, - matrix.m10 * point.x + matrix.m11 * point.y + matrix.m12 * point.z + matrix.m13, - matrix.m20 * point.x + matrix.m21 * point.y + matrix.m22 * point.z + matrix.m23 + (matrix.m00 * point.x + matrix.m01 * point.y + matrix.m02 * point.z + matrix.m30) / w, + (matrix.m10 * point.x + matrix.m11 * point.y + matrix.m12 * point.z + matrix.m31) / w, + (matrix.m20 * point.x + matrix.m21 * point.y + matrix.m22 * point.z + matrix.m32) / w ); } @@ -620,14 +695,31 @@ public static Vector3d TransformPoint(Fixed4x4 matrix, Vector3d point) public static Vector3d InverseTransformPoint(Fixed4x4 matrix, Vector3d point) { // Invert the transformation matrix - if (!Invert(matrix, out Fixed4x4? inverseMatrix) || !inverseMatrix.HasValue) + if (!Invert(matrix, out Fixed4x4 inverseMatrix)) throw new InvalidOperationException("Matrix is not invertible."); - // Apply the inverse matrix to the point (homogeneous coordinates) + if (inverseMatrix.IsAffine) + { + return new Vector3d( + inverseMatrix.m00 * point.x + inverseMatrix.m01 * point.y + inverseMatrix.m02 * point.z + inverseMatrix.m30, + inverseMatrix.m10 * point.x + inverseMatrix.m11 * point.y + inverseMatrix.m12 * point.z + inverseMatrix.m31, + inverseMatrix.m20 * point.x + inverseMatrix.m21 * point.y + inverseMatrix.m22 * point.z + inverseMatrix.m32 + ); + } + + return FullInverseTransformPoint(inverseMatrix, point); + } + + private static Vector3d FullInverseTransformPoint(Fixed4x4 matrix, Vector3d point) + { + // Full 4×4 transformation (needed for perspective projections) + 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( - inverseMatrix.Value.m00 * point.x + inverseMatrix.Value.m01 * point.y + inverseMatrix.Value.m02 * point.z + inverseMatrix.Value.m03, - inverseMatrix.Value.m10 * point.x + inverseMatrix.Value.m11 * point.y + inverseMatrix.Value.m12 * point.z + inverseMatrix.Value.m13, - inverseMatrix.Value.m20 * point.x + inverseMatrix.Value.m21 * point.y + inverseMatrix.Value.m22 * point.z + inverseMatrix.Value.m23 + (matrix.m00 * point.x + matrix.m01 * point.y + matrix.m02 * point.z + matrix.m30) / w, + (matrix.m10 * point.x + matrix.m11 * point.y + matrix.m12 * point.z + matrix.m31) / w, + (matrix.m20 * point.x + matrix.m21 * point.y + matrix.m22 * point.z + matrix.m32) / w ); } @@ -693,30 +785,56 @@ public static Vector3d InverseTransformPoint(Fixed4x4 matrix, Vector3d point) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Fixed4x4 operator *(Fixed4x4 lhs, Fixed4x4 rhs) { + if (lhs.IsAffine && rhs.IsAffine) + { + // Optimized affine multiplication (skips full 4×4 multiplication) + return new Fixed4x4( + lhs.m00 * rhs.m00 + lhs.m01 * rhs.m10 + lhs.m02 * rhs.m20, + lhs.m00 * rhs.m01 + lhs.m01 * rhs.m11 + lhs.m02 * rhs.m21, + lhs.m00 * rhs.m02 + lhs.m01 * rhs.m12 + lhs.m02 * rhs.m22, + Fixed64.Zero, + + lhs.m10 * rhs.m00 + lhs.m11 * rhs.m10 + lhs.m12 * rhs.m20, + lhs.m10 * rhs.m01 + lhs.m11 * rhs.m11 + lhs.m12 * rhs.m21, + lhs.m10 * rhs.m02 + lhs.m11 * rhs.m12 + lhs.m12 * rhs.m22, + Fixed64.Zero, + + lhs.m20 * rhs.m00 + lhs.m21 * rhs.m10 + lhs.m22 * rhs.m20, + lhs.m20 * rhs.m01 + lhs.m21 * rhs.m11 + lhs.m22 * rhs.m21, + lhs.m20 * rhs.m02 + lhs.m21 * rhs.m12 + lhs.m22 * rhs.m22, + Fixed64.Zero, + + lhs.m30 * rhs.m00 + lhs.m31 * rhs.m10 + lhs.m32 * rhs.m20 + rhs.m30, + lhs.m30 * rhs.m01 + lhs.m31 * rhs.m11 + lhs.m32 * rhs.m21 + rhs.m31, + lhs.m30 * rhs.m02 + lhs.m31 * rhs.m12 + lhs.m32 * rhs.m22 + rhs.m32, + Fixed64.One + ); + } + + // Full 4×4 multiplication (fallback for perspective matrices) return new Fixed4x4( - // First row + // Upper-left 3×3 matrix multiplication (rotation & scale) lhs.m00 * rhs.m00 + lhs.m01 * rhs.m10 + lhs.m02 * rhs.m20 + lhs.m03 * rhs.m30, lhs.m00 * rhs.m01 + lhs.m01 * rhs.m11 + lhs.m02 * rhs.m21 + lhs.m03 * rhs.m31, lhs.m00 * rhs.m02 + lhs.m01 * rhs.m12 + lhs.m02 * rhs.m22 + lhs.m03 * rhs.m32, lhs.m00 * rhs.m03 + lhs.m01 * rhs.m13 + lhs.m02 * rhs.m23 + lhs.m03 * rhs.m33, - // Second row lhs.m10 * rhs.m00 + lhs.m11 * rhs.m10 + lhs.m12 * rhs.m20 + lhs.m13 * rhs.m30, lhs.m10 * rhs.m01 + lhs.m11 * rhs.m11 + lhs.m12 * rhs.m21 + lhs.m13 * rhs.m31, lhs.m10 * rhs.m02 + lhs.m11 * rhs.m12 + lhs.m12 * rhs.m22 + lhs.m13 * rhs.m32, lhs.m10 * rhs.m03 + lhs.m11 * rhs.m13 + lhs.m12 * rhs.m23 + lhs.m13 * rhs.m33, - // Third row lhs.m20 * rhs.m00 + lhs.m21 * rhs.m10 + lhs.m22 * rhs.m20 + lhs.m23 * rhs.m30, lhs.m20 * rhs.m01 + lhs.m21 * rhs.m11 + lhs.m22 * rhs.m21 + lhs.m23 * rhs.m31, lhs.m20 * rhs.m02 + lhs.m21 * rhs.m12 + lhs.m22 * rhs.m22 + lhs.m23 * rhs.m32, lhs.m20 * rhs.m03 + lhs.m21 * rhs.m13 + lhs.m22 * rhs.m23 + lhs.m23 * rhs.m33, - // Fourth row (Translation component) + // Compute new translation lhs.m30 * rhs.m00 + lhs.m31 * rhs.m10 + lhs.m32 * rhs.m20 + lhs.m33 * rhs.m30, lhs.m30 * rhs.m01 + lhs.m31 * rhs.m11 + lhs.m32 * rhs.m21 + lhs.m33 * rhs.m31, lhs.m30 * rhs.m02 + lhs.m31 * rhs.m12 + lhs.m32 * rhs.m22 + lhs.m33 * rhs.m32, - lhs.m30 * rhs.m03 + lhs.m31 * rhs.m13 + lhs.m32 * rhs.m23 + lhs.m33 * rhs.m33); + lhs.m30 * rhs.m03 + lhs.m31 * rhs.m13 + lhs.m32 * rhs.m23 + lhs.m33 * rhs.m33 + ); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -736,7 +854,7 @@ public static Vector3d InverseTransformPoint(Fixed4x4 matrix, Vector3d point) #region Equality and HashCode Overrides [MethodImpl(MethodImplOptions.AggressiveInlining)] - public override bool Equals(object obj) + public override bool Equals(object? obj) { return obj is Fixed4x4 x && Equals(x); } diff --git a/src/FixedMathSharp/Numerics/FixedCurve.cs b/src/FixedMathSharp/Numerics/FixedCurve.cs index 436a2be..af0eb11 100644 --- a/src/FixedMathSharp/Numerics/FixedCurve.cs +++ b/src/FixedMathSharp/Numerics/FixedCurve.cs @@ -1,7 +1,6 @@ using System; using System.Linq; - #if NET8_0_OR_GREATER using System.Text.Json.Serialization; #endif diff --git a/src/FixedMathSharp/Numerics/FixedQuaternion.cs b/src/FixedMathSharp/Numerics/FixedQuaternion.cs index 8de01b5..3f4754c 100644 --- a/src/FixedMathSharp/Numerics/FixedQuaternion.cs +++ b/src/FixedMathSharp/Numerics/FixedQuaternion.cs @@ -385,6 +385,63 @@ public static FixedQuaternion FromEulerAngles(Fixed64 pitch, Fixed64 yaw, Fixed6 return GetNormalized(new FixedQuaternion(x, y, z, w)); } + /// + /// Computes the logarithm of a quaternion, which represents the rotational displacement. + /// This is useful for interpolation and angular velocity calculations. + /// + /// The quaternion to compute the logarithm of. + /// A Vector3d representing the logarithm of the quaternion (axis-angle representation). + /// + /// 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. + /// + 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; + } + + /// + /// Computes the angular velocity required to move from `previousRotation` to `currentRotation` over a given time step. + /// + /// The current orientation as a quaternion. + /// The previous orientation as a quaternion. + /// The time step over which the rotation occurs. + /// A Vector3d representing the angular velocity (in radians per second). + /// + /// 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. + /// + 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 + } + /// /// Performs a simple linear interpolation between the components of the input quaternions /// diff --git a/src/FixedMathSharp/Numerics/Vector3d.cs b/src/FixedMathSharp/Numerics/Vector3d.cs index df1ef9b..3a88936 100644 --- a/src/FixedMathSharp/Numerics/Vector3d.cs +++ b/src/FixedMathSharp/Numerics/Vector3d.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)] diff --git a/tests/FixedMathSharp.Tests/Fixed4x4.Tests.cs b/tests/FixedMathSharp.Tests/Fixed4x4.Tests.cs index 1d8e46e..bbdaa58 100644 --- a/tests/FixedMathSharp.Tests/Fixed4x4.Tests.cs +++ b/tests/FixedMathSharp.Tests/Fixed4x4.Tests.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 diff --git a/tests/FixedMathSharp.Tests/FixedQuanternion.Tests.cs b/tests/FixedMathSharp.Tests/FixedQuanternion.Tests.cs index 5e15571..b0e4ad7 100644 --- a/tests/FixedMathSharp.Tests/FixedQuanternion.Tests.cs +++ b/tests/FixedMathSharp.Tests/FixedQuanternion.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