From 1f602513cbb75e138df359a3e21f8115c98e48a1 Mon Sep 17 00:00:00 2001
From: Reino Ruusu <reino.ruusu@semantum.fi>
Date: Tue, 10 Dec 2019 20:30:55 +0200
Subject: [PATCH] Fix createRotation() and avoid unnecessary trigonometrics
 roundtrip

gitlab #68

Change-Id: Ib5acbb46055fe52f75059092ef47ce0d37d69891
---
 .../src/org/simantics/g3d/math/MathTools.java | 64 +++++++++++++------
 1 file changed, 44 insertions(+), 20 deletions(-)

diff --git a/org.simantics.g3d/src/org/simantics/g3d/math/MathTools.java b/org.simantics.g3d/src/org/simantics/g3d/math/MathTools.java
index b9941c8b..1b44cc45 100644
--- a/org.simantics.g3d/src/org/simantics/g3d/math/MathTools.java
+++ b/org.simantics.g3d/src/org/simantics/g3d/math/MathTools.java
@@ -146,17 +146,28 @@ public class MathTools {
 	public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, float d) {
 		return (planeNormal.dot(point) + d);
 	}
-	
+
 	public static Vector3d projectToPlane(Vector3d v, Vector3d planeNormal) {
-	    //v.normalize();
-	    //planeNormal.normalize();
-	    Vector3d t = new Vector3d();
-	    t.cross(v,planeNormal);
-	    t.cross(planeNormal, t);
-	    return t;
-	    
+		//v.normalize();
+		//planeNormal.normalize();
+		Vector3d t = new Vector3d();
+		if (planeNormal == X_AXIS) {
+			t.set(0, v.y, v.z);
+		}
+		else if (planeNormal == Y_AXIS) {
+			t.set(v.x, 0, v.z);
+		}
+		else if (planeNormal == Z_AXIS) {
+			t.set(v.x, v.y, 0);
+		}
+		else {
+			t.cross(v,planeNormal);
+			t.cross(planeNormal, t);
+		}
+		
+		return t;
 	}
-	
+
 	public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Tuple3d intersectPoint) {
 		intersectPoint.set(planePoint);
 		intersectPoint.sub(linePoint);
@@ -769,9 +780,8 @@ public class MathTools {
 			result.z = 0.0;
 		} else if (d < -0.9999) {
 			// original and rotated are parallel, pointing at the opposite direction 
-			Vector3d a = Z_AXIS;
-			if (Math.abs(a.dot(original)) > 0.8 )
-				a = Y_AXIS;
+			Vector3d a = projectToPlane(getMinAxis(original), original);
+			a.normalize();
 			result.set(a, Math.PI);
 		} else {
 			double angle = original.angle(rotated);
@@ -796,20 +806,34 @@ public class MathTools {
 			result.y = 0.0;
 			result.z = 0.0;
 		} else if (d < -0.9999) {
-			// original and rotated are parallel, pointing at the opposite direction 
-			Vector3d a = Z_AXIS;
-			if (Math.abs(a.dot(original)) > 0.8 )
-				a = Y_AXIS;
-			getQuat(a, Math.PI, result);
-			
+			// original and rotated are parallel, pointing at the opposite direction
+			Vector3d a = projectToPlane(getMinAxis(original), original);
+			a.normalize();
+			result.set(a.getX(), a.getY(), a.getZ(), 0.0);
 		} else {
-			double angle = original.angle(rotated);
+			// Cosine and sine of half angle
+			double cosHalf = Math.sqrt((1 + d)/2);
+			double sinHalf = Math.sqrt((1 - d)/2);
 			Vector3d axis = new Vector3d();
 			axis.cross(original, rotated);
-			getQuat(axis, angle, result);
+			axis.normalize();
+			result.set(sinHalf * axis.getX(), sinHalf * axis.getY(), sinHalf * axis.getZ(), cosHalf);
 		}
 		return true;
 	}
+
+	public static Vector3d getMinAxis(Vector3d original) {
+		double absX = Math.abs(original.getX());
+		double absY = Math.abs(original.getY());
+		double absZ = Math.abs(original.getZ());
+		
+		if (absX <= absY && absX <= absZ)
+			return X_AXIS;
+		else if (absY <= absZ)
+			return Y_AXIS;
+		else
+			return Z_AXIS;
+	}
 	
 	public static boolean createRotation(Vector3d original, Vector3d rotated, Vector3d axis, AxisAngle4d result) {
         
-- 
2.45.2