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