src/main/java/jp/sfjp/mikutoga/math/MkQuat.java

   1 /*
   2  * quaternion rotation
   3  *
   4  * License : The MIT License
   5  * Copyright(c) 2011 MikuToga Partners
   6  */
   7
   8 package jp.sfjp.mikutoga.math;
   9
  10 /**
  11  * クォータニオンによる回転表現。
  12  * <p>虚部q1,q2,q3と実部qwから構成される。
  13  */
  14 public strictfp class MkQuat {
  15
  16     private static final double HALF_PI = StrictMath.PI / 2.0;
  17     private static final double DBL_PI = StrictMath.PI * 2.0;
  18     private static final int STEP_BELOW = 5;
  19     private static final double BELOWONE;
  20
  21     static{
  22         double one = 1.0;
  23         for(int ct = 1; ct <= STEP_BELOW; ct++){
  24             one = StrictMath.nextAfter(one, 0.0);
  25         }
  26         BELOWONE = one;
  27
  28         assert BELOWONE < 1.0;
  29     }
  30
  31
  32     private double q1;
  33     private double q2;
  34     private double q3;
  35     private double qw;
  36
  37
  38     /**
  39      * コンストラクタ。
  40      * <p>虚部が全て0.0、実部が1.0となる。
  41      */
  42     public MkQuat(){
  43         this(0.0, 0.0, 0.0, 1.0);
  44         return;
  45     }
  46
  47     /**
  48      * コンストラクタ。
  49      * @param q コピー元クォータニオン
  50      */
  51     public MkQuat(MkQuat q){
  52         this(q.q1, q.q2, q.q3, q.qw);
  53         return;
  54     }
  55
  56     /**
  57      * コンストラクタ。
  58      * @param q1 虚部1
  59      * @param q2 虚部2
  60      * @param q3 虚部3
  61      * @param qw 実部
  62      */
  63     public MkQuat(double q1, double q2, double q3, double qw){
  64         super();
  65         this.q1 = q1;
  66         this.q2 = q2;
  67         this.q3 = q3;
  68         this.qw = qw;
  69         return;
  70     }
  71
  72
  73     /**
  74      * クォータニオン積を求め格納する。
  75      * <p>クォータニオン積では交換則が成り立たない。
  76      * <p>引数は同一インスタンスを含んでもよい。
  77      * @param qA 積前項
  78      * @param qB 積後項
  79      * @param result 積の格納先
  80      */
  81     public static void mul(MkQuat qA,
  82                            MkQuat qB,
  83                            MkQuat result ){
  84         double aq1 = qA.getQ1();
  85         double aq2 = qA.getQ2();
  86         double aq3 = qA.getQ3();
  87         double aqw = qA.getQW();
  88
  89         double bq1 = qB.getQ1();
  90         double bq2 = qB.getQ2();
  91         double bq3 = qB.getQ3();
  92         double bqw = qB.getQW();
  93
  94         double rq1;
  95         double rq2;
  96         double rq3;
  97         double rqw;
  98
  99         rq1 = aq2 * bq3 - aq3 * bq2 + aqw * bq1 + aq1 * bqw;
 100         rq2 = aq3 * bq1 - aq1 * bq3 + aqw * bq2 + aq2 * bqw;
 101         rq3 = aq1 * bq2 - aq2 * bq1 + aqw * bq3 + aq3 * bqw;
 102         rqw = aqw * bqw - aq1 * bq1 - aq2 * bq2 - aq3 * bq3;
 103
 104         result.q1 = rq1;
 105         result.q2 = rq2;
 106         result.q3 = rq3;
 107         result.qw = rqw;
 108
 109         return;
 110     }
 111
 112     /**
 113      * 共役(共軛)クォータニオンを求め格納する。
 114      * <p>引数は同一インスタンスでもよい。
 115      * @param q クォータニオン
 116      * @param result 格納先
 117      */
 118     public static void conjugate(MkQuat q, MkQuat result){
 119         result.q1 = -q.q1;
 120         result.q2 = -q.q2;
 121         result.q3 = -q.q3;
 122         result.qw =  q.qw;
 123         return;
 124     }
 125
 126     /**
 127      * 単位クォータニオンを求め格納する。
 128      * <p>引数は同一インスタンスでもよい。
 129      * @param q クォータニオン
 130      * @param result 格納先
 131      */
 132     public static void normalize(MkQuat q, MkQuat result){
 133         double abs = q.abs();
 134
 135         double nq1 = q.q1 / abs;
 136         double nq2 = q.q2 / abs;
 137         double nq3 = q.q3 / abs;
 138         double nqw = q.qw / abs;
 139
 140         result.q1 = nq1;
 141         result.q2 = nq2;
 142         result.q3 = nq3;
 143         result.qw = nqw;
 144
 145         return;
 146     }
 147
 148     /**
 149      * 逆元クォータニオンを求め格納する。
 150      * <p>対象クォータニオンの絶対値が小さい場合、
 151      * 無限大が虚部実部に入る可能性がある。
 152      * <p>引数は同一インスタンスでもよい。
 153      * @param q クォータニオン
 154      * @param result 格納先
 155      */
 156     public static void inverse(MkQuat q, MkQuat result){
 157         double sum = 0.0;
 158         sum += q.q1 * q.q1;
 159         sum += q.q2 * q.q2;
 160         sum += q.q3 * q.q3;
 161         sum += q.qw * q.qw;
 162
 163         double nq1 = -q.q1 / sum;
 164         double nq2 = -q.q2 / sum;
 165         double nq3 = -q.q3 / sum;
 166         double nqw =  q.qw / sum;
 167
 168         result.q1 = nq1;
 169         result.q2 = nq2;
 170         result.q3 = nq3;
 171         result.qw = nqw;
 172
 173         return;
 174     }
 175
 176
 177     /**
 178      * 虚部1を返す。
 179      * @return 虚部1
 180      */
 181     public double getQ1() {
 182         return this.q1;
 183     }
 184
 185     /**
 186      * 虚部2を返す。
 187      * @return 虚部2
 188      */
 189     public double getQ2() {
 190         return this.q2;
 191     }
 192
 193     /**
 194      * 虚部3を返す。
 195      * @return 虚部3
 196      */
 197     public double getQ3() {
 198         return this.q3;
 199     }
 200
 201     /**
 202      * 実部を返す。
 203      * @return 実部
 204      */
 205     public double getQW() {
 206         return this.qw;
 207     }
 208
 209     /**
 210      * 虚部1を設定する。
 211      * @param q1Arg 虚部1
 212      */
 213     public void setQ1(double q1Arg) {
 214         this.q1 = q1Arg;
 215         return;
 216     }
 217
 218     /**
 219      * 虚部2を設定する。
 220      * @param q2Arg 虚部2
 221      */
 222     public void setQ2(double q2Arg) {
 223         this.q2 = q2Arg;
 224         return;
 225     }
 226
 227     /**
 228      * 虚部3を設定する。
 229      * @param q3Arg 虚部3
 230      */
 231     public void setQ3(double q3Arg) {
 232         this.q3 = q3Arg;
 233         return;
 234     }
 235
 236     /**
 237      * 実部を設定する。
 238      * @param wArg 実部
 239      */
 240     public void setQW(double wArg) {
 241         this.qw = wArg;
 242         return;
 243     }
 244
 245     /**
 246      * 虚部実部を設定する。
 247      * @param q1Arg 虚部1
 248      * @param q2Arg 虚部2
 249      * @param q3Arg 虚部3
 250      * @param wArg 実部
 251      */
 252     public void setQ123W(double q1Arg, double q2Arg, double q3Arg,
 253                           double wArg ){
 254         this.q1 = q1Arg;
 255         this.q2 = q2Arg;
 256         this.q3 = q3Arg;
 257         this.qw = wArg;
 258         return;
 259     }
 260
 261     /**
 262      * クォータニオンの絶対値を返す。
 263      * @return クォータニオンの絶対値
 264      */
 265     public double abs(){
 266         double sum = 0.0;
 267         sum += this.q1 * this.q1;
 268         sum += this.q2 * this.q2;
 269         sum += this.q3 * this.q3;
 270         sum += this.qw * this.qw;
 271         double result = StrictMath.sqrt(sum);
 272         return result;
 273     }
 274
 275     /**
 276      * 位置情報を読み込む。
 277      * <p>虚部q1,q2,q3にX,Y,Z軸の変量が入る。
 278      * <p>実部には0が入る。
 279      * @param xPos X位置
 280      * @param yPos Y位置
 281      * @param zPos Z位置
 282      */
 283     public void setPos3D(double xPos, double yPos, double zPos){
 284         this.q1 = xPos;
 285         this.q2 = yPos;
 286         this.q3 = zPos;
 287         this.qw = 0.0;
 288         return;
 289     }
 290
 291     /**
 292      * 位置情報を読み込む。
 293      * <p>虚部q1,q2,q3にX,Y,Z軸の変量が入る。
 294      * <p>実部には0が入る。
 295      * @param pos 位置情報
 296      */
 297     public void setPos3D(MkPos3D pos){
 298         setPos3D(pos.getXpos(), pos.getYpos(), pos.getZpos());
 299         return;
 300     }
 301
 302     /**
 303      * YXZオイラー角を読み込む。
 304      * <p>Y軸回転、X軸回転、Z軸回転の順に
 305      * 個別回転クォータニオンの積をとったものと等しい。
 306      * @param xRot X軸回転量(ラジアン)。第2軸
 307      * @param yRot Y軸回転量(ラジアン)。第1軸
 308      * @param zRot Z軸回転量(ラジアン)。第3軸
 309      */
 310     public void setEulerYXZ(double xRot, double yRot, double zRot){
 311         double hx = xRot / 2.0;
 312         double hy = yRot / 2.0;
 313         double hz = zRot / 2.0;
 314
 315         double chx = StrictMath.cos(hx);
 316         double chy = StrictMath.cos(hy);
 317         double chz = StrictMath.cos(hz);
 318
 319         double shx = StrictMath.sin(hx);
 320         double shy = StrictMath.sin(hy);
 321         double shz = StrictMath.sin(hz);
 322
 323         this.q1 = chy * shx * chz + shy * chx * shz;
 324         this.q2 = shy * chx * chz - chy * shx * shz;
 325         this.q3 = chy * chx * shz - shy * shx * chz;
 326         this.qw = chy * chx * chz + shy * shx * shz;
 327
 328         return;
 329     }
 330
 331     /**
 332      * YXZオイラー角を読み込む。
 333      * <p>Y軸回転、X軸回転、Z軸回転の順に
 334      * 個別回転クォータニオンの積をとったものと等しい。
 335      * @param rot YXZオイラー角
 336      */
 337     public void setEulerYXZ(EulerYXZ rot){
 338         setEulerYXZ(rot.getXRot(), rot.getYRot(), rot.getZRot());
 339         return;
 340     }
 341
 342     /**
 343      * クォータニオンをYXZオイラー角へと変換する。
 344      * <p>ジンバルロック時のYZ配分が指定可能。
 345      * @param result YXZオイラー角
 346      * @param oldY ジンバルロック時(オイラー角Xが直角etc.)
 347      * に使われるY軸回転量
 348      */
 349     public void toEulerYXZ(EulerYXZ result, double oldY){
 350         double qx = this.q1;
 351         double qy = this.q2;
 352         double qz = this.q3;
 353         double qqw = this.qw;
 354
 355         double qx2 = qx * qx;
 356         double qy2 = qy * qy;
 357         double qz2 = qz * qz;
 358
 359         double qwx = qqw * qx;
 360         double qwy = qqw * qy;
 361         double qwz = qqw * qz;
 362
 363         double qxy = qx * qy;
 364         double qxz = qx * qz;
 365         double qyz = qy * qz;
 366
 367         double m00 = 1.0 - 2.0 * (qy2 + qz2);
 368         double m01 = 2.0 * (qxy - qwz);
 369         double m02 = 2.0 * (qwy + qxz);
 370
 371         double m10 = 2.0 * (qxy + qwz);
 372         double m11 = 1.0 - 2.0 * (qx2 + qz2);
 373         double m12 = 2.0 * (qyz - qwx);
 374
 375 //      double m20 = 2.0 * (qxz - qwy);
 376 //      double m21 = 2.0 * (qwx + qyz);
 377         double m22 = 1.0 - 2.0 * (qx2 + qy2);
 378
 379         double resultX;
 380         double resultY;
 381         double resultZ;
 382
 383         /*
 384         private static final double EPSILON = StrictMath.ulp(1.0);
 385         private static final double TDELTA = EPSILON * 4;
 386         if(   StrictMath.abs(m11) <= TDELTA
 387            || StrictMath.abs(m22) <= TDELTA ){
 388         */
 389         // Y,Zが一意に定まらない場合
 390         if(StrictMath.abs(m12) >= BELOWONE){
 391             resultX = -StrictMath.copySign(HALF_PI, m12);
 392
 393             resultY = oldY;
 394
 395             resultZ = StrictMath.atan2(-m01, m00);
 396             if(resultX >= 0.0) resultZ += resultY;
 397             else               resultZ -= resultY;
 398
 399             if(StrictMath.abs(resultZ) > StrictMath.PI){
 400                 resultZ -= StrictMath.copySign(DBL_PI, resultZ);
 401             }
 402         }else{
 403             resultX = StrictMath.asin(-m12);
 404             resultY = StrictMath.atan2(m02, m22);
 405             resultZ = StrictMath.atan2(m10, m11);
 406         }
 407
 408         result.setXRot(resultX);
 409         result.setYRot(resultY);
 410         result.setZRot(resultZ);
 411
 412         return;
 413     }
 414
 415     /**
 416      * クォータニオンをYXZオイラー角へと変換する。
 417      * @param result YXZオイラー角
 418      */
 419     public void toEulerYXZ(EulerYXZ result){
 420         toEulerYXZ(result, 0.0);
 421         return;
 422     }
 423
 424     /**
 425      * 回転クォータニオンを用いて点座標を回転させる。
 426      * 座標インスタンスは同一でもよい。
 427      * @param pos 点座標
 428      * @param result 格納先
 429      */
 430     public void rotatePos(MkPos3D pos, MkPos3D result){
 431         // 回転クォータニオンr (Q)
 432         double rQ1 = this.q1;
 433         double rQ2 = this.q2;
 434         double rQ3 = this.q3;
 435         double rQW = this.qw;
 436
 437         // 点座標p (P)
 438         double pQ1 = pos.getXpos();
 439         double pQ2 = pos.getYpos();
 440         double pQ3 = pos.getZpos();
 441         double pQW = 0.0;
 442
 443         // 共役クォータニオンrr (Q')
 444         double rrQ1 = -rQ1;
 445         double rrQ2 = -rQ2;
 446         double rrQ3 = -rQ3;
 447         double rrQW =  rQW;
 448
 449         // QP
 450         double rpQ1;
 451         double rpQ2;
 452         double rpQ3;
 453         double rpQW;
 454
 455         rpQ1 = rQ2 * pQ3 - rQ3 * pQ2 + rQW * pQ1 + rQ1 * pQW;
 456         rpQ2 = rQ3 * pQ1 - rQ1 * pQ3 + rQW * pQ2 + rQ2 * pQW;
 457         rpQ3 = rQ1 * pQ2 - rQ2 * pQ1 + rQW * pQ3 + rQ3 * pQW;
 458         rpQW = rQW * pQW - rQ1 * pQ1 - rQ2 * pQ2 - rQ3 * pQ3;
 459
 460         // QPQ'
 461         double rprrQ1;
 462         double rprrQ2;
 463         double rprrQ3;
 464 //      double rprrQW;
 465
 466         rprrQ1 = rpQ2 * rrQ3 - rpQ3 * rrQ2 + rpQW * rrQ1 + rpQ1 * rrQW;
 467         rprrQ2 = rpQ3 * rrQ1 - rpQ1 * rrQ3 + rpQW * rrQ2 + rpQ2 * rrQW;
 468         rprrQ3 = rpQ1 * rrQ2 - rpQ2 * rrQ1 + rpQW * rrQ3 + rpQ3 * rrQW;
 469 //      rprrQW = rpQW * rrQW - rpQ1 * rrQ1 - rpQ2 * rrQ2 - rpQ3 * rrQ3;
 470
 471 //      assert rprrQW == 0.0;
 472
 473         result.setXpos(rprrQ1);
 474         result.setYpos(rprrQ2);
 475         result.setZpos(rprrQ3);
 476
 477         return;
 478     }
 479
 480     /**
 481      * {@inheritDoc}
 482      * @return {@inheritDoc}
 483      */
 484     @Override
 485     public String toString(){
 486         StringBuilder result = new StringBuilder();
 487         result.append("q1=") .append(this.q1);
 488         result.append(" q2=").append(this.q2);
 489         result.append(" q3=").append(this.q3);
 490         result.append(" w=") .append(this.qw);
 491         return  result.toString();
 492     }
 493
 494 }