Skip to content

Commit

Permalink
Fix floating point math issue
Browse files Browse the repository at this point in the history
  • Loading branch information
@gkjohnson
gkjohnson committed Dec 5, 2017
1 parent 2856d9c commit bfbb44c
Show file tree
Hide file tree
Showing 3 changed files with 43 additions and 34 deletions.
22 changes: 16 additions & 6 deletions Assets/Tests.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,12 +30,22 @@ public class Tests : MonoBehaviour {

Debug.Assert(ExpectException(() => new CoordinateFrame("XYZ", "XXZ")));
Debug.Assert(ExpectException(() => new CoordinateFrame("XYX", "XYZ")));

// Here to test issue #1
// https://github.com/gkjohnson/coordinate-frame-converter/issues/1
EulerAngles e = new EulerAngles(170, 90, -27);
EulerAngles to = Conversions.ConvertEulerOrder(Frames.Unity.RotationOrder, Frames.Unity.RotationOrder, e);
Debug.Assert(AreRotationsEquivalent(Frames.Unity.RotationOrder, e, to));

// Cases that failed before fixing the floating point math issue
{
EulerAngles e = new EulerAngles(170, 90, -27);
EulerAngles to = Conversions.ConvertEulerOrder(Frames.Unity.RotationOrder, Frames.Unity.RotationOrder, e);
Debug.Assert(AreRotationsEquivalent(Frames.Unity.RotationOrder, e, to));
}

{
AxisSet ro = new AxisSet("-X-Y-X", true);
EulerAngles e = new EulerAngles(0, 90, 210);
EulerAngles to = Conversions.ConvertEulerOrder(Frames.Unity.RotationOrder, ro, e);
EulerAngles back = Conversions.ConvertEulerOrder(ro, Frames.Unity.RotationOrder, to);

Debug.Assert(AreRotationsEquivalent(Frames.Unity.RotationOrder, e, back));
}

StartCoroutine(RunConversionTests());
}
Expand Down
49 changes: 25 additions & 24 deletions Assets/helpers/AngleExtraction.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ public enum EulerResult {
EA_NOT_UNIQUE_SUM
}

