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 EPSILON = StrictMath.ulp(1.0);
18 private static final double TDELTA = EPSILON * 4;
21 assert StrictMath.ulp(StrictMath.PI) <= TDELTA;
33 * <p>虚部が全て0.0、実部が1.0となる。
36 this(0.0, 0.0, 0.0, 1.0);
42 * @param q コピー元クォータニオン
44 public MkQuat(MkQuat q){
45 this(q.q1, q.q2, q.q3, q.qw);
56 public MkQuat(double q1, double q2, double q3, double qw){
68 * <p>クォータニオン積では交換則が成り立たない。
69 * <p>引数は同一インスタンスを含んでもよい。
74 public static void mul(MkQuat qA,
77 double aq1 = qA.getQ1();
78 double aq2 = qA.getQ2();
79 double aq3 = qA.getQ3();
80 double aqw = qA.getQW();
82 double bq1 = qB.getQ1();
83 double bq2 = qB.getQ2();
84 double bq3 = qB.getQ3();
85 double bqw = qB.getQW();
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;
106 * 共役(共軛)クォータニオンを求め格納する。
107 * <p>引数は同一インスタンスでもよい。
111 public static void conjugate(MkQuat q, MkQuat result){
121 * <p>引数は同一インスタンスでもよい。
125 public static void normalize(MkQuat q, MkQuat result){
126 double abs = q.abs();
128 double nq1 = q.q1 / abs;
129 double nq2 = q.q2 / abs;
130 double nq3 = q.q3 / abs;
131 double nqw = q.qw / abs;
143 * <p>対象クォータニオンの絶対値が小さい場合、
145 * <p>引数は同一インスタンスでもよい。
149 public static void inverse(MkQuat q, MkQuat result){
156 double nq1 = -q.q1 / sum;
157 double nq2 = -q.q2 / sum;
158 double nq3 = -q.q3 / sum;
159 double nqw = q.qw / sum;
174 public double getQ1() {
182 public double getQ2() {
190 public double getQ3() {
198 public double getQW() {
206 public void setQ1(double q1Arg) {
215 public void setQ2(double q2Arg) {
224 public void setQ3(double q3Arg) {
233 public void setQW(double wArg) {
245 public void setQ123W(double q1Arg, double q2Arg, double q3Arg,
256 * @return クォータニオンの絶対値
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);
270 * <p>虚部q1,q2,q3にX,Y,Z軸の変量が入る。
276 public void setPos3D(double xPos, double yPos, double zPos){
286 * <p>虚部q1,q2,q3にX,Y,Z軸の変量が入る。
290 public void setPos3D(MkPos3D pos){
291 setPos3D(pos.getXpos(), pos.getYpos(), pos.getZpos());
297 * <p>Y軸回転、X軸回転、Z軸回転の順に
298 * 個別回転クォータニオンの積をとったものと等しい。
299 * @param xRot X軸回転量(ラジアン)。第2軸
300 * @param yRot Y軸回転量(ラジアン)。第1軸
301 * @param zRot Z軸回転量(ラジアン)。第3軸
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;
308 double chx = StrictMath.cos(hx);
309 double chy = StrictMath.cos(hy);
310 double chz = StrictMath.cos(hz);
312 double shx = StrictMath.sin(hx);
313 double shy = StrictMath.sin(hy);
314 double shz = StrictMath.sin(hz);
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;
326 * <p>Y軸回転、X軸回転、Z軸回転の順に
327 * 個別回転クォータニオンの積をとったものと等しい。
328 * @param rot YXZオイラー角
330 public void setEulerYXZ(EulerYXZ rot){
331 setEulerYXZ(rot.getXRot(), rot.getYRot(), rot.getZRot());
336 * クォータニオンをYXZオイラー角へと変換する。
337 * <p>ジンバルロック時のYZ配分が指定可能。
338 * @param result YXZオイラー角
339 * @param oldY ジンバルロック時(オイラー角Xが直角etc.)
342 public void toEulerYXZ(EulerYXZ result, double oldY){
346 double qqw = this.qw;
348 double qx2 = qx * qx;
349 double qy2 = qy * qy;
350 double qz2 = qz * qz;
352 double qwx = qqw * qx;
353 double qwy = qqw * qy;
354 double qwz = qqw * qz;
356 double qxy = qx * qy;
357 double qxz = qx * qz;
358 double qyz = qy * qz;
360 double m00 = 1.0 - 2.0 * (qy2 + qz2);
361 double m01 = 2.0 * (qxy - qwz);
362 double m02 = 2.0 * (qwy + qxz);
364 double m10 = 2.0 * (qxy + qwz);
365 double m11 = 1.0 - 2.0 * (qx2 + qz2);
366 double m12 = 2.0 * (qyz - qwx);
368 // double m20 = 2.0 * (qxz - qwy);
369 // double m21 = 2.0 * (qwx + qyz);
370 double m22 = 1.0 - 2.0 * (qx2 + qy2);
376 if (m12 < -1.0) resultX = +HALF_PI;
377 else if(m12 > +1.0) resultX = -HALF_PI;
378 else resultX = StrictMath.asin(-m12);
380 if( StrictMath.abs(m11) <= TDELTA // Y,Zが一意に定まらない場合
381 || StrictMath.abs(m22) <= TDELTA ){
383 resultZ = StrictMath.atan2(-m01, m00) + oldY;
385 resultY = StrictMath.atan2(m02, m22);
386 resultZ = StrictMath.atan2(m10, m11);
389 result.setXRot(resultX);
390 result.setYRot(resultY);
391 result.setZRot(resultZ);
397 * クォータニオンをYXZオイラー角へと変換する。
398 * @param result YXZオイラー角
400 public void toEulerYXZ(EulerYXZ result){
401 toEulerYXZ(result, 0.0);
406 * 回転クォータニオンを用いて点座標を回転させる。
411 public void rotatePos(MkPos3D pos, MkPos3D result){
413 double rQ1 = this.q1;
414 double rQ2 = this.q2;
415 double rQ3 = this.q3;
416 double rQW = this.qw;
419 double pQ1 = pos.getXpos();
420 double pQ2 = pos.getYpos();
421 double pQ3 = pos.getZpos();
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;
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;
452 // assert rprrQW == 0.0;
454 result.setXpos(rprrQ1);
455 result.setYpos(rprrQ2);
456 result.setZpos(rprrQ3);
463 * @return {@inheritDoc}
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();