--- /dev/null
+package org.simantics.g3d.math;\r
+\r
+import javax.vecmath.AxisAngle4d;\r
+import javax.vecmath.Quat4d;\r
+import javax.vecmath.Vector3d;\r
+\r
+public class EulerTools {\r
+\r
+ public enum Order {\r
+ XYX, XYZ, XZX, XZY, YXY, YXZ, YZX, YZY, ZXY, ZXZ, ZYX, ZYZ\r
+ };\r
+\r
+ public static Quat4d getQuatFromEuler(Order order, Vector3d a) {\r
+ return getQuatFromEuler(order, a.x, a.y, a.z);\r
+ }\r
+ \r
+ public static Quat4d getQuatFromEuler(Order order, double a1, double a2, double a3) {\r
+ Quat4d q1 = new Quat4d();\r
+ Quat4d q2 = new Quat4d();\r
+ Quat4d q3 = new Quat4d();\r
+ switch (order) {\r
+ case XYX:\r
+ q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));\r
+ q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));\r
+ q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));\r
+ break;\r
+ case XYZ:\r
+ q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));\r
+ q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));\r
+ q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));\r
+ break;\r
+ case XZX:\r
+ q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));\r
+ q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));\r
+ q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));\r
+ break;\r
+ case XZY:\r
+ q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));\r
+ q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));\r
+ q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));\r
+ break;\r
+ case YXY:\r
+ q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));\r
+ q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));\r
+ q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));\r
+ break;\r
+ case YXZ:\r
+ q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));\r
+ q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));\r
+ q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));\r
+ break;\r
+ case YZX:\r
+ q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));\r
+ q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));\r
+ q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));\r
+ break;\r
+ case YZY:\r
+ q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));\r
+ q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));\r
+ q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));\r
+ break;\r
+ case ZXY:\r
+ q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));\r
+ q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));\r
+ q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));\r
+ break;\r
+ case ZXZ:\r
+ q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));\r
+ q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));\r
+ q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));\r
+ break;\r
+ case ZYX:\r
+ q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));\r
+ q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));\r
+ q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));\r
+ break;\r
+ case ZYZ:\r
+ q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));\r
+ q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));\r
+ q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));\r
+ break;\r
+ }\r
+ q1.mul(q2);\r
+ q1.mul(q3);\r
+ return q1;\r
+ }\r
+ \r
+ /**\r
+ * See http://noelhughes.net/uploads/quat_2_euler_paper_ver3.pdf\r
+ * @param order\r
+ * @param q\r
+ * @return\r
+ */\r
+ public static Vector3d getEulerFromQuat(Order order, Quat4d q) {\r
+ Vector3d euler = new Vector3d();\r
+ \r
+// Vector3d v1 = new Vector3d();\r
+// Vector3d v2 = new Vector3d();\r
+ Vector3d v3 = new Vector3d();\r
+ Vector3d v3n = new Vector3d();\r
+ \r
+ switch (order) {\r
+ case XYX:\r
+// v1.x = 1.0;\r
+// v2.y = 1.0;\r
+ v3.x = 1.0;\r
+ v3n.y = 1.0;\r
+ break;\r
+ case XYZ:\r
+// v1.x = 1.0;\r
+// v2.y = 1.0;\r
+ v3.z = 1.0;\r
+ v3n.x = 1.0;\r
+ break;\r
+ case XZX:\r
+// v1.x = 1.0;\r
+// v2.z = 1.0;\r
+ v3.x = 1.0;\r
+ v3n.y = 1.0;\r
+ break;\r
+ case XZY:\r
+// v1.x = 1.0;\r
+// v2.z = 1.0;\r
+ v3.y = 1.0;\r
+ v3n.z = 1.0;\r
+ break;\r
+ case YXY:\r
+// v1.y = 1.0;\r
+// v2.x = 1.0;\r
+ v3.y = 1.0;\r
+ v3n.z = 1.0;\r
+ break;\r
+ case YXZ:\r
+// v1.y = 1.0;\r
+// v2.x = 1.0;\r
+ v3.z = 1.0;\r
+ v3n.x = 1.0;\r
+ break;\r
+ case YZX:\r
+// v1.y = 1.0;\r
+// v2.z = 1.0;\r
+ v3.x = 1.0;\r
+ v3n.y = 1.0;\r
+ break;\r
+ case YZY:\r
+// v1.y = 1.0;\r
+// v2.z = 1.0;\r
+ v3.y = 1.0;\r
+ v3n.z = 1.0;\r
+ break;\r
+ case ZXY:\r
+// v1.z = 1.0;\r
+// v2.x = 1.0;\r
+ v3.y = 1.0;\r
+ v3n.z = 1.0;\r
+ break;\r
+ case ZXZ:\r
+// v1.z = 1.0;\r
+// v2.x = 1.0;\r
+ v3.z = 1.0;\r
+ v3n.x = 1.0;\r
+ break;\r
+ case ZYX:\r
+// v1.z = 1.0;\r
+// v2.y = 1.0;\r
+ v3.x = 1.0;\r
+ v3n.y = 1.0;\r
+ break;\r
+ case ZYZ:\r
+// v1.z = 1.0;\r
+// v2.y = 1.0;\r
+ v3.z = 1.0;\r
+ v3n.x = 1.0;\r
+ break;\r
+ }\r
+ Vector3d v3r = new Vector3d();\r
+ MathTools.rotate(q, v3, v3r);\r
+ v3r.normalize();\r
+ \r
+ switch (order) {\r
+\r
+ case XZX:\r
+ euler.x = Math.atan2(v3r.z, v3r.y);\r
+ euler.y = Math.acos(v3r.x);\r
+ break;\r
+ case YXY:\r
+ euler.x = Math.atan2(v3r.x, v3r.z);\r
+ euler.y = Math.acos(v3r.y);\r
+ break;\r
+ case ZYZ:\r
+ euler.x = Math.atan2(v3r.y, v3r.x);\r
+ euler.y = Math.acos(v3r.z);\r
+ break;\r
+ \r
+ case XZY:\r
+ euler.x = Math.atan2(v3r.z, v3r.y);\r
+ euler.y = -Math.asin(v3r.x);\r
+ break;\r
+ case YXZ:\r
+ euler.x = Math.atan2(v3r.x, v3r.z);\r
+ euler.y = -Math.asin(v3r.y);\r
+ break;\r
+ case ZYX:\r
+ euler.x = Math.atan2(v3r.y, v3r.x);\r
+ euler.y = -Math.asin(v3r.z);\r
+ break;\r
+ \r
+ case XYX:\r
+ euler.x = Math.atan2(v3r.y, -v3r.z);\r
+ //euler.x = Math.atan2(v3r.y, -v3r.x);\r
+ euler.y = Math.acos(v3r.x);\r
+ break;\r
+ case YZY:\r
+ euler.x = Math.atan2(v3r.z, -v3r.x);\r
+ //euler.x = Math.atan2(v3r.z, -v3r.y);\r
+ euler.y = Math.acos(v3r.y);\r
+ break;\r
+ case ZXZ:\r
+ euler.x = Math.atan2(v3r.x, -v3r.y);\r
+ //euler.x = Math.atan2(v3r.x, -v3r.z);\r
+ euler.y = Math.acos(v3r.z);\r
+ break;\r
+ \r
+ case XYZ:\r
+ euler.x = Math.atan2(-v3r.y, v3r.z);\r
+ euler.y = Math.asin(v3r.x);\r
+ break;\r
+ case YZX:\r
+ euler.x = Math.atan2(-v3r.z, v3r.x);\r
+ euler.y = Math.asin(v3r.y);\r
+ break;\r
+ case ZXY:\r
+ euler.x = Math.atan2(-v3r.x, v3r.y);\r
+ euler.y = Math.asin(v3r.z);\r
+ break;\r
+ }\r
+ \r
+ Quat4d q1 = new Quat4d();\r
+ q1.w = Math.cos(euler.x*0.5);\r
+ Quat4d q2 = new Quat4d();\r
+ q2.w = Math.cos(euler.y*0.5);\r
+ \r
+ switch (order) {\r
+ case XYX:\r
+ case XYZ:\r
+ case XZX:\r
+ case XZY:\r
+ q1.x = Math.sin(euler.x*0.5);\r
+ break;\r
+ case YXY:\r
+ case YXZ:\r
+ case YZX:\r
+ case YZY:\r
+ q1.y = Math.sin(euler.x*0.5);\r
+ break;\r
+ case ZXY:\r
+ case ZXZ:\r
+ case ZYX:\r
+ case ZYZ:\r
+ q1.z = Math.sin(euler.x*0.5);\r
+ break;\r
+ }\r
+ \r
+ switch (order) {\r
+ case YXY:\r
+ case YXZ:\r
+ case ZXY:\r
+ case ZXZ:\r
+ q2.x = Math.sin(euler.y*0.5);\r
+ break;\r
+ case XYX:\r
+ case XYZ:\r
+ case ZYX:\r
+ case ZYZ:\r
+ q2.y = Math.sin(euler.y*0.5);\r
+ break;\r
+ case XZX:\r
+ case XZY:\r
+ case YZX:\r
+ case YZY:\r
+ q2.z = Math.sin(euler.y*0.5);\r
+ break;\r
+ }\r
+ \r
+ Quat4d q12 = new Quat4d();\r
+ q12.mul(q1, q2);\r
+ \r
+ Vector3d v3n12 = new Vector3d();\r
+ Vector3d v3ng = new Vector3d();\r
+ MathTools.rotate(q12, v3n, v3n12);\r
+ MathTools.rotate(q, v3n, v3ng);\r
+ \r
+ double dot = v3n12.dot(v3ng);\r
+ dot = MathTools.clamp(-1.0, 1.0, dot);\r
+ euler.z = Math.abs(Math.acos(dot));\r
+ Vector3d vc = new Vector3d();\r
+ vc.cross(v3n12, v3ng);\r
+ euler.z *= Math.signum(vc.dot(v3r));\r
+ \r
+ return euler;\r
+ }\r
+\r
+ \r
+ public static void main(String args[]) {\r
+ \r
+ boolean all = false;\r
+ boolean allOrder = false;\r
+ if (all) {\r
+ testAll();\r
+ } else if (allOrder) {\r
+ test(Order.YXZ);\r
+ } else {\r
+ //test(Order.ZXY,30,60,45);\r
+ //test(Order.YXZ,30,0,0);\r
+ //test(Order.YXZ,30,90,60);\r
+ test(Order.YXZ,300,240,360);\r
+ }\r
+ }\r
+ \r
+ private static void testAll() {\r
+ double start = 0.0;\r
+ double end = 90.0;\r
+ double step = 30.0;\r
+ for (double a1 = start; a1 <= end; a1+= step) {\r
+ double r1 = MathTools.degToRad(a1);\r
+ for (double a2 = start; a2 <= end; a2+= step) {\r
+ double r2 = MathTools.degToRad(a2);\r
+ for (double a3 = start; a3 <= end; a3+= step) {\r
+ double r3 = MathTools.degToRad(a3);\r
+ for (Order order : Order.values()) {\r
+ Quat4d q = EulerTools.getQuatFromEuler(order, r1, r2, r3);\r
+ Vector3d a = EulerTools.getEulerFromQuat(order, q);\r
+ Quat4d q2 = EulerTools.getQuatFromEuler(order, a.x,a.y,a.z);\r
+ a.x = MathTools.radToDeg(a.x);\r
+ a.y = MathTools.radToDeg(a.y);\r
+ a.z = MathTools.radToDeg(a.z);\r
+ System.out.println(toString(a1) +" " + toString(a2) + " " + toString(a3) + " " + order + "\t" + toString(a) + "\t" + toString(q) + "\t" + toString(q2));\r
+ }\r
+ } \r
+ } \r
+ }\r
+ }\r
+ \r
+ private static void test(Order order) {\r
+ double start = 0.0;\r
+ double end = 360.0;\r
+ double step = 30.0;\r
+ for (double a1 = start; a1 <= end; a1+= step) {\r
+ double r1 = MathTools.degToRad(a1);\r
+ for (double a2 = start; a2 <= end; a2+= step) {\r
+ double r2 = MathTools.degToRad(a2);\r
+ for (double a3 = start; a3 <= end; a3+= step) {\r
+ double r3 = MathTools.degToRad(a3);\r
+ \r
+ Quat4d q = EulerTools.getQuatFromEuler(order, r1, r2, r3);\r
+ Vector3d a = EulerTools.getEulerFromQuat(order, q);\r
+ Quat4d q2 = EulerTools.getQuatFromEuler(order, a.x,a.y,a.z);\r
+ a.x = MathTools.radToDeg(a.x);\r
+ a.y = MathTools.radToDeg(a.y);\r
+ a.z = MathTools.radToDeg(a.z);\r
+ \r
+ System.out.println(toString(a1) +" " + toString(a2) + " " + toString(a3) + " " + order + "\t" + toString(a) + "\t" + toString(q) + "\t" + toString(q2));\r
+ } \r
+ } \r
+ }\r
+ }\r
+ \r
+ private static String toString(double d) {\r
+ return String.format("%1$6.2f", d);\r
+ }\r
+ \r
+ private static String toString(Vector3d v) {\r
+ return "("+toString(v.x) +", "+ toString(v.y) + ", " + toString(v.z) +")";\r
+ }\r
+ \r
+ private static String toString(Quat4d v) {\r
+ return "("+toString(v.x) +", "+ toString(v.y) + ", " + toString(v.z) + ", " + toString(v.w) +")";\r
+ }\r
+ \r
+ private static void test(Order order, double deg1, double deg2, double deg3) {\r
+ double r1 = MathTools.degToRad(deg1);\r
+ double r2 = MathTools.degToRad(deg2);\r
+ double r3 = MathTools.degToRad(deg3);\r
+ \r
+ Quat4d q = EulerTools.getQuatFromEuler(order, r1, r2, r3);\r
+ Vector3d a = EulerTools.getEulerFromQuat(order, q);\r
+ Quat4d q2 = EulerTools.getQuatFromEuler(order, a.x,a.y,a.z);\r
+ a.x = MathTools.radToDeg(a.x);\r
+ a.y = MathTools.radToDeg(a.y);\r
+ a.z = MathTools.radToDeg(a.z);\r
+ System.out.println(toString(deg1) +" " + toString(deg2) + " " + toString(deg3) + " " + order + "\t" + toString(a) + "\t" + toString(q) + "\t" + toString(q2));\r
+\r
+ }\r
+}\r
Code Review / simantics / 3d.git / blobdiff