ジンバルロック判定を改善

diff --git a/src/main/java/jp/sfjp/mikutoga/math/MkQuat.java b/src/main/java/jp/sfjp/mikutoga/math/MkQuat.java
index ba6e649..974732c 100644 (file)
--- a/src/main/java/jp/sfjp/mikutoga/math/MkQuat.java
+++ b/src/main/java/jp/sfjp/mikutoga/math/MkQuat.java
@@ -14,11 +14,18 @@ package jp.sfjp.mikutoga.math;
 public strictfp class MkQuat {
 
     private static final double HALF_PI = StrictMath.PI / 2.0;
 public strictfp class MkQuat {
 
     private static final double HALF_PI = StrictMath.PI / 2.0;

-    private static final double EPSILON = StrictMath.ulp(1.0);
-    private static final double TDELTA = EPSILON * 4;
+    private static final double DBL_PI = StrictMath.PI * 2.0;
+    private static final int STEP_BELOW = 5;
+    private static final double BELOWONE;

 
     static{
 
     static{

-        assert StrictMath.ulp(StrictMath.PI) <= TDELTA;
+        double one = 1.0;
+        for(int ct=1; ct<=STEP_BELOW; ct++){
+            one = StrictMath.nextAfter(one, 0.0);
+        }
+        BELOWONE = one;
+
+        assert BELOWONE < 1.0;

     }
 
 
     }
 
 


@@ -373,15 +380,27 @@ public strictfp class MkQuat {
         double resultY;
         double resultZ;
 
         double resultY;
         double resultZ;
 

-        if     (m12 < -1.0) resultX = +HALF_PI;
-        else if(m12 > +1.0) resultX = -HALF_PI;
-        else                resultX = StrictMath.asin(-m12);
-
-        if(   StrictMath.abs(m11) <= TDELTA    // Y,Zが一意に定まらない場合
+        /*
+        private static final double EPSILON = StrictMath.ulp(1.0);
+        private static final double TDELTA = EPSILON * 4;
+        if(   StrictMath.abs(m11) <= TDELTA

            || StrictMath.abs(m22) <= TDELTA ){
            || StrictMath.abs(m22) <= TDELTA ){

+        */
+        // Y,Zが一意に定まらない場合
+        if(StrictMath.abs(m12) >= BELOWONE){
+            resultX = -StrictMath.copySign(HALF_PI, m12);
+

             resultY = oldY;
             resultY = oldY;

-            resultZ = StrictMath.atan2(-m01, m00) + oldY;
+
+            resultZ = StrictMath.atan2(-m01, m00);
+            if(resultX >= 0.0) resultZ += resultY;
+            else               resultZ -= resultY;
+
+            if(StrictMath.abs(resultZ) > StrictMath.PI){
+                resultZ -= StrictMath.copySign(DBL_PI, resultZ);
+            }

         }else{
         }else{

+            resultX = StrictMath.asin(-m12);

             resultY = StrictMath.atan2(m02, m22);
             resultZ = StrictMath.atan2(m10, m11);
         }
             resultY = StrictMath.atan2(m02, m22);
             resultZ = StrictMath.atan2(m10, m11);
         }

diff --git a/src/test/java/jp/sfjp/mikutoga/math/MkQuatTest.java b/src/test/java/jp/sfjp/mikutoga/math/MkQuatTest.java
index 6db9786..85cd7cc 100644 (file)
--- a/src/test/java/jp/sfjp/mikutoga/math/MkQuatTest.java
+++ b/src/test/java/jp/sfjp/mikutoga/math/MkQuatTest.java
@@ -41,16 +41,27 @@ public strictfp class MkQuatTest {
     public void tearDown() {
     }
 
     public void tearDown() {
     }
 

-    private void assert0UlpEquals(double expected, double x){
-        assertUlpEquals(expected, x, 0);
+    private void assert0UlpEquals(double expected, double result){
+        assertUlpEquals(expected, result, 0);

         return;
     }
 
         return;
     }
 

-    private void assertUlpEquals(double expected, double x, int ulpNum){
-        double ulp = StrictMath.ulp(expected);
+    private void assertUlpEquals(double expected, double result, int ulpNum){
+        double ulpExpected = StrictMath.ulp(expected);
+        double ulpResult   = StrictMath.ulp(result);
+        double ulp = StrictMath.max(ulpExpected, ulpResult);

         double delta = ulp * ulpNum;
 
         double delta = ulp * ulpNum;
 

-        assertEquals(expected, x, delta);
+        try{
+            assertEquals(expected, result, delta);
+        }catch(AssertionError e){
+            double dist = StrictMath.abs(expected - result);
+            String msg = e.getMessage();
+            msg += " ULP * " + (dist / ulp);
+            AssertionError err = new AssertionError(msg);
+            err.initCause(e);
+            throw err;
+        }

 
         return;
     }
 
         return;
     }


@@ -145,6 +156,23 @@ public strictfp class MkQuatTest {
         assert0UlpEquals(54.0, result.getQ3());
         assert0UlpEquals(-8.0, result.getQW());
 
         assert0UlpEquals(54.0, result.getQ3());
         assert0UlpEquals(-8.0, result.getQW());
 

+        EulerYXZ eu = new EulerYXZ();
+        qqa.setEulerYXZ(0.0, RAD_60DEG, 0.0);
+        qqb.setEulerYXZ(RAD_30DEG, 0.0, 0.0);
+        MkQuat.mul(qqa, qqb, result);
+        result.toEulerYXZ(eu, 0.0);
+        assertUlpEquals(RAD_30DEG, eu.getXRot(), 1);
+        assertUlpEquals(RAD_60DEG, eu.getYRot(), 0);
+        assertEquals(0.0, eu.getZRot(), EPSILON);
+
+        qqa.setEulerYXZ(0.0, RAD_15DEG, 0.0);
+        qqb.setEulerYXZ(0.0, RAD_30DEG, 0.0);
+        MkQuat.mul(qqa, qqb, result);
+        result.toEulerYXZ(eu, 0.0);
+        assert0UlpEquals(0.0, eu.getXRot());
+        assert0UlpEquals(RAD_45DEG, eu.getYRot());
+        assert0UlpEquals(0.0, eu.getZRot());
+

         return;
     }
 
         return;
     }
 


@@ -189,10 +217,10 @@ public strictfp class MkQuatTest {
 
         qq = new MkQuat(2.0, 2.0, 2.0, 2.0);
         MkQuat.normalize(qq, result);
 
         qq = new MkQuat(2.0, 2.0, 2.0, 2.0);
         MkQuat.normalize(qq, result);

-        assertUlpEquals(0.5, result.getQ1(), 0);
-        assertUlpEquals(0.5, result.getQ2(), 0);
-        assertUlpEquals(0.5, result.getQ3(), 0);
-        assertUlpEquals(0.5, result.getQW(), 0);
+        assert0UlpEquals(0.5, result.getQ1());
+        assert0UlpEquals(0.5, result.getQ2());
+        assert0UlpEquals(0.5, result.getQ3());
+        assert0UlpEquals(0.5, result.getQW());

 
         return;
     }
 
         return;
     }


@@ -217,10 +245,10 @@ public strictfp class MkQuatTest {
 
         qq = new MkQuat(2.0, 2.0, 2.0, 2.0);
         MkQuat.inverse(qq, result);
 
         qq = new MkQuat(2.0, 2.0, 2.0, 2.0);
         MkQuat.inverse(qq, result);

-        assertUlpEquals(-0.125, result.getQ1(), 0);
-        assertUlpEquals(-0.125, result.getQ2(), 0);
-        assertUlpEquals(-0.125, result.getQ3(), 0);
-        assertUlpEquals(0.125, result.getQW(), 0);
+        assert0UlpEquals(-0.125, result.getQ1());
+        assert0UlpEquals(-0.125, result.getQ2());
+        assert0UlpEquals(-0.125, result.getQ3());
+        assert0UlpEquals(0.125, result.getQW());

 
         return;
     }
 
         return;
     }


@@ -241,10 +269,10 @@ public strictfp class MkQuatTest {
         assert0UlpEquals(0.0, qq.abs());
 
         qq = new MkQuat(1.0, 2.0, 0.0, 2.0);
         assert0UlpEquals(0.0, qq.abs());
 
         qq = new MkQuat(1.0, 2.0, 0.0, 2.0);

-        assertUlpEquals(3.0, qq.abs(), 0);
+        assert0UlpEquals(3.0, qq.abs());

 
         qq = new MkQuat(2.0, 2.0, 2.0, 2.0);
 
         qq = new MkQuat(2.0, 2.0, 2.0, 2.0);

-        assertUlpEquals(4.0, qq.abs(), 0);
+        assert0UlpEquals(4.0, qq.abs());

 
         return;
     }
 
         return;
     }


