1 package org.simantics.g3d.math;
\r
3 import javax.vecmath.AxisAngle4d;
\r
4 import javax.vecmath.Quat4d;
\r
5 import javax.vecmath.Vector3d;
\r
7 public class EulerTools {
\r
10 XYX, XYZ, XZX, XZY, YXY, YXZ, YZX, YZY, ZXY, ZXZ, ZYX, ZYZ
\r
13 public static Quat4d getQuatFromEuler(Order order, Vector3d a) {
\r
14 return getQuatFromEuler(order, a.x, a.y, a.z);
\r
17 public static Quat4d getQuatFromEuler(Order order, double a1, double a2, double a3) {
\r
18 Quat4d q1 = new Quat4d();
\r
19 Quat4d q2 = new Quat4d();
\r
20 Quat4d q3 = new Quat4d();
\r
23 q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));
\r
24 q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));
\r
25 q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));
\r
28 q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));
\r
29 q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));
\r
30 q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));
\r
33 q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));
\r
34 q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));
\r
35 q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));
\r
38 q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));
\r
39 q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));
\r
40 q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));
\r
43 q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));
\r
44 q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));
\r
45 q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));
\r
48 q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));
\r
49 q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));
\r
50 q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));
\r
53 q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));
\r
54 q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));
\r
55 q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));
\r
58 q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));
\r
59 q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));
\r
60 q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));
\r
63 q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));
\r
64 q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));
\r
65 q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));
\r
68 q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));
\r
69 q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));
\r
70 q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));
\r
73 q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));
\r
74 q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));
\r
75 q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));
\r
78 q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));
\r
79 q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));
\r
80 q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));
\r
89 * See http://noelhughes.net/uploads/quat_2_euler_paper_ver3.pdf
\r
94 public static Vector3d getEulerFromQuat(Order order, Quat4d q) {
\r
95 Vector3d euler = new Vector3d();
\r
97 // Vector3d v1 = new Vector3d();
\r
98 // Vector3d v2 = new Vector3d();
\r
99 Vector3d v3 = new Vector3d();
\r
100 Vector3d v3n = new Vector3d();
\r
176 Vector3d v3r = new Vector3d();
\r
177 MathTools.rotate(q, v3, v3r);
\r
183 euler.x = Math.atan2(v3r.z, v3r.y);
\r
184 euler.y = Math.acos(v3r.x);
\r
187 euler.x = Math.atan2(v3r.x, v3r.z);
\r
188 euler.y = Math.acos(v3r.y);
\r
191 euler.x = Math.atan2(v3r.y, v3r.x);
\r
192 euler.y = Math.acos(v3r.z);
\r
196 euler.x = Math.atan2(v3r.z, v3r.y);
\r
197 euler.y = -Math.asin(v3r.x);
\r
200 euler.x = Math.atan2(v3r.x, v3r.z);
\r
201 euler.y = -Math.asin(v3r.y);
\r
204 euler.x = Math.atan2(v3r.y, v3r.x);
\r
205 euler.y = -Math.asin(v3r.z);
\r
209 euler.x = Math.atan2(v3r.y, -v3r.z);
\r
210 //euler.x = Math.atan2(v3r.y, -v3r.x);
\r
211 euler.y = Math.acos(v3r.x);
\r
214 euler.x = Math.atan2(v3r.z, -v3r.x);
\r
215 //euler.x = Math.atan2(v3r.z, -v3r.y);
\r
216 euler.y = Math.acos(v3r.y);
\r
219 euler.x = Math.atan2(v3r.x, -v3r.y);
\r
220 //euler.x = Math.atan2(v3r.x, -v3r.z);
\r
221 euler.y = Math.acos(v3r.z);
\r
225 euler.x = Math.atan2(-v3r.y, v3r.z);
\r
226 euler.y = Math.asin(v3r.x);
\r
229 euler.x = Math.atan2(-v3r.z, v3r.x);
\r
230 euler.y = Math.asin(v3r.y);
\r
233 euler.x = Math.atan2(-v3r.x, v3r.y);
\r
234 euler.y = Math.asin(v3r.z);
\r
238 Quat4d q1 = new Quat4d();
\r
239 q1.w = Math.cos(euler.x*0.5);
\r
240 Quat4d q2 = new Quat4d();
\r
241 q2.w = Math.cos(euler.y*0.5);
\r
248 q1.x = Math.sin(euler.x*0.5);
\r
254 q1.y = Math.sin(euler.x*0.5);
\r
260 q1.z = Math.sin(euler.x*0.5);
\r
269 q2.x = Math.sin(euler.y*0.5);
\r
275 q2.y = Math.sin(euler.y*0.5);
\r
281 q2.z = Math.sin(euler.y*0.5);
\r
285 Quat4d q12 = new Quat4d();
\r
288 Vector3d v3n12 = new Vector3d();
\r
289 Vector3d v3ng = new Vector3d();
\r
290 MathTools.rotate(q12, v3n, v3n12);
\r
291 MathTools.rotate(q, v3n, v3ng);
\r
293 double dot = v3n12.dot(v3ng);
\r
294 dot = MathTools.clamp(-1.0, 1.0, dot);
\r
295 euler.z = Math.abs(Math.acos(dot));
\r
296 Vector3d vc = new Vector3d();
\r
297 vc.cross(v3n12, v3ng);
\r
298 euler.z *= Math.signum(vc.dot(v3r));
\r
304 public static void main(String args[]) {
\r
306 boolean all = false;
\r
307 boolean allOrder = false;
\r
310 } else if (allOrder) {
\r
313 //test(Order.ZXY,30,60,45);
\r
314 //test(Order.YXZ,30,0,0);
\r
315 //test(Order.YXZ,30,90,60);
\r
316 test(Order.YXZ,300,240,360);
\r
320 private static void testAll() {
\r
321 double start = 0.0;
\r
323 double step = 30.0;
\r
324 for (double a1 = start; a1 <= end; a1+= step) {
\r
325 double r1 = MathTools.degToRad(a1);
\r
326 for (double a2 = start; a2 <= end; a2+= step) {
\r
327 double r2 = MathTools.degToRad(a2);
\r
328 for (double a3 = start; a3 <= end; a3+= step) {
\r
329 double r3 = MathTools.degToRad(a3);
\r
330 for (Order order : Order.values()) {
\r
331 Quat4d q = EulerTools.getQuatFromEuler(order, r1, r2, r3);
\r
332 Vector3d a = EulerTools.getEulerFromQuat(order, q);
\r
333 Quat4d q2 = EulerTools.getQuatFromEuler(order, a.x,a.y,a.z);
\r
334 a.x = MathTools.radToDeg(a.x);
\r
335 a.y = MathTools.radToDeg(a.y);
\r
336 a.z = MathTools.radToDeg(a.z);
\r
337 System.out.println(toString(a1) +" " + toString(a2) + " " + toString(a3) + " " + order + "\t" + toString(a) + "\t" + toString(q) + "\t" + toString(q2));
\r
344 private static void test(Order order) {
\r
345 double start = 0.0;
\r
346 double end = 360.0;
\r
347 double step = 30.0;
\r
348 for (double a1 = start; a1 <= end; a1+= step) {
\r
349 double r1 = MathTools.degToRad(a1);
\r
350 for (double a2 = start; a2 <= end; a2+= step) {
\r
351 double r2 = MathTools.degToRad(a2);
\r
352 for (double a3 = start; a3 <= end; a3+= step) {
\r
353 double r3 = MathTools.degToRad(a3);
\r
355 Quat4d q = EulerTools.getQuatFromEuler(order, r1, r2, r3);
\r
356 Vector3d a = EulerTools.getEulerFromQuat(order, q);
\r
357 Quat4d q2 = EulerTools.getQuatFromEuler(order, a.x,a.y,a.z);
\r
358 a.x = MathTools.radToDeg(a.x);
\r
359 a.y = MathTools.radToDeg(a.y);
\r
360 a.z = MathTools.radToDeg(a.z);
\r
362 System.out.println(toString(a1) +" " + toString(a2) + " " + toString(a3) + " " + order + "\t" + toString(a) + "\t" + toString(q) + "\t" + toString(q2));
\r
368 private static String toString(double d) {
\r
369 return String.format("%1$6.2f", d);
\r
372 private static String toString(Vector3d v) {
\r
373 return "("+toString(v.x) +", "+ toString(v.y) + ", " + toString(v.z) +")";
\r
376 private static String toString(Quat4d v) {
\r
377 return "("+toString(v.x) +", "+ toString(v.y) + ", " + toString(v.z) + ", " + toString(v.w) +")";
\r
380 private static void test(Order order, double deg1, double deg2, double deg3) {
\r
381 double r1 = MathTools.degToRad(deg1);
\r
382 double r2 = MathTools.degToRad(deg2);
\r
383 double r3 = MathTools.degToRad(deg3);
\r
385 Quat4d q = EulerTools.getQuatFromEuler(order, r1, r2, r3);
\r
386 Vector3d a = EulerTools.getEulerFromQuat(order, q);
\r
387 Quat4d q2 = EulerTools.getQuatFromEuler(order, a.x,a.y,a.z);
\r
388 a.x = MathTools.radToDeg(a.x);
\r
389 a.y = MathTools.radToDeg(a.y);
\r
390 a.z = MathTools.radToDeg(a.z);
\r
391 System.out.println(toString(deg1) +" " + toString(deg2) + " " + toString(deg3) + " " + order + "\t" + toString(a) + "\t" + toString(q) + "\t" + toString(q2));
\r