From 970cf58da828909d95eaaaec3f3956fa97455315 Mon Sep 17 00:00:00 2001 From: Olyutorskii Date: Thu, 28 Mar 2013 19:41:10 +0900 Subject: [PATCH] =?utf8?q?=E3=82=B8=E3=83=B3=E3=83=90=E3=83=AB=E3=83=AD?= =?utf8?q?=E3=83=83=E3=82=AF=E5=88=A4=E5=AE=9A=E3=82=92=E6=94=B9=E5=96=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit --- src/main/java/jp/sfjp/mikutoga/math/MkQuat.java | 37 +++- .../java/jp/sfjp/mikutoga/math/MkQuatTest.java | 221 +++++++++++++++++++-- 2 files changed, 233 insertions(+), 25 deletions(-) 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 --- 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; - 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{ - 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; - 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 ){ + */ + // Y,Zが一意に定まらない場合 + if(StrictMath.abs(m12) >= BELOWONE){ + resultX = -StrictMath.copySign(HALF_PI, m12); + 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{ + resultX = StrictMath.asin(-m12); 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 --- 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() { } - private void assert0UlpEquals(double expected, double x){ - assertUlpEquals(expected, x, 0); + private void assert0UlpEquals(double expected, double result){ + assertUlpEquals(expected, result, 0); 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; - 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; } @@ -145,6 +156,23 @@ public strictfp class MkQuatTest { 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; } @@ -189,10 +217,10 @@ public strictfp class MkQuatTest { 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; } @@ -217,10 +245,10 @@ public strictfp class MkQuatTest { 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; } @@ -241,10 +269,10 @@ public strictfp class MkQuatTest { 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); - assertUlpEquals(4.0, qq.abs(), 0); + assert0UlpEquals(4.0, qq.abs()); return; } @@ -348,6 +376,8 @@ public strictfp class MkQuatTest { 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()); @@ -365,6 +395,17 @@ public strictfp class MkQuatTest { 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()); @@ -373,7 +414,7 @@ public strictfp class MkQuatTest { 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); @@ -389,6 +430,66 @@ public strictfp class MkQuatTest { 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; } @@ -436,6 +537,14 @@ public strictfp class MkQuatTest { 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); @@ -454,6 +563,8 @@ public strictfp class MkQuatTest { 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); @@ -492,6 +603,84 @@ public strictfp class MkQuatTest { 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; } -- 2.11.0