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