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