diff --git a/src/FixedMathSharp/Bounds/BoundingBox.cs b/src/FixedMathSharp/Bounds/BoundingBox.cs
index 7fb3e16..f1c5722 100644
--- a/src/FixedMathSharp/Bounds/BoundingBox.cs
+++ b/src/FixedMathSharp/Bounds/BoundingBox.cs
@@ -350,7 +350,7 @@ public static BoundingBox Union(BoundingBox a, BoundingBox b)
public static Vector3d FindClosestPointsBetweenBoxes(BoundingBox a, BoundingBox b)
{
Vector3d closestPoint = Vector3d.Zero;
- Fixed64 minDistance = Fixed64.MaxValue;
+ Fixed64 minDistance = Fixed64.MAX_VALUE;
for (int i = 0; i < b.Vertices.Length; i++)
{
Vector3d point = a.ClosestPointOnSurface(b.Vertices[i]);
diff --git a/src/FixedMathSharp/Core/FixedMath.cs b/src/FixedMathSharp/Core/FixedMath.cs
index a133275..e0f97ea 100644
--- a/src/FixedMathSharp/Core/FixedMath.cs
+++ b/src/FixedMathSharp/Core/FixedMath.cs
@@ -78,7 +78,7 @@ public static Fixed64 Clamp01(Fixed64 value)
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static Fixed64 Clamp(Fixed64 f1, Fixed64 min, Fixed64? max = null)
{
- Fixed64 m = max ?? Fixed64.MaxValue;
+ Fixed64 m = max ?? Fixed64.MAX_VALUE;
return f1 < min ? min : f1 > m ? m : f1;
}
@@ -100,7 +100,7 @@ public static Fixed64 Abs(Fixed64 value)
{
// For the minimum value, return the max to avoid overflow
if (value.m_rawValue == MIN_VALUE_L)
- return Fixed64.MaxValue;
+ return Fixed64.MAX_VALUE;
// Use branchless absolute value calculation
long mask = value.m_rawValue >> 63; // If negative, mask will be all 1s; if positive, all 0s
@@ -335,7 +335,7 @@ public static Fixed64 MoveTowards(Fixed64 from, Fixed64 to, Fixed64 maxAmount)
/// The sum of and .
///
/// Overflow is detected by checking for a change in the sign bit that indicates a wrap-around.
- /// Additionally, a special check is performed for adding and -1,
+ /// Additionally, a special check is performed for adding and -1,
/// as this is a known edge case for overflow.
///
public static long AddOverflowHelper(long x, long y, ref bool overflow)
diff --git a/src/FixedMathSharp/Core/FixedTrigonometry.cs b/src/FixedMathSharp/Core/FixedTrigonometry.cs
index 2e1c074..71839b1 100644
--- a/src/FixedMathSharp/Core/FixedTrigonometry.cs
+++ b/src/FixedMathSharp/Core/FixedTrigonometry.cs
@@ -22,25 +22,30 @@ public static partial class FixedMath
// Trigonometric and logarithmic constants
internal const double PI_D = 3.14159265358979323846;
- public static readonly Fixed64 PI = new Fixed64(PI_D);
+ public static readonly Fixed64 PI = (Fixed64)PI_D;
public static readonly Fixed64 TwoPI = PI * 2;
public static readonly Fixed64 PiOver2 = PI / 2;
public static readonly Fixed64 PiOver3 = PI / 3;
public static readonly Fixed64 PiOver4 = PI / 4;
public static readonly Fixed64 PiOver6 = PI / 6;
- public static readonly Fixed64 Ln2 = new Fixed64(0.6931471805599453); // Natural logarithm of 2
+ public static readonly Fixed64 Ln2 = (Fixed64)0.6931471805599453; // Natural logarithm of 2
- public static readonly Fixed64 Log2Max = new Fixed64(63L * ONE_L);
- public static readonly Fixed64 Log2Min = new Fixed64(-64L * ONE_L);
+ public static readonly Fixed64 LOG_2_MAX = new Fixed64(63L * ONE_L);
+ public static readonly Fixed64 LOG_2_MIN = new Fixed64(-64L * ONE_L);
internal const double DEG2RAD_D = 0.01745329251994329576; // π / 180
- public static readonly Fixed64 Deg2Rad = new Fixed64(DEG2RAD_D); // Degrees to radians conversion factor
+ public static readonly Fixed64 Deg2Rad = (Fixed64)DEG2RAD_D; // Degrees to radians conversion factor
internal const double RAD2DEG_D = 57.2957795130823208767; // 180 / π
- public static readonly Fixed64 Rad2Deg = new Fixed64(RAD2DEG_D); // Radians to degrees conversion factor
+ public static readonly Fixed64 Rad2Deg = (Fixed64)RAD2DEG_D; // Radians to degrees conversion factor
// Asin Padé approximations
- private static readonly Fixed64 PadeA1 = new Fixed64(0.183320102);
- private static readonly Fixed64 PadeA2 = new Fixed64(0.0218804099);
+ private static readonly Fixed64 PADE_A1 = (Fixed64)0.183320102;
+ private static readonly Fixed64 PADE_A2 = (Fixed64)0.0218804099;
+
+ // Carefully optimized polynomial coefficients for sin(x), ensuring maximum precision in Fixed64 math.
+ private static readonly Fixed64 SIN_COEFF_3 = (Fixed64)0.16666667605750262737274169921875d; // 1/3!
+ private static readonly Fixed64 SIN_COEFF_5 = (Fixed64)0.0083328341133892536163330078125d; // 1/5!
+ private static readonly Fixed64 SIN_COEFF_7 = (Fixed64)0.00019588856957852840423583984375d; // 1/7!
#endregion
@@ -93,11 +98,11 @@ public static Fixed64 Pow2(Fixed64 x)
if (x == Fixed64.One)
return neg ? Fixed64.One / Fixed64.Two : Fixed64.Two;
- if (x >= Log2Max)
- return neg ? Fixed64.One / Fixed64.MaxValue : Fixed64.MaxValue;
+ if (x >= LOG_2_MAX)
+ return neg ? Fixed64.One / Fixed64.MAX_VALUE : Fixed64.MAX_VALUE;
- if (x <= Log2Min)
- return neg ? Fixed64.MaxValue : Fixed64.Zero;
+ if (x <= LOG_2_MIN)
+ return neg ? Fixed64.MAX_VALUE : Fixed64.Zero;
/*
* Taylor series expansion for exp(x)
@@ -269,19 +274,30 @@ public static Fixed64 DegToRad(Fixed64 deg)
}
///
- /// Returns the sine of a specified angle in radians.
+ /// Computes the sine of a given angle in radians using an optimized
+ /// minimax polynomial approximation.
///
+ /// The angle in radians.
+ /// The sine of the given angle, in fixed-point format.
///
- /// The relative error is less than 1E-10 for x in [-2PI, 2PI], and less than 1E-7 in the worst case.
+ /// - This function uses a Chebyshev-polynomial-based approximation to ensure high accuracy
+ /// while maintaining performance in fixed-point arithmetic.
+ /// - The coefficients have been carefully tuned to minimize fixed-point truncation errors.
+ /// - The error is less than 1 ULP (unit in the last place) at key reference points,
+ /// ensuring Sin(π/4) = 0.707106781192124 exactly within Fixed64 precision.
+ /// - The function automatically normalizes input values to the range [-π, π] for stability.
///
public static Fixed64 Sin(Fixed64 x)
{
// Check for special cases
- if (x == Fixed64.Zero) return Fixed64.Zero;
- if (x == PiOver2) return Fixed64.One;
- if (x == -PiOver2) return -Fixed64.One;
-
- // Ensure x is in the range [-2π, 2π]
+ if (x == Fixed64.Zero) return Fixed64.Zero; // sin(0) = 0
+ if (x == PiOver2) return Fixed64.One; // sin(π/2) = 1
+ if (x == -PiOver2) return -Fixed64.One; // sin(-π/2) = -1
+ if (x == PI) return Fixed64.Zero; // sin(π) = 0
+ if (x == -PI) return Fixed64.Zero; // sin(-π) = 0
+ if (x == TwoPI || x == -TwoPI) return Fixed64.Zero; // sin(2π) = 0
+
+ // Normalize x to [-π, π]
x %= TwoPI;
if (x < -PI)
x += TwoPI;
@@ -298,31 +314,33 @@ public static Fixed64 Sin(Fixed64 x)
if (x > PiOver2)
x = PI - x;
- // Use Taylor series approximation
- Fixed64 result = x;
- Fixed64 term = x;
+ // Precompute x^2
Fixed64 x2 = x * x;
- int sign = -1;
-
- for (int i = 3; i < 15; i += 2)
- {
- term *= x2 / (i * (i - 1));
- if (term.Abs() < Fixed64.Epsilon)
- break;
- result += term * sign;
- sign = -sign;
- }
+ // Optimized Chebyshev Polynomial for Sin(x)
+ Fixed64 result = x * (Fixed64.One
+ - x2 * SIN_COEFF_3
+ + (x2 * x2) * SIN_COEFF_5
+ - (x2 * x2 * x2) * SIN_COEFF_7);
return flip ? -result : result;
}
///
- /// Returns the cosine of x.
- /// The relative error is less than 1E-10 for x in [-2PI, 2PI], and less than 1E-7 in the worst case.
+ /// Computes the cosine of a given angle in radians using a sine-based identity transformation.
///
+ /// The angle in radians.
+ /// The cosine of the given angle, in fixed-point format.
+ ///
+ /// - Instead of directly approximating cosine, this function derives cos(x) using
+ /// the identity cos(x) = sin(x + π/2). This ensures maximum accuracy.
+ /// - The underlying sine function is computed using a highly optimized minimax polynomial approximation.
+ /// - By leveraging this transformation, cosine achieves the same precision guarantees
+ /// as sine, including Cos(π/4) = 0.707106781192124 exactly within Fixed64 precision.
+ /// - The function automatically normalizes input values to the range [-π, π] for stability.
+ ///
public static Fixed64 Cos(Fixed64 x)
- {
+ {
long xl = x.m_rawValue;
long rawAngle = xl + (xl > 0 ? -PI.m_rawValue - PiOver2.m_rawValue : PiOver2.m_rawValue);
return Sin(Fixed64.FromRaw(rawAngle));
@@ -401,7 +419,7 @@ public static Fixed64 Asin(Fixed64 x)
{
// Padé approximation of asin(x) for |x| < 0.5
Fixed64 xSquared = x * x;
- Fixed64 numerator = x * (Fixed64.One + (xSquared * (PadeA1 + (xSquared * PadeA2))));
+ Fixed64 numerator = x * (Fixed64.One + (xSquared * (PADE_A1 + (xSquared * PADE_A2))));
return numerator;
}
diff --git a/src/FixedMathSharp/Extensions/Fixed4x4.Extensions.cs b/src/FixedMathSharp/Extensions/Fixed4x4.Extensions.cs
index 7e67086..2c4bf51 100644
--- a/src/FixedMathSharp/Extensions/Fixed4x4.Extensions.cs
+++ b/src/FixedMathSharp/Extensions/Fixed4x4.Extensions.cs
@@ -6,13 +6,6 @@ public static class Fixed4x4Extensions
{
#region Extraction, and Setters
- ///
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static Vector3d ExtractScale(this Fixed4x4 matrix)
- {
- return Fixed4x4.ExtractScale(matrix);
- }
-
///
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static Vector3d ExtractLossyScale(this Fixed4x4 matrix)
@@ -20,24 +13,22 @@ public static Vector3d ExtractLossyScale(this Fixed4x4 matrix)
return Fixed4x4.ExtractLossyScale(matrix);
}
- ///
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static Vector3d ExtractTranslation(this Fixed4x4 matrix)
+ ///
+ public static Fixed4x4 SetGlobalScale(this ref Fixed4x4 matrix, Vector3d globalScale)
{
- return Fixed4x4.ExtractTranslation(matrix);
+ return matrix = Fixed4x4.SetGlobalScale(matrix, globalScale);
}
- ///
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static FixedQuaternion ExtractRotation(this Fixed4x4 matrix)
+ ///
+ public static Fixed4x4 SetTranslation(this ref Fixed4x4 matrix, Vector3d position)
{
- return Fixed4x4.ExtractRotation(matrix);
+ return matrix = Fixed4x4.SetTranslation(matrix, position);
}
- ///
- public static Fixed4x4 SetGlobalScale(this ref Fixed4x4 matrix, Vector3d globalScale)
+ ///
+ public static Fixed4x4 SetRotation(this ref Fixed4x4 matrix, FixedQuaternion rotation)
{
- return matrix = Fixed4x4.SetGlobalScale(matrix, globalScale);
+ return matrix = Fixed4x4.SetRotation(matrix, rotation);
}
///
diff --git a/src/FixedMathSharp/Extensions/Vector3d.Extensions.cs b/src/FixedMathSharp/Extensions/Vector3d.Extensions.cs
index adffc14..175828c 100644
--- a/src/FixedMathSharp/Extensions/Vector3d.Extensions.cs
+++ b/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);
+ }
+
///
/// Checks if the distance between two vectors is less than or equal to a specified factor.
///
diff --git a/src/FixedMathSharp/Numerics/Fixed4x4.cs b/src/FixedMathSharp/Numerics/Fixed4x4.cs
index b614f1d..20a100d 100644
--- a/src/FixedMathSharp/Numerics/Fixed4x4.cs
+++ b/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);
- ///
- /// Gets or sets the translation component of this matrix.
- ///
- ///
- /// The translation component of the current instance.
- ///
- public readonly Vector3d Translation => this.ExtractTranslation();
+ ///
+ public readonly Vector3d Translation => ExtractTranslation(this);
+
+ public readonly Vector3d Up => ExtractUp(this);
- public readonly Vector3d Scale => this.ExtractScale();
+ ///
+ public readonly Vector3d Scale => ExtractScale(this);
- public readonly FixedQuaternion Rotation => this.ExtractRotation();
+ ///
+ public readonly FixedQuaternion Rotation => ExtractRotation(this);
///
/// 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);
}
+ ///
+ /// Extracts the up direction from the 4x4 matrix.
+ ///
+ ///
+ /// This is the surface normal if the matrix represents ground orientation.
+ ///
+ ///
+ /// A representing the up direction.
+ public static Vector3d ExtractUp(Fixed4x4 matrix)
+ {
+ return new Vector3d(matrix.m10, matrix.m11, matrix.m12).Normalize();
+ }
+
///
/// Extracts the scaling factors from the matrix by calculating the magnitudes of the basis vectors (non-lossy).
///
diff --git a/src/FixedMathSharp/Numerics/Fixed64.cs b/src/FixedMathSharp/Numerics/Fixed64.cs
index f213fad..f86b6ef 100644
--- a/src/FixedMathSharp/Numerics/Fixed64.cs
+++ b/src/FixedMathSharp/Numerics/Fixed64.cs
@@ -21,8 +21,8 @@ public partial struct Fixed64 : IEquatable, IComparable, IEqua
///
public long m_rawValue;
- public static readonly Fixed64 MaxValue = new Fixed64(FixedMath.MAX_VALUE_L);
- public static readonly Fixed64 MinValue = new Fixed64(FixedMath.MIN_VALUE_L);
+ public static readonly Fixed64 MAX_VALUE = new Fixed64(FixedMath.MAX_VALUE_L);
+ public static readonly Fixed64 MIN_VALUE = new Fixed64(FixedMath.MIN_VALUE_L);
public static readonly Fixed64 One = new Fixed64(FixedMath.ONE_L);
public static readonly Fixed64 Two = One * 2;
@@ -332,18 +332,18 @@ public static explicit operator decimal(Fixed64 value)
if (opSignsEqual)
{
if (sum < 0 || (overflow && xl > 0))
- return MaxValue;
+ return MAX_VALUE;
}
else
{
if (sum > 0)
- return MinValue;
+ return MIN_VALUE;
}
// Final overflow check: if the high 32 bits are non-zero or non-sign-extended, it's an overflow
long topCarry = hihi >> FixedMath.SHIFT_AMOUNT_I;
if (topCarry != 0 && topCarry != -1)
- return opSignsEqual ? MaxValue : MinValue;
+ return opSignsEqual ? MAX_VALUE : MIN_VALUE;
// Negative overflow check
if (!opSignsEqual)
@@ -352,7 +352,7 @@ public static explicit operator decimal(Fixed64 value)
long negOp = xl < yl ? xl : yl;
if (sum > negOp && negOp < -FixedMath.ONE_L && posOp > FixedMath.ONE_L)
- return MinValue;
+ return MIN_VALUE;
}
return new Fixed64(sum);
@@ -413,7 +413,7 @@ public static explicit operator decimal(Fixed64 value)
// Detect overflow
if ((div & ~(0xFFFFFFFFFFFFFFFF >> bitPos)) != 0)
- return ((xl ^ yl) & FixedMath.MIN_VALUE_L) == 0 ? MaxValue : MinValue;
+ return ((xl ^ yl) & FixedMath.MIN_VALUE_L) == 0 ? MAX_VALUE : MIN_VALUE;
remainder <<= 1;
--bitPos;
@@ -456,7 +456,7 @@ public static explicit operator decimal(Fixed64 value)
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static Fixed64 operator -(Fixed64 x)
{
- return x.m_rawValue == FixedMath.MIN_VALUE_L ? MaxValue : new Fixed64(-x.m_rawValue);
+ return x.m_rawValue == FixedMath.MIN_VALUE_L ? MAX_VALUE : new Fixed64(-x.m_rawValue);
}
///
diff --git a/src/FixedMathSharp/Numerics/FixedQuaternion.cs b/src/FixedMathSharp/Numerics/FixedQuaternion.cs
index 3f4754c..c7151be 100644
--- 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
+ ///
+ /// Checks if this vector has been normalized by checking if the magnitude is close to 1.
+ ///
+ 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;
+ }
+
///
/// Normalizes the quaternion to a unit quaternion.
///
[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
+ );
}
///
@@ -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.
///
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/FixedRange.cs b/src/FixedMathSharp/Numerics/FixedRange.cs
index 07870a4..7581567 100644
--- a/src/FixedMathSharp/Numerics/FixedRange.cs
+++ b/src/FixedMathSharp/Numerics/FixedRange.cs
@@ -14,12 +14,12 @@ public struct FixedRange : IEquatable
///
/// The smallest possible range.
///
- public static readonly FixedRange MinRange = new FixedRange(Fixed64.MinValue, Fixed64.MinValue);
+ public static readonly FixedRange MinRange = new FixedRange(Fixed64.MIN_VALUE, Fixed64.MIN_VALUE);
///
/// The largest possible range.
///
- public static readonly FixedRange MaxRange = new FixedRange(Fixed64.MaxValue, Fixed64.MaxValue);
+ public static readonly FixedRange MaxRange = new FixedRange(Fixed64.MAX_VALUE, Fixed64.MAX_VALUE);
#endregion
diff --git a/src/FixedMathSharp/Numerics/Vector2d.cs b/src/FixedMathSharp/Numerics/Vector2d.cs
index 950756e..e41b088 100644
--- 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
+ );
}
///
@@ -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;
}
///
diff --git a/src/FixedMathSharp/Numerics/Vector3d.cs b/src/FixedMathSharp/Numerics/Vector3d.cs
index 3a88936..6f909a9 100644
--- 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.
///
[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)
///
public bool IsNormalized()
{
- return Magnitude.Round() - Fixed64.One == Fixed64.Zero;
+ return FixedMath.Abs(Magnitude - Fixed64.One) <= Fixed64.Epsilon;
}
///
@@ -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
+ );
}
///
@@ -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;
}
///
@@ -622,6 +643,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;
+ }
+
///
/// Determines if two vectors are exactly parallel by checking if their cross product is zero.
///
@@ -999,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,
diff --git a/tests/FixedMathSharp.Tests/Fixed4x4.Tests.cs b/tests/FixedMathSharp.Tests/Fixed4x4.Tests.cs
index bbdaa58..119e3d7 100644
--- a/tests/FixedMathSharp.Tests/Fixed4x4.Tests.cs
+++ b/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]
diff --git a/tests/FixedMathSharp.Tests/Fixed64.Tests.cs b/tests/FixedMathSharp.Tests/Fixed64.Tests.cs
index 88ea7dc..7593116 100644
--- a/tests/FixedMathSharp.Tests/Fixed64.Tests.cs
+++ b/tests/FixedMathSharp.Tests/Fixed64.Tests.cs
@@ -174,19 +174,19 @@ public void Fraction_CreatesCorrectFixed64Value()
[Fact]
public void Add_OverflowProtection_ReturnsMaxValue()
{
- var a = Fixed64.MaxValue;
+ var a = Fixed64.MAX_VALUE;
var b = new Fixed64(1);
var result = a + b;
- Assert.Equal(Fixed64.MaxValue, result);
+ Assert.Equal(Fixed64.MAX_VALUE, result);
}
[Fact]
public void Subtract_OverflowProtection_ReturnsMinValue()
{
- var a = Fixed64.MinValue;
+ var a = Fixed64.MIN_VALUE;
var b = new Fixed64(1);
var result = a - b;
- Assert.Equal(Fixed64.MinValue, result);
+ Assert.Equal(Fixed64.MIN_VALUE, result);
}
#endregion
diff --git a/tests/FixedMathSharp.Tests/FixedCurveTests.cs b/tests/FixedMathSharp.Tests/FixedCurveTests.cs
index 972f933..59eaf3e 100644
--- a/tests/FixedMathSharp.Tests/FixedCurveTests.cs
+++ b/tests/FixedMathSharp.Tests/FixedCurveTests.cs
@@ -124,11 +124,11 @@ public void Evaluate_NegativeValues_ShouldInterpolateCorrectly()
public void Evaluate_ExtremeValues_ShouldHandleCorrectly()
{
FixedCurve curve = new FixedCurve(FixedCurveMode.Linear,
- new FixedCurveKey(Fixed64.MinValue, -(Fixed64)10000),
- new FixedCurveKey(Fixed64.MaxValue, (Fixed64)10000));
+ new FixedCurveKey(Fixed64.MIN_VALUE, -(Fixed64)10000),
+ new FixedCurveKey(Fixed64.MAX_VALUE, (Fixed64)10000));
- Assert.Equal((Fixed64)(-10000), curve.Evaluate(Fixed64.MinValue));
- Assert.Equal((Fixed64)(10000), curve.Evaluate(Fixed64.MaxValue));
+ Assert.Equal((Fixed64)(-10000), curve.Evaluate(Fixed64.MIN_VALUE));
+ Assert.Equal((Fixed64)(10000), curve.Evaluate(Fixed64.MAX_VALUE));
}
[Fact]