4 * License : The MIT License
5 * Copyright(c) 2011 MikuToga Partners
8 package jp.sourceforge.mikutoga.math;
12 * <p>虚部q1,q2,q3と実部qwから構成される。
14 public strictfp class MkQuat {
16 private static final double HALF_PI = StrictMath.PI / 2.0;
27 * <p>虚部が全て0.0、実部が1.0となる。
30 this(0.0, 0.0, 0.0, 1.0);
36 * @param q コピー元クォータニオン
38 public MkQuat(MkQuat q){
39 this(q.q1, q.q2, q.q3, q.qw);
50 public MkQuat(double q1, double q2, double q3, double qw){
62 * <p>クォータニオン積では交換則が成り立たない。
63 * <p>引数は同一インスタンスを含んでもよい。
68 public static void mul(MkQuat qA,
71 double aq1 = qA.getQ1();
72 double aq2 = qA.getQ2();
73 double aq3 = qA.getQ3();
74 double aqw = qA.getQW();
76 double bq1 = qB.getQ1();
77 double bq2 = qB.getQ2();
78 double bq3 = qB.getQ3();
79 double bqw = qB.getQW();
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;
100 * 共役(共軛)クォータニオンを求め格納する。
101 * <p>引数は同一インスタンスでもよい。
105 public static void conjugate(MkQuat q, MkQuat result){
115 * <p>引数は同一インスタンスでもよい。
119 public static void normalize(MkQuat q, MkQuat result){
120 double abs = q.abs();
122 double nq1 = q.q1 / abs;
123 double nq2 = q.q2 / abs;
124 double nq3 = q.q3 / abs;
125 double nqw = q.qw / abs;
137 * <p>対象クォータニオンの絶対値が小さい場合、無限大が虚部実部に入る可能性がある。
138 * <p>引数は同一インスタンスでもよい。
142 public static void inverse(MkQuat q, MkQuat result){
149 double nq1 = -q.q1 / sum;
150 double nq2 = -q.q2 / sum;
151 double nq3 = -q.q3 / sum;
152 double nqw = q.qw / sum;
167 public double getQ1() {
175 public double getQ2() {
183 public double getQ3() {
191 public double getQW() {
199 public void setQ1(double q1) {
208 public void setQ2(double q2) {
217 public void setQ3(double q3) {
226 public void setQW(double w) {
233 * @return クォータニオンの絶対値
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);
247 * <p>虚部q1,q2,q3にX,Y,Z軸の変量が入る。
253 public void readPos3D(double xPos, double yPos, double zPos){
263 * <p>虚部q1,q2,q3にX,Y,Z軸の変量が入る。
267 public void readPos3D(MkPos3D pos){
268 readPos3D(pos.getXpos(), pos.getYpos(), pos.getZpos());
274 * <p>Y軸回転、X軸回転、Z軸回転の順に個別回転クォータニオンの積をとったものと等しい。
275 * @param xRot X軸回転量(ラジアン)。第2軸
276 * @param yRot Y軸回転量(ラジアン)。第1軸
277 * @param zRot Z軸回転量(ラジアン)。第3軸
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;
284 double chx = StrictMath.cos(hx);
285 double chy = StrictMath.cos(hy);
286 double chz = StrictMath.cos(hz);
288 double shx = StrictMath.sin(hx);
289 double shy = StrictMath.sin(hy);
290 double shz = StrictMath.sin(hz);
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;
302 * <p>Y軸回転、X軸回転、Z軸回転の順に個別回転クォータニオンの積をとったものと等しい。
303 * @param rot YXZオイラー角
305 public void readEulerYXZ(EulerYXZ rot){
306 readEulerYXZ(rot.getXRot(), rot.getYRot(), rot.getZRot());
311 * クォータニオンをYXZオイラー角へと変換する。
312 * <p>ジンバルロック時のYZ配分が指定可能。
313 * @param result YXZオイラー角
314 * @param oldY ジンバルロック時に使われるY軸回転量
316 public void toEulerYXZ(EulerYXZ result, double oldY){
320 double qqw = this.qw;
322 double qx2 = qx * qx;
323 double qy2 = qy * qy;
324 double qz2 = qz * qz;
326 double qwx = qqw * qx;
327 double qwy = qqw * qy;
328 double qwz = qqw * qz;
330 double qxy = qx * qy;
331 double qxz = qx * qz;
332 double qyz = qy * qz;
334 double m00 = 1.0 - 2.0 * (qy2 + qz2);
335 double m01 = 2.0 * (qxy - qwz);
336 double m02 = 2.0 * (qwy + qxz);
338 double m10 = 2.0 * (qxy + qwz);
339 double m11 = 1.0 - 2.0 * (qx2 + qz2);
340 double m12 = 2.0 * (qyz - qwx);
342 // double m20 = 2.0 * (qxz - qwy);
343 // double m21 = 2.0 * (qwx + qyz);
344 double m22 = 1.0 - 2.0 * (qx2 + qy2);
350 if (m12 < -1.0) resultX = +HALF_PI;
351 else if(m12 > +1.0) resultX = -HALF_PI;
352 else resultX = StrictMath.asin(-m12);
354 if(m11 == 0.0 || m22 == 0.0){ // Y,Zが一意に定まらない場合
356 resultZ = StrictMath.atan2(-m01, m00) + oldY;
358 resultY = StrictMath.atan2(m02, m22);
359 resultZ = StrictMath.atan2(m10, m11);
362 result.setXRot(resultX);
363 result.setYRot(resultY);
364 result.setZRot(resultZ);
370 * クォータニオンをYXZオイラー角へと変換する。
371 * @param result YXZオイラー角
373 public void toEulerYXZ(EulerYXZ result){
374 toEulerYXZ(result, 0.0);
379 * 回転クォータニオンを用いて点座標を回転させる。
384 public void rotatePos(MkPos3D pos, MkPos3D result){
386 double rQ1 = this.q1;
387 double rQ2 = this.q2;
388 double rQ3 = this.q3;
389 double rQW = this.qw;
392 double pQ1 = pos.getXpos();
393 double pQ2 = pos.getYpos();
394 double pQ3 = pos.getZpos();
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;
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;
425 // assert rprrQW == 0.0;
427 result.setXpos(rprrQ1);
428 result.setYpos(rprrQ2);
429 result.setZpos(rprrQ3);
436 * @return {@inheritDoc}
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();