4 * License : The MIT License
5 * Copyright(c) 2011 MikuToga Partners
8 package jp.sfjp.mikutoga.math;
12 * <p>虚部q1,q2,q3と実部qwから構成される。
14 public strictfp class MkQuat {
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;
23 for(int ct=1; ct<=STEP_BELOW; ct++){
24 one = StrictMath.nextAfter(one, 0.0);
28 assert BELOWONE < 1.0;
40 * <p>虚部が全て0.0、実部が1.0となる。
43 this(0.0, 0.0, 0.0, 1.0);
49 * @param q コピー元クォータニオン
51 public MkQuat(MkQuat q){
52 this(q.q1, q.q2, q.q3, q.qw);
63 public MkQuat(double q1, double q2, double q3, double qw){
75 * <p>クォータニオン積では交換則が成り立たない。
76 * <p>引数は同一インスタンスを含んでもよい。
81 public static void mul(MkQuat qA,
84 double aq1 = qA.getQ1();
85 double aq2 = qA.getQ2();
86 double aq3 = qA.getQ3();
87 double aqw = qA.getQW();
89 double bq1 = qB.getQ1();
90 double bq2 = qB.getQ2();
91 double bq3 = qB.getQ3();
92 double bqw = qB.getQW();
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;
113 * 共役(共軛)クォータニオンを求め格納する。
114 * <p>引数は同一インスタンスでもよい。
118 public static void conjugate(MkQuat q, MkQuat result){
128 * <p>引数は同一インスタンスでもよい。
132 public static void normalize(MkQuat q, MkQuat result){
133 double abs = q.abs();
135 double nq1 = q.q1 / abs;
136 double nq2 = q.q2 / abs;
137 double nq3 = q.q3 / abs;
138 double nqw = q.qw / abs;
150 * <p>対象クォータニオンの絶対値が小さい場合、
152 * <p>引数は同一インスタンスでもよい。
156 public static void inverse(MkQuat q, MkQuat result){
163 double nq1 = -q.q1 / sum;
164 double nq2 = -q.q2 / sum;
165 double nq3 = -q.q3 / sum;
166 double nqw = q.qw / sum;
181 public double getQ1() {
189 public double getQ2() {
197 public double getQ3() {
205 public double getQW() {
213 public void setQ1(double q1Arg) {
222 public void setQ2(double q2Arg) {
231 public void setQ3(double q3Arg) {
240 public void setQW(double wArg) {
252 public void setQ123W(double q1Arg, double q2Arg, double q3Arg,
263 * @return クォータニオンの絶対値
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);
277 * <p>虚部q1,q2,q3にX,Y,Z軸の変量が入る。
283 public void setPos3D(double xPos, double yPos, double zPos){
293 * <p>虚部q1,q2,q3にX,Y,Z軸の変量が入る。
297 public void setPos3D(MkPos3D pos){
298 setPos3D(pos.getXpos(), pos.getYpos(), pos.getZpos());
304 * <p>Y軸回転、X軸回転、Z軸回転の順に
305 * 個別回転クォータニオンの積をとったものと等しい。
306 * @param xRot X軸回転量(ラジアン)。第2軸
307 * @param yRot Y軸回転量(ラジアン)。第1軸
308 * @param zRot Z軸回転量(ラジアン)。第3軸
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;
315 double chx = StrictMath.cos(hx);
316 double chy = StrictMath.cos(hy);
317 double chz = StrictMath.cos(hz);
319 double shx = StrictMath.sin(hx);
320 double shy = StrictMath.sin(hy);
321 double shz = StrictMath.sin(hz);
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;
333 * <p>Y軸回転、X軸回転、Z軸回転の順に
334 * 個別回転クォータニオンの積をとったものと等しい。
335 * @param rot YXZオイラー角
337 public void setEulerYXZ(EulerYXZ rot){
338 setEulerYXZ(rot.getXRot(), rot.getYRot(), rot.getZRot());
343 * クォータニオンをYXZオイラー角へと変換する。
344 * <p>ジンバルロック時のYZ配分が指定可能。
345 * @param result YXZオイラー角
346 * @param oldY ジンバルロック時(オイラー角Xが直角etc.)
349 public void toEulerYXZ(EulerYXZ result, double oldY){
353 double qqw = this.qw;
355 double qx2 = qx * qx;
356 double qy2 = qy * qy;
357 double qz2 = qz * qz;
359 double qwx = qqw * qx;
360 double qwy = qqw * qy;
361 double qwz = qqw * qz;
363 double qxy = qx * qy;
364 double qxz = qx * qz;
365 double qyz = qy * qz;
367 double m00 = 1.0 - 2.0 * (qy2 + qz2);
368 double m01 = 2.0 * (qxy - qwz);
369 double m02 = 2.0 * (qwy + qxz);
371 double m10 = 2.0 * (qxy + qwz);
372 double m11 = 1.0 - 2.0 * (qx2 + qz2);
373 double m12 = 2.0 * (qyz - qwx);
375 // double m20 = 2.0 * (qxz - qwy);
376 // double m21 = 2.0 * (qwx + qyz);
377 double m22 = 1.0 - 2.0 * (qx2 + qy2);
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 ){
390 if(StrictMath.abs(m12) >= BELOWONE){
391 resultX = -StrictMath.copySign(HALF_PI, m12);
395 resultZ = StrictMath.atan2(-m01, m00);
396 if(resultX >= 0.0) resultZ += resultY;
397 else resultZ -= resultY;
399 if(StrictMath.abs(resultZ) > StrictMath.PI){
400 resultZ -= StrictMath.copySign(DBL_PI, resultZ);
403 resultX = StrictMath.asin(-m12);
404 resultY = StrictMath.atan2(m02, m22);
405 resultZ = StrictMath.atan2(m10, m11);
408 result.setXRot(resultX);
409 result.setYRot(resultY);
410 result.setZRot(resultZ);
416 * クォータニオンをYXZオイラー角へと変換する。
417 * @param result YXZオイラー角
419 public void toEulerYXZ(EulerYXZ result){
420 toEulerYXZ(result, 0.0);
425 * 回転クォータニオンを用いて点座標を回転させる。
430 public void rotatePos(MkPos3D pos, MkPos3D result){
432 double rQ1 = this.q1;
433 double rQ2 = this.q2;
434 double rQ3 = this.q3;
435 double rQW = this.qw;
438 double pQ1 = pos.getXpos();
439 double pQ2 = pos.getYpos();
440 double pQ3 = pos.getZpos();
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;
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;
471 // assert rprrQW == 0.0;
473 result.setXpos(rprrQ1);
474 result.setYpos(rprrQ2);
475 result.setZpos(rprrQ3);
482 * @return {@inheritDoc}
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();