static float epsilon = 1e-7f;
static double Get3x3ElementAtIndex(Matrix4x4 mat, int i) {
int col = i / 3;
int row = i % 3;
Expand All @@ -36,9 +37,9 @@ public static Vector3 ExtractEulerXYZ(Matrix4x4 mat, out EulerResult eulerRes)
// | r20 r21 r22 | | -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy |
// +- -+ +- -+

if (Get3x3ElementAtIndex(mat, 2) < 1)
if (Get3x3ElementAtIndex(mat, 2) < 1 - epsilon)
{
if (Get3x3ElementAtIndex(mat, 2) > -1)
if (Get3x3ElementAtIndex(mat, 2) > -(1 - epsilon))
{
// y_angle = asin(r02)
// x_angle = atan2(-r12,r22)
Expand Down Expand Up @@ -84,9 +85,9 @@ public static Vector3 ExtractEulerXZY(Matrix4x4 mat, out EulerResult eulerRes)
// | r20 r21 r22 | | -cx*sy+cy*sx*sz cz*sx cx*cy+sx*sy*sz |
// +- -+ +- -+

if (Get3x3ElementAtIndex(mat, 1) < 1)
if (Get3x3ElementAtIndex(mat, 1) < 1 - epsilon)
{
if (Get3x3ElementAtIndex(mat, 1) > -1)
if (Get3x3ElementAtIndex(mat, 1) > -(1 - epsilon))
{
// z_angle = asin(-r01)
// x_angle = atan2(r21,r11)
Expand Down Expand Up @@ -132,9 +133,9 @@ public static Vector3 ExtractEulerYXZ(Matrix4x4 mat, out EulerResult eulerRes)
// | r20 r21 r22 | | -cz*sy+cy*sx*sz cy*cz*sx+sy*sz cx*cy |
// +- -+ +- -+

if (Get3x3ElementAtIndex(mat, 5) < 1)
if (Get3x3ElementAtIndex(mat, 5) < 1 - epsilon)
{
if (Get3x3ElementAtIndex(mat, 5) > -1)
if (Get3x3ElementAtIndex(mat, 5) > -(1 - epsilon))
{
// x_angle = asin(-r12)
// y_angle = atan2(r02,r22)
Expand Down Expand Up @@ -180,9 +181,9 @@ public static Vector3 ExtractEulerYZX(Matrix4x4 mat, out EulerResult eulerRes)
// | r20 r21 r22 | | -cz*sy cy*sx+cx*sy*sz cx*cy-sx*sy*sz |
// +- -+ +- -+

if (Get3x3ElementAtIndex(mat, 3) < 1)
if (Get3x3ElementAtIndex(mat, 3) < 1 - epsilon)
{
if (Get3x3ElementAtIndex(mat, 3) > -1)
if (Get3x3ElementAtIndex(mat, 3) > -(1 - epsilon))
{
// z_angle = asin(r10)
// y_angle = atan2(-r20,r00)
Expand Down Expand Up @@ -228,9 +229,9 @@ public static Vector3 ExtractEulerZXY(Matrix4x4 mat, out EulerResult eulerRes)
// | r20 r21 r22 | | -cx*sy sx cx*cy |
// +- -+ +- -+

if (Get3x3ElementAtIndex(mat, 7) < 1)
if (Get3x3ElementAtIndex(mat, 7) < 1 - epsilon)
{
if (Get3x3ElementAtIndex(mat, 7) > -1)
if (Get3x3ElementAtIndex(mat, 7) > -(1 - epsilon))
{
// x_angle = asin(r21)
// z_angle = atan2(-r01,r11)
Expand Down Expand Up @@ -277,9 +278,9 @@ public static Vector3 ExtractEulerZYX(Matrix4x4 mat, out EulerResult eulerRes)
// | r20 r21 r22 | | -sy cy*sx cx*cy |
// +- -+ +- -+

if (Get3x3ElementAtIndex(mat, 6) < 1)
if (Get3x3ElementAtIndex(mat, 6) < 1 - epsilon)
{
if (Get3x3ElementAtIndex(mat, 6) > -1)
if (Get3x3ElementAtIndex(mat, 6) > -(1 - epsilon))
{
// y_angle = asin(-r20)
// z_angle = atan2(r10,r00)
Expand Down Expand Up @@ -325,9 +326,9 @@ public static Vector3 ExtractEulerXYX(Matrix4x4 mat, out EulerResult eulerRes)
// | r20 r21 r22 | | -sy*cx0 cx1*sx0+cy*cx0*sx1 cy*cx0*cx1-sx0*sx1 |
// +- -+ +- -+

if (Get3x3ElementAtIndex(mat, 0) < 1)
if (Get3x3ElementAtIndex(mat, 0) < 1 - epsilon)
{
if (Get3x3ElementAtIndex(mat, 0) > -1)
if (Get3x3ElementAtIndex(mat, 0) > -(1 - epsilon))
{
// y_angle = acos(r00)
// x0_angle = atan2(r10,-r20)
Expand Down Expand Up @@ -369,9 +370,9 @@ public static Vector3 ExtractEulerXZX(Matrix4x4 mat, out EulerResult eulerRes)
// | r20 r21 r22 | | sz*sx0 cz*cx1*sx0+cx0*sx1 cx0*cx1-cz*sx0*sx1 |
// +- -+ +- -+

if (Get3x3ElementAtIndex(mat, 0) < 1)
if (Get3x3ElementAtIndex(mat, 0) < 1 - epsilon)
{
if (Get3x3ElementAtIndex(mat, 0) > -1)
if (Get3x3ElementAtIndex(mat, 0) > -(1 - epsilon))
{
// z_angle = acos(r00)
// x0_angle = atan2(r20,r10)
Expand Down Expand Up @@ -413,9 +414,9 @@ public static Vector3 ExtractEulerYXY(Matrix4x4 mat, out EulerResult eulerRes)
// | r20 r21 r22 | | -cy1*sy0-cx*cy0*sy1 sx*cy0 cx*cy0*cy1-sy0*sy1 |
// +- -+ +- -+

if (Get3x3ElementAtIndex(mat, 4) < 1)
if (Get3x3ElementAtIndex(mat, 4) < 1 - epsilon)
{
if (Get3x3ElementAtIndex(mat, 4) > -1)
if (Get3x3ElementAtIndex(mat, 4) > -(1 - epsilon))
{
// x_angle = acos(r11)
// y0_angle = atan2(r01,r21)
Expand Down Expand Up @@ -457,9 +458,9 @@ public static Vector3 ExtractEulerYZY(Matrix4x4 mat, out EulerResult eulerRes)
// | r20 r21 r22 | | -cz*cy1*sy0-cy0*sy1 sz*sy0 cy0*cy1-cz*sy0*sy1 |
// +- -+ +- -+

if (Get3x3ElementAtIndex(mat, 4) < 1)
if (Get3x3ElementAtIndex(mat, 4) < 1 - epsilon)
{
if (Get3x3ElementAtIndex(mat, 4) > -1)
if (Get3x3ElementAtIndex(mat, 4) > -(1 - epsilon))
{
// z_angle = acos(r11)
// y0_angle = atan2(r21,-r01)
Expand Down Expand Up @@ -501,9 +502,9 @@ public static Vector3 ExtractEulerZXZ(Matrix4x4 mat, out EulerResult eulerRes)
// | r20 r21 r22 | | sx*sz1 sx*cz1 cx |
// +- -+ +- -+

if (Get3x3ElementAtIndex(mat, 8) < 1)
if (Get3x3ElementAtIndex(mat, 8) < 1 - epsilon)
{
if (Get3x3ElementAtIndex(mat, 8) > -1)
if (Get3x3ElementAtIndex(mat, 8) > -(1 - epsilon))
{
// x_angle = acos(r22)
// z0_angle = atan2(r02,-r12)
Expand Down Expand Up @@ -545,9 +546,9 @@ public static Vector3 ExtractEulerZYZ(Matrix4x4 mat, out EulerResult eulerRes)
// | r20 r21 r22 | | -sy*cz1 sy*sz1 cy |
// +- -+ +- -+

if (Get3x3ElementAtIndex(mat, 8) < 1)
if (Get3x3ElementAtIndex(mat, 8) < 1 - epsilon)
{
if (Get3x3ElementAtIndex(mat, 8) > -1)
if (Get3x3ElementAtIndex(mat, 8) > -(1 - epsilon))
{
// y_angle = acos(r22)
// z0_angle = atan2(r12,r02)
Expand Down
6 changes: 2 additions & 4 deletions Assets/lib/Conversions.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -163,10 +163,8 @@ static class Conversions {
angles *= -1;
res = Quaternion.Euler(angles) * res;
}

// See issue #1
// https://github.com/gkjohnson/coordinate-frame-converter/issues/1
return Quaternion.Euler(res.eulerAngles);

return res;
}

// Outputs euler angles in degrees
Expand Down

0 comments on commit bfbb44c

Please sign in to comment.