4 * License : The MIT License
5 * Copyright(c) 2011 MikuToga Partners
8 package jp.sfjp.mikutoga.math;
13 * <p>虚部q1,q2,q3と実部qwから構成される。
15 public strictfp class MkQuat {
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;
24 for(int ct = 1; ct <= STEP_BELOW; ct++){
25 one = StrictMath.nextAfter(one, 0.0);
29 assert BELOWONE < 1.0;
42 * <p>虚部が全て0.0、実部が1.0となる。
45 this(0.0, 0.0, 0.0, 1.0);
52 * @param q コピー元クォータニオン
54 public MkQuat(MkQuat q){
55 this(q.q1, q.q2, q.q3, q.qw);
67 public MkQuat(double q1, double q2, double q3, double qw){
80 * <p>クォータニオン積では交換則が成り立たない。
82 * <p>引数は同一インスタンスを含んでもよい。
88 public static void mul(MkQuat qA,
91 double aq1 = qA.getQ1();
92 double aq2 = qA.getQ2();
93 double aq3 = qA.getQ3();
94 double aqw = qA.getQW();
96 double bq1 = qB.getQ1();
97 double bq2 = qB.getQ2();
98 double bq3 = qB.getQ3();
99 double bqw = qB.getQW();
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;
120 * 共役(共軛)クォータニオンを求め格納する。
122 * <p>引数は同一インスタンスでもよい。
127 public static void conjugate(MkQuat q, MkQuat result){
138 * <p>引数は同一インスタンスでもよい。
143 public static void normalize(MkQuat q, MkQuat result){
144 double abs = q.abs();
146 double nq1 = q.q1 / abs;
147 double nq2 = q.q2 / abs;
148 double nq3 = q.q3 / abs;
149 double nqw = q.qw / abs;
162 * <p>対象クォータニオンの絶対値が小さい場合、
165 * <p>引数は同一インスタンスでもよい。
170 public static void inverse(MkQuat q, MkQuat result){
177 double nq1 = -q.q1 / sum;
178 double nq2 = -q.q2 / sum;
179 double nq3 = -q.q3 / sum;
180 double nqw = q.qw / sum;
195 public double getQ1() {
203 public double getQ2() {
211 public double getQ3() {
219 public double getQW() {
227 public void setQ1(double q1Arg) {
236 public void setQ2(double q2Arg) {
245 public void setQ3(double q3Arg) {
254 public void setQW(double wArg) {
266 public void setQ123W(double q1Arg, double q2Arg, double q3Arg,
277 * @return クォータニオンの絶対値
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);
292 * <p>虚部q1,q2,q3にX,Y,Z軸の変量が入る。
300 public void setPos3D(double xPos, double yPos, double zPos){
311 * <p>虚部q1,q2,q3にX,Y,Z軸の変量が入る。
317 public void setPos3D(MkPos3D pos){
318 setPos3D(pos.getXpos(), pos.getYpos(), pos.getZpos());
325 * <p>Y軸回転、X軸回転、Z軸回転の順に
326 * 個別回転クォータニオンの積をとったものと等しい。
328 * @param xRot X軸回転量(ラジアン)。第2軸
329 * @param yRot Y軸回転量(ラジアン)。第1軸
330 * @param zRot Z軸回転量(ラジアン)。第3軸
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;
337 double chx = StrictMath.cos(hx);
338 double chy = StrictMath.cos(hy);
339 double chz = StrictMath.cos(hz);
341 double shx = StrictMath.sin(hx);
342 double shy = StrictMath.sin(hy);
343 double shz = StrictMath.sin(hz);
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;
356 * <p>Y軸回転、X軸回転、Z軸回転の順に
357 * 個別回転クォータニオンの積をとったものと等しい。
359 * @param rot YXZオイラー角
361 public void setEulerYXZ(EulerYXZ rot){
362 setEulerYXZ(rot.getXRot(), rot.getYRot(), rot.getZRot());
367 * クォータニオンをYXZオイラー角へと変換する。
369 * <p>ジンバルロック時のYZ配分が指定可能。
371 * @param result YXZオイラー角
372 * @param oldY ジンバルロック時(オイラー角Xが直角etc.)
375 public void toEulerYXZ(EulerYXZ result, double oldY){
379 double qqw = this.qw;
381 double qx2 = qx * qx;
382 double qy2 = qy * qy;
383 double qz2 = qz * qz;
385 double qwx = qqw * qx;
386 double qwy = qqw * qy;
387 double qwz = qqw * qz;
389 double qxy = qx * qy;
390 double qxz = qx * qz;
391 double qyz = qy * qz;
393 double m00 = 1.0 - 2.0 * (qy2 + qz2);
394 double m01 = 2.0 * (qxy - qwz);
395 double m02 = 2.0 * (qwy + qxz);
397 double m10 = 2.0 * (qxy + qwz);
398 double m11 = 1.0 - 2.0 * (qx2 + qz2);
399 double m12 = 2.0 * (qyz - qwx);
401 // double m20 = 2.0 * (qxz - qwy);
402 // double m21 = 2.0 * (qwx + qyz);
403 double m22 = 1.0 - 2.0 * (qx2 + qy2);
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 ){
416 if(StrictMath.abs(m12) >= BELOWONE){
417 resultX = -StrictMath.copySign(HALF_PI, m12);
421 resultZ = StrictMath.atan2(-m01, m00);
422 if(resultX >= 0.0) resultZ += resultY;
423 else resultZ -= resultY;
425 if(StrictMath.abs(resultZ) > StrictMath.PI){
426 resultZ -= StrictMath.copySign(DBL_PI, resultZ);
429 resultX = StrictMath.asin(-m12);
430 resultY = StrictMath.atan2(m02, m22);
431 resultZ = StrictMath.atan2(m10, m11);
434 result.setXRot(resultX);
435 result.setYRot(resultY);
436 result.setZRot(resultZ);
442 * クォータニオンをYXZオイラー角へと変換する。
443 * @param result YXZオイラー角
445 public void toEulerYXZ(EulerYXZ result){
446 toEulerYXZ(result, 0.0);
451 * 回転クォータニオンを用いて点座標を回転させる。
456 public void rotatePos(MkPos3D pos, MkPos3D result){
458 double rQ1 = this.q1;
459 double rQ2 = this.q2;
460 double rQ3 = this.q3;
461 double rQW = this.qw;
464 double pQ1 = pos.getXpos();
465 double pQ2 = pos.getYpos();
466 double pQ3 = pos.getZpos();
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;
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;
496 // rprrQW = rpQW * rrQW - rpQ1 * rrQ1 - rpQ2 * rrQ2 - rpQ3 * rrQ3;
497 // assert rprrQW == 0.0;
499 result.setXpos(rprrQ1);
500 result.setYpos(rprrQ2);
501 result.setZpos(rprrQ3);
508 * @return {@inheritDoc}
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();