fix Matrix3x2 rotation

pull/19/head
cosmonaut 2022-05-04 11:03:02 -07:00
parent 6e600b108f
commit 40e4be6d36
2 changed files with 20 additions and 16 deletions

View File

@ -104,6 +104,12 @@ namespace MoonWorks.Math.Fixed
: integralPart + One;
}
//Formula taken from https://docs.microsoft.com/en-us/dotnet/api/system.math.ieeeremainder?view=net-6.0
public static Fix64 IEEERemainder(Fix64 dividend, Fix64 divisor)
{
return dividend - (divisor * Round(dividend / divisor));
}
public static Fix64 Min(Fix64 x, Fix64 y)
{
return (x < y) ? x : y;

View File

@ -51,6 +51,8 @@ namespace MoonWorks.Math.Fixed
0, 0
);
private static readonly Fix64 RotationEpsilon = (Fix64.One / new Fix64(1000)) * (Fix64.Pi / new Fix64(180));
/// <summary>
/// Returns the multiplicative identity matrix.
/// </summary>
@ -343,31 +345,29 @@ namespace MoonWorks.Math.Fixed
{
Matrix3x2 result;
radians = radians % Fix64.PiTimes2;
radians = Fix64.IEEERemainder(radians, Fix64.PiTimes2);
Fix64 c, s;
Fix64 epsilon = (Fix64) 0.001f * (Fix64.Pi / new Fix64(180));
if (radians > -epsilon && radians < epsilon)
if (radians > -RotationEpsilon && radians < RotationEpsilon)
{
// Exact case for zero rotation.
c = Fix64.One;
s = Fix64.Zero;
}
else if (radians > Fix64.PiOver2 - epsilon && radians < Fix64.PiOver2 + epsilon)
else if (radians > Fix64.PiOver2 - RotationEpsilon && radians < Fix64.PiOver2 + RotationEpsilon)
{
// Exact case for 90 degree rotation.
c = Fix64.Zero;
s = Fix64.One;
}
else if (radians < -Fix64.Pi + epsilon || radians > Fix64.Pi - epsilon)
else if (radians < -Fix64.Pi + RotationEpsilon || radians > Fix64.Pi - RotationEpsilon)
{
// Exact case for 180 degree rotation.
c = -Fix64.One;
s = Fix64.Zero;
}
else if (radians > -Fix64.PiOver2 - epsilon && radians < -Fix64.PiOver2 + epsilon)
else if (radians > -Fix64.PiOver2 - RotationEpsilon && radians < -Fix64.PiOver2 + RotationEpsilon)
{
// Exact case for 270 degree rotation.
c = Fix64.Zero;
@ -376,8 +376,8 @@ namespace MoonWorks.Math.Fixed
else
{
// Arbitrary rotation.
c = (Fix64) Fix64.Cos(radians);
s = (Fix64) Fix64.Sin(radians);
c = Fix64.Cos(radians);
s = Fix64.Sin(radians);
}
// [ c s ]
@ -403,31 +403,29 @@ namespace MoonWorks.Math.Fixed
{
Matrix3x2 result;
radians = radians % Fix64.PiTimes2;
radians = Fix64.IEEERemainder(radians, Fix64.PiTimes2);
Fix64 c, s;
Fix64 epsilon = (Fix64) 0.001f * (Fix64.Pi / new Fix64(180));
if (radians > -epsilon && radians < epsilon)
if (radians > -RotationEpsilon && radians < RotationEpsilon)
{
// Exact case for zero rotation.
c = Fix64.One;
s = Fix64.Zero;
}
else if (radians > Fix64.PiOver2 - epsilon && radians < Fix64.PiOver2 + epsilon)
else if (radians > Fix64.PiOver2 - RotationEpsilon && radians < Fix64.PiOver2 + RotationEpsilon)
{
// Exact case for 90 degree rotation.
c = Fix64.Zero;
s = Fix64.One;
}
else if (radians < -Fix64.Pi + epsilon || radians > Fix64.Pi - epsilon)
else if (radians < -Fix64.Pi + RotationEpsilon || radians > Fix64.Pi - RotationEpsilon)
{
// Exact case for 180 degree rotation.
c = -Fix64.One;
s = Fix64.Zero;
}
else if (radians > -Fix64.PiOver2 - epsilon && radians < -Fix64.PiOver2 + epsilon)
else if (radians > -Fix64.PiOver2 - RotationEpsilon && radians < -Fix64.PiOver2 + RotationEpsilon)
{
// Exact case for 270 degree rotation.
c = Fix64.Zero;