X-Git-Url: http://git.osdn.net/view?p=mikutoga%2FTogaGem.git;a=blobdiff_plain;f=src%2Fmain%2Fjava%2Fjp%2Fsfjp%2Fmikutoga%2Fmath%2FMkQuat.java;h=974732c9354019440a6e375b6ad57ce3e29a83bd;hp=ba6e6496e3dc29d2177f82d85cbc467525ce7ad3;hb=970cf58da828909d95eaaaec3f3956fa97455315;hpb=2afbea9380c472bb23e76a29bd366cad1cb0465c

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);
         }