fix Matrix3x2 rotation
parent
6e600b108f
commit
40e4be6d36
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue