1 /*******************************************************************************
\r
2 * Copyright (c) 2012, 2013 Association for Decentralized Information Management in
\r
4 * All rights reserved. This program and the accompanying materials
\r
5 * are made available under the terms of the Eclipse Public License v1.0
\r
6 * which accompanies this distribution, and is available at
\r
7 * http://www.eclipse.org/legal/epl-v10.html
\r
10 * VTT Technical Research Centre of Finland - initial API and implementation
\r
11 *******************************************************************************/
\r
12 package org.simantics.g3d.math;
\r
14 import javax.vecmath.AxisAngle4d;
\r
15 import javax.vecmath.Quat4d;
\r
16 import javax.vecmath.Vector3d;
\r
18 public class EulerTools {
\r
21 XYX, XYZ, XZX, XZY, YXY, YXZ, YZX, YZY, ZXY, ZXZ, ZYX, ZYZ
\r
24 public static Quat4d getQuatFromEuler(Order order, Vector3d a) {
\r
25 return getQuatFromEuler(order, a.x, a.y, a.z);
\r
28 public static Quat4d getQuatFromEuler(Order order, double a1, double a2, double a3) {
\r
29 Quat4d q1 = new Quat4d();
\r
30 Quat4d q2 = new Quat4d();
\r
31 Quat4d q3 = new Quat4d();
\r
34 q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));
\r
35 q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));
\r
36 q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));
\r
39 q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));
\r
40 q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));
\r
41 q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));
\r
44 q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));
\r
45 q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));
\r
46 q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));
\r
49 q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));
\r
50 q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));
\r
51 q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));
\r
54 q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));
\r
55 q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));
\r
56 q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));
\r
59 q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));
\r
60 q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));
\r
61 q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));
\r
64 q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));
\r
65 q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));
\r
66 q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));
\r
69 q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));
\r
70 q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));
\r
71 q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));
\r
74 q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));
\r
75 q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));
\r
76 q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));
\r
79 q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));
\r
80 q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));
\r
81 q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));
\r
84 q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));
\r
85 q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));
\r
86 q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));
\r
89 q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));
\r
90 q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));
\r
91 q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));
\r
100 * See http://noelhughes.net/uploads/quat_2_euler_paper_ver3.pdf
\r
105 public static Vector3d getEulerFromQuat(Order order, Quat4d q) {
\r
106 Vector3d euler = new Vector3d();
\r
108 // Vector3d v1 = new Vector3d();
\r
109 // Vector3d v2 = new Vector3d();
\r
110 Vector3d v3 = new Vector3d();
\r
111 Vector3d v3n = new Vector3d();
\r
187 Vector3d v3r = new Vector3d();
\r
188 MathTools.rotate(q, v3, v3r);
\r
194 euler.x = Math.atan2(v3r.z, v3r.y);
\r
195 euler.y = Math.acos(v3r.x);
\r
198 euler.x = Math.atan2(v3r.x, v3r.z);
\r
199 euler.y = Math.acos(v3r.y);
\r
202 euler.x = Math.atan2(v3r.y, v3r.x);
\r
203 euler.y = Math.acos(v3r.z);
\r
207 euler.x = Math.atan2(v3r.z, v3r.y);
\r
208 euler.y = -Math.asin(v3r.x);
\r
211 euler.x = Math.atan2(v3r.x, v3r.z);
\r
212 euler.y = -Math.asin(v3r.y);
\r
215 euler.x = Math.atan2(v3r.y, v3r.x);
\r
216 euler.y = -Math.asin(v3r.z);
\r
220 euler.x = Math.atan2(v3r.y, -v3r.z);
\r
221 //euler.x = Math.atan2(v3r.y, -v3r.x);
\r
222 euler.y = Math.acos(v3r.x);
\r
225 euler.x = Math.atan2(v3r.z, -v3r.x);
\r
226 //euler.x = Math.atan2(v3r.z, -v3r.y);
\r
227 euler.y = Math.acos(v3r.y);
\r
230 euler.x = Math.atan2(v3r.x, -v3r.y);
\r
231 //euler.x = Math.atan2(v3r.x, -v3r.z);
\r
232 euler.y = Math.acos(v3r.z);
\r
236 euler.x = Math.atan2(-v3r.y, v3r.z);
\r
237 euler.y = Math.asin(v3r.x);
\r
240 euler.x = Math.atan2(-v3r.z, v3r.x);
\r
241 euler.y = Math.asin(v3r.y);
\r
244 euler.x = Math.atan2(-v3r.x, v3r.y);
\r
245 euler.y = Math.asin(v3r.z);
\r
249 Quat4d q1 = new Quat4d();
\r
250 q1.w = Math.cos(euler.x*0.5);
\r
251 Quat4d q2 = new Quat4d();
\r
252 q2.w = Math.cos(euler.y*0.5);
\r
259 q1.x = Math.sin(euler.x*0.5);
\r
265 q1.y = Math.sin(euler.x*0.5);
\r
271 q1.z = Math.sin(euler.x*0.5);
\r
280 q2.x = Math.sin(euler.y*0.5);
\r
286 q2.y = Math.sin(euler.y*0.5);
\r
292 q2.z = Math.sin(euler.y*0.5);
\r
296 Quat4d q12 = new Quat4d();
\r
299 Vector3d v3n12 = new Vector3d();
\r
300 Vector3d v3ng = new Vector3d();
\r
301 MathTools.rotate(q12, v3n, v3n12);
\r
302 MathTools.rotate(q, v3n, v3ng);
\r
304 double dot = v3n12.dot(v3ng);
\r
305 dot = MathTools.clamp(-1.0, 1.0, dot);
\r
306 euler.z = Math.abs(Math.acos(dot));
\r
307 Vector3d vc = new Vector3d();
\r
308 vc.cross(v3n12, v3ng);
\r
309 euler.z *= Math.signum(vc.dot(v3r));
\r
315 public static void main(String args[]) {
\r
317 boolean all = false;
\r
318 boolean allOrder = false;
\r
321 } else if (allOrder) {
\r
324 //test(Order.ZXY,30,60,45);
\r
325 //test(Order.YXZ,30,0,0);
\r
326 //test(Order.YXZ,30,90,60);
\r
327 test(Order.YXZ,300,240,360);
\r
331 private static void testAll() {
\r
332 double start = 0.0;
\r
334 double step = 30.0;
\r
335 for (double a1 = start; a1 <= end; a1+= step) {
\r
336 double r1 = MathTools.degToRad(a1);
\r
337 for (double a2 = start; a2 <= end; a2+= step) {
\r
338 double r2 = MathTools.degToRad(a2);
\r
339 for (double a3 = start; a3 <= end; a3+= step) {
\r
340 double r3 = MathTools.degToRad(a3);
\r
341 for (Order order : Order.values()) {
\r
342 Quat4d q = EulerTools.getQuatFromEuler(order, r1, r2, r3);
\r
343 Vector3d a = EulerTools.getEulerFromQuat(order, q);
\r
344 Quat4d q2 = EulerTools.getQuatFromEuler(order, a.x,a.y,a.z);
\r
345 a.x = MathTools.radToDeg(a.x);
\r
346 a.y = MathTools.radToDeg(a.y);
\r
347 a.z = MathTools.radToDeg(a.z);
\r
348 System.out.println(toString(a1) +" " + toString(a2) + " " + toString(a3) + " " + order + "\t" + toString(a) + "\t" + toString(q) + "\t" + toString(q2));
\r
355 private static void test(Order order) {
\r
356 double start = 0.0;
\r
357 double end = 360.0;
\r
358 double step = 30.0;
\r
359 for (double a1 = start; a1 <= end; a1+= step) {
\r
360 double r1 = MathTools.degToRad(a1);
\r
361 for (double a2 = start; a2 <= end; a2+= step) {
\r
362 double r2 = MathTools.degToRad(a2);
\r
363 for (double a3 = start; a3 <= end; a3+= step) {
\r
364 double r3 = MathTools.degToRad(a3);
\r
366 Quat4d q = EulerTools.getQuatFromEuler(order, r1, r2, r3);
\r
367 Vector3d a = EulerTools.getEulerFromQuat(order, q);
\r
368 Quat4d q2 = EulerTools.getQuatFromEuler(order, a.x,a.y,a.z);
\r
369 a.x = MathTools.radToDeg(a.x);
\r
370 a.y = MathTools.radToDeg(a.y);
\r
371 a.z = MathTools.radToDeg(a.z);
\r
373 System.out.println(toString(a1) +" " + toString(a2) + " " + toString(a3) + " " + order + "\t" + toString(a) + "\t" + toString(q) + "\t" + toString(q2));
\r
379 private static String toString(double d) {
\r
380 return String.format("%1$6.2f", d);
\r
383 private static String toString(Vector3d v) {
\r
384 return "("+toString(v.x) +", "+ toString(v.y) + ", " + toString(v.z) +")";
\r
387 private static String toString(Quat4d v) {
\r
388 return "("+toString(v.x) +", "+ toString(v.y) + ", " + toString(v.z) + ", " + toString(v.w) +")";
\r
391 private static void test(Order order, double deg1, double deg2, double deg3) {
\r
392 double r1 = MathTools.degToRad(deg1);
\r
393 double r2 = MathTools.degToRad(deg2);
\r
394 double r3 = MathTools.degToRad(deg3);
\r
396 Quat4d q = EulerTools.getQuatFromEuler(order, r1, r2, r3);
\r
397 Vector3d a = EulerTools.getEulerFromQuat(order, q);
\r
398 Quat4d q2 = EulerTools.getQuatFromEuler(order, a.x,a.y,a.z);
\r
399 a.x = MathTools.radToDeg(a.x);
\r
400 a.y = MathTools.radToDeg(a.y);
\r
401 a.z = MathTools.radToDeg(a.z);
\r
402 System.out.println(toString(deg1) +" " + toString(deg2) + " " + toString(deg3) + " " + order + "\t" + toString(a) + "\t" + toString(q) + "\t" + toString(q2));
\r