Fix createRotation() and avoid unnecessary trigonometrics roundtrip

gitlab #68

Change-Id: Ib5acbb46055fe52f75059092ef47ce0d37d69891
(cherry picked from commit 1f602513cbb75e138df359a3e21f8115c98e48a1)

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 b9941c8b94a774605ed91fd55752150fe3aab88d..1b44cc4542321ffc11f2921d5fd481420df3ee05 100644 (file)
--- 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) {