X-Git-Url: https://gerrit.simantics.org/r/gitweb?a=blobdiff_plain;ds=sidebyside;f=org.simantics.g3d%2Fsrc%2Forg%2Fsimantics%2Fg3d%2Fmath%2FEulerTools.java;fp=org.simantics.g3d%2Fsrc%2Forg%2Fsimantics%2Fg3d%2Fmath%2FEulerTools.java;h=4545ccb3030fa94950969ad1bd2ae5a20f819c23;hb=87b3241ec277ba3d8e414b26186a032c9cdcaeed;hp=0000000000000000000000000000000000000000;hpb=1f0bcd66274375f2278d2e6c486cb28257a5f7b2;p=simantics%2F3d.git

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