パッケージ変更。テスト整備。

[mikutoga/TogaGem.git] / src / main / java / jp / sfjp / mikutoga / math / MkQuat.java

diff --git a/src/main/java/jp/sourceforge/mikutoga/math/MkQuat.java b/src/main/java/jp/sfjp/mikutoga/math/MkQuat.java
similarity index 84%
rename from src/main/java/jp/sourceforge/mikutoga/math/MkQuat.java
rename to src/main/java/jp/sfjp/mikutoga/math/MkQuat.java
index 533fe20..ba6e649 100644 (file)
--- a/src/main/java/jp/sourceforge/mikutoga/math/MkQuat.java
+++ b/src/main/java/jp/sfjp/mikutoga/math/MkQuat.java
@@ -5,7 +5,7 @@
  * Copyright(c) 2011 MikuToga Partners
  */
 
-package jp.sourceforge.mikutoga.math;
+package jp.sfjp.mikutoga.math;
 
 /**
  * クォータニオンによる回転表現。
@@ -14,6 +14,12 @@ package jp.sourceforge.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;
+
+    static{
+        assert StrictMath.ulp(StrictMath.PI) <= TDELTA;
+    }
 
 
     private double q1;
@@ -134,7 +140,8 @@ public strictfp class MkQuat {
 
     /**
      * 逆元クォータニオンを求め格納する。
-     * <p>対象クォータニオンの絶対値が小さい場合、無限大が虚部実部に入る可能性がある。
+     * <p>対象クォータニオンの絶対値が小さい場合、
+     * 無限大が虚部実部に入る可能性がある。
      * <p>引数は同一インスタンスでもよい。
      * @param q クォータニオン
      * @param result 格納先
@@ -194,37 +201,53 @@ public strictfp class MkQuat {
 
     /**
      * 虚部1を設定する。
-     * @param q1 虚部1
+     * @param q1Arg 虚部1
      */
-    public void setQ1(double q1) {
-        this.q1 = q1;
+    public void setQ1(double q1Arg) {
+        this.q1 = q1Arg;
         return;
     }
 
     /**
      * 虚部2を設定する。
-     * @param q2 虚部2
+     * @param q2Arg 虚部2
      */
-    public void setQ2(double q2) {
-        this.q2 = q2;
+    public void setQ2(double q2Arg) {
+        this.q2 = q2Arg;
         return;
     }
 
     /**
      * 虚部3を設定する。
-     * @param q3 虚部3
+     * @param q3Arg 虚部3
      */
-    public void setQ3(double q3) {
-        this.q3 = q3;
+    public void setQ3(double q3Arg) {
+        this.q3 = q3Arg;
         return;
     }
 
     /**
      * 実部を設定する。
-     * @param w 実部
+     * @param wArg 実部
+     */
+    public void setQW(double wArg) {
+        this.qw = wArg;
+        return;
+    }
+
+    /**
+     * 虚部実部を設定する。
+     * @param q1Arg 虚部1
+     * @param q2Arg 虚部2
+     * @param q3Arg 虚部3
+     * @param wArg 実部
      */
-    public void setQW(double w) {
-        this.qw = w;
+    public void setQ123W(double q1Arg, double q2Arg, double q3Arg,
+                          double wArg ){
+        this.q1 = q1Arg;
+        this.q2 = q2Arg;
+        this.q3 = q3Arg;
+        this.qw = wArg;
         return;
     }
 
@@ -250,7 +273,7 @@ public strictfp class MkQuat {
      * @param yPos Y位置
      * @param zPos Z位置
      */
-    public void readPos3D(double xPos, double yPos, double zPos){
+    public void setPos3D(double xPos, double yPos, double zPos){
         this.q1 = xPos;
         this.q2 = yPos;
         this.q3 = zPos;
@@ -264,19 +287,20 @@ public strictfp class MkQuat {
      * <p>実部には0が入る。
      * @param pos 位置情報
      */
-    public void readPos3D(MkPos3D pos){
-        readPos3D(pos.getXpos(), pos.getYpos(), pos.getZpos());
+    public void setPos3D(MkPos3D pos){
+        setPos3D(pos.getXpos(), pos.getYpos(), pos.getZpos());
         return;
     }
 
     /**
      * YXZオイラー角を読み込む。
-     * <p>Y軸回転、X軸回転、Z軸回転の順に個別回転クォータニオンの積をとったものと等しい。
+     * <p>Y軸回転、X軸回転、Z軸回転の順に
+     * 個別回転クォータニオンの積をとったものと等しい。
      * @param xRot X軸回転量(ラジアン)。第2軸
      * @param yRot Y軸回転量(ラジアン)。第1軸
      * @param zRot Z軸回転量(ラジアン)。第3軸
      */
-    public void readEulerYXZ(double xRot, double yRot, double zRot){
+    public void setEulerYXZ(double xRot, double yRot, double zRot){
         double hx = xRot / 2.0;
         double hy = yRot / 2.0;
         double hz = zRot / 2.0;
@@ -299,11 +323,12 @@ public strictfp class MkQuat {
 
     /**
      * YXZオイラー角を読み込む。
-     * <p>Y軸回転、X軸回転、Z軸回転の順に個別回転クォータニオンの積をとったものと等しい。
+     * <p>Y軸回転、X軸回転、Z軸回転の順に
+     * 個別回転クォータニオンの積をとったものと等しい。
      * @param rot YXZオイラー角
      */
-    public void readEulerYXZ(EulerYXZ rot){
-        readEulerYXZ(rot.getXRot(), rot.getYRot(), rot.getZRot());
+    public void setEulerYXZ(EulerYXZ rot){
+        setEulerYXZ(rot.getXRot(), rot.getYRot(), rot.getZRot());
         return;
     }
 
@@ -311,7 +336,8 @@ public strictfp class MkQuat {
      * クォータニオンをYXZオイラー角へと変換する。
      * <p>ジンバルロック時のYZ配分が指定可能。
      * @param result YXZオイラー角
-     * @param oldY ジンバルロック時に使われるY軸回転量
+     * @param oldY ジンバルロック時(オイラー角Xが直角etc.)
+     * に使われるY軸回転量
      */
     public void toEulerYXZ(EulerYXZ result, double oldY){
         double qx = this.q1;
@@ -351,7 +377,8 @@ public strictfp class MkQuat {
         else if(m12 > +1.0) resultX = -HALF_PI;
         else                resultX = StrictMath.asin(-m12);
 
-        if(m11 == 0.0 || m22 == 0.0){     // Y,Zが一意に定まらない場合
+        if(   StrictMath.abs(m11) <= TDELTA    // Y,Zが一意に定まらない場合
+           || StrictMath.abs(m22) <= TDELTA ){
             resultY = oldY;
             resultZ = StrictMath.atan2(-m01, m00) + oldY;
         }else{