OSDN Git Service

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