MoonsideGames
/
MoonWorks
1
0
Fork 1
Code Issues 4 Pull Requests Projects Releases 1 Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/Math/Fixed/Fix64.cs
View File

@ -104,6 +104,12 @@ namespace MoonWorks.Math.Fixed
: integralPart + One; : 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) public static Fix64 Min(Fix64 x, Fix64 y)
{ {
return (x < y) ? x : y; return (x < y) ? x : y;

30
src/Math/Fixed/Matrix3x2.cs
View File

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