@@ -348,6 +376,8 @@ public strictfp class MkQuatTest {
         assertUlpEquals(2.0, eu.getYRot(), 2);
         assertUlpEquals(3.0, eu.getZRot(), 0);
 
         assertUlpEquals(2.0, eu.getYRot(), 2);
         assertUlpEquals(3.0, eu.getZRot(), 0);
 

+        // ジンバルロック
+

         qq.setEulerYXZ(RAD_90DEG, 0.0, 0.0);
         qq.toEulerYXZ(eu, 0.0);
         assert0UlpEquals(RAD_90DEG, eu.getXRot());
         qq.setEulerYXZ(RAD_90DEG, 0.0, 0.0);
         qq.toEulerYXZ(eu, 0.0);
         assert0UlpEquals(RAD_90DEG, eu.getXRot());


@@ -365,6 +395,17 @@ public strictfp class MkQuatTest {
         assert0UlpEquals(RAD_15DEG, eu.getYRot());
         assertUlpEquals(-RAD_15DEG, eu.getZRot(), 2);
 
         assert0UlpEquals(RAD_15DEG, eu.getYRot());
         assertUlpEquals(-RAD_15DEG, eu.getZRot(), 2);
 

+        qq.setEulerYXZ(-RAD_90DEG, RAD_30DEG, 0.0);
+        qq.toEulerYXZ(eu, 0.0);
+        assert0UlpEquals(-RAD_90DEG, eu.getXRot());
+        assert0UlpEquals(0.0, eu.getYRot());
+        assertUlpEquals(RAD_30DEG, eu.getZRot(), 1);
+
+        qq.toEulerYXZ(eu, RAD_15DEG);
+        assert0UlpEquals(-RAD_90DEG, eu.getXRot());
+        assert0UlpEquals(RAD_15DEG, eu.getYRot());
+        assertUlpEquals(RAD_15DEG, eu.getZRot(), 2);
+

         qq.setEulerYXZ(RAD_90DEG, RAD_45DEG, 0.0);
         qq.toEulerYXZ(eu, 0.0);
         assert0UlpEquals(RAD_90DEG, eu.getXRot());
         qq.setEulerYXZ(RAD_90DEG, RAD_45DEG, 0.0);
         qq.toEulerYXZ(eu, 0.0);
         assert0UlpEquals(RAD_90DEG, eu.getXRot());


@@ -373,7 +414,7 @@ public strictfp class MkQuatTest {
 
         qq.setEulerYXZ(RAD_90DEG, RAD_45DEG, RAD_45DEG);
         qq.toEulerYXZ(eu, 0.0);
 
         qq.setEulerYXZ(RAD_90DEG, RAD_45DEG, RAD_45DEG);
         qq.toEulerYXZ(eu, 0.0);

-        assertEquals(RAD_90DEG, eu.getXRot(), 1E-7);
+        assert0UlpEquals(RAD_90DEG, eu.getXRot());

         assert0UlpEquals(0.0, eu.getYRot());
         assertEquals(0.0, eu.getZRot(), EPSILON);
 
         assert0UlpEquals(0.0, eu.getYRot());
         assertEquals(0.0, eu.getZRot(), EPSILON);
 


@@ -389,6 +430,66 @@ public strictfp class MkQuatTest {
         assertUlpEquals(RAD_45DEG, eu.getYRot(), 1);
         assertUlpEquals(RAD_45DEG, eu.getZRot(), 1);
 
         assertUlpEquals(RAD_45DEG, eu.getYRot(), 1);
         assertUlpEquals(RAD_45DEG, eu.getZRot(), 1);
 

+        double xRad;
+        double yRad;
+        double zRad;
+
+        xRad = StrictMath.toRadians(95);
+        yRad = StrictMath.toRadians(0);
+        zRad = StrictMath.toRadians(0);
+        qq.setEulerYXZ(xRad, yRad, zRad);
+        qq.toEulerYXZ(eu, yRad);
+        assertUlpEquals(StrictMath.toRadians(85), eu.getXRot(), 1);
+        assertUlpEquals(StrictMath.toRadians(180), eu.getYRot(), 0);
+        assertUlpEquals(StrictMath.toRadians(180), eu.getZRot(), 0);
+
+        return;
+    }
+
+    /**
+     * Test of critical case, of class MkQuat.
+     */
+    @Test
+    public void testCritical() {
+        System.out.println("critical");
+
+        EulerYXZ eu;
+        MkQuat qq;
+        double xRad;
+        double yRad;
+        double zRad;
+
+        qq = new MkQuat();
+        eu = new EulerYXZ();
+
+        xRad = StrictMath.toRadians(89);
+        yRad = StrictMath.toRadians(80);
+        zRad = StrictMath.toRadians(41);
+        qq.setEulerYXZ(xRad, yRad, zRad);
+        qq.toEulerYXZ(eu, 0.0);
+        assertUlpEquals(xRad, eu.getXRot(), 164);
+        assertUlpEquals(yRad, eu.getYRot(), 143);
+        assertUlpEquals(zRad, eu.getZRot(), 211);
+
+        // ジンバルロック判定境界ケース
+        xRad = StrictMath.toRadians(90);
+        yRad = StrictMath.toRadians(6);
+        zRad = StrictMath.toRadians(7);
+        qq.setEulerYXZ(xRad, yRad, zRad);
+        qq.toEulerYXZ(eu, yRad);
+        assert0UlpEquals(xRad, eu.getXRot());
+        assert0UlpEquals(yRad, eu.getYRot());
+        assert0UlpEquals(zRad, eu.getZRot());
+
+        xRad = StrictMath.toRadians(89.999);
+        yRad = StrictMath.toRadians(89.999);
+        zRad = StrictMath.toRadians(89.999);
+        qq.setEulerYXZ(xRad, yRad, zRad);
+        qq.toEulerYXZ(eu, yRad);
+        assertUlpEquals(xRad, eu.getXRot(), 83029);
+        assertUlpEquals(yRad, eu.getYRot(), 80108);
+        assertUlpEquals(zRad, eu.getZRot(), 80108);
+

         return;
     }
 
         return;
     }
 


@@ -436,6 +537,14 @@ public strictfp class MkQuatTest {
         pos = new MkPos3D(1.0, 1.0, 1.0);
         result = new MkPos3D();
 
         pos = new MkPos3D(1.0, 1.0, 1.0);
         result = new MkPos3D();
 

+        // No Rotation
+        qq.setEulerYXZ(0.0, 0.0, 0.0);
+        qq.rotatePos(pos, result);
+        assert0UlpEquals(1.0, result.getXpos());
+        assert0UlpEquals(1.0, result.getYpos());
+        assert0UlpEquals(1.0, result.getZpos());
+
+        // test Left Hand Rotation

         qq.setEulerYXZ(0.0, RAD_90DEG, 0.0);
         qq.rotatePos(pos, result);
         assertUlpEquals(1.0, result.getXpos(), 0);
         qq.setEulerYXZ(0.0, RAD_90DEG, 0.0);
         qq.rotatePos(pos, result);
         assertUlpEquals(1.0, result.getXpos(), 0);


@@ -454,6 +563,8 @@ public strictfp class MkQuatTest {
         assertUlpEquals(1.0, result.getYpos(), 0);
         assertUlpEquals(1.0, result.getZpos(), 0);
 
         assertUlpEquals(1.0, result.getYpos(), 0);
         assertUlpEquals(1.0, result.getZpos(), 0);
 

+        // test Euler axis order
+

         qq.setEulerYXZ(RAD_90DEG, RAD_90DEG, 0.0);
         qq.rotatePos(pos, result);
         assertUlpEquals(1.0, result.getXpos(), 2);
         qq.setEulerYXZ(RAD_90DEG, RAD_90DEG, 0.0);
         qq.rotatePos(pos, result);
         assertUlpEquals(1.0, result.getXpos(), 2);


@@ -492,6 +603,84 @@ public strictfp class MkQuatTest {
         assertUlpEquals(1.0, result.getYpos(), 2);
         assertUlpEquals(3.0, result.getZpos(), 0);
 
         assertUlpEquals(1.0, result.getYpos(), 2);
         assertUlpEquals(3.0, result.getZpos(), 0);
 

+        // rotate origin point
+
+        pos = new MkPos3D(0.0, 0.0, 0.0);
+        qq.setEulerYXZ(RAD_15DEG, RAD_30DEG, RAD_45DEG);
+        qq.rotatePos(pos, result);
+        assert0UlpEquals(0.0, result.getXpos());
+        assert0UlpEquals(0.0, result.getYpos());
+        assert0UlpEquals(0.0, result.getZpos());
+
+        return;
+    }
+
+    /**
+     * Test of full matching, of class MkQuat.
+     */
+//    @Test
+    public void testFullMatch(){
+        System.out.println("full match");
+
+        for(int ix = -5; ix <= 90; ix++){
+            for(int iy = -5; iy <= 179; iy++){
+                for(int iz = -5; iz <= 179; iz++){
+                    try{
+                        rotTest(ix, iy, iz);
+                    }catch(AssertionError e){
+                        System.out.println(""+ix+":"+iy+":"+iz);
+                        throw e;
+                    }
+                }
+            }
+        }
+
+        return;
+    }
+
+    /**
+     * Test of full matching, of class MkQuat.
+     */
+//    @Test
+    public void testFullMatchRev(){
+        System.out.println("full match rev");
+
+        for(int ix = -90; ix <= 5; ix++){
+            for(int iy = -179; iy <= 5; iy++){
+                for(int iz = -179; iz <= 5; iz++){
+                    try{
+                        rotTest(ix, iy, iz);
+                    }catch(AssertionError e){
+                        System.out.println(""+ix+":"+iy+":"+iz);
+                        throw e;
+                    }
+                }
+            }
+        }
+
+        return;
+    }
+
+    private void rotTest(double ix, double iy, double iz){
+        EulerYXZ result;
+        MkQuat qq;
+
+        qq = new MkQuat();
+        result = new EulerYXZ();
+
+        double xRad = StrictMath.toRadians(ix);
+        double yRad = StrictMath.toRadians(iy);
+        double zRad = StrictMath.toRadians(iz);
+
+        qq.setEulerYXZ(xRad, yRad, zRad);
+        qq.toEulerYXZ(result, yRad);
+
+        final double DELTA = EPSILON * 164;
+
+        assertEquals(xRad, result.getXRot(), DELTA);
+        assertEquals(yRad, result.getYRot(), DELTA);
+        assertEquals(zRad, result.getZRot(), DELTA);
+

         return;
     }
 
         return;
     }