Convert variable angle turn turn to fixed angle

[simantics/3d.git] / org.simantics.g3d / src / org / simantics / g3d / math / MathTools.java

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 0fbd110f3025e96ad7856ef4c17fc5cf40887b15..d761b0ec8350450e8f45829fc8446ffb823bff01 100644 (file)
--- a/org.simantics.g3d/src/org/simantics/g3d/math/MathTools.java
+++ b/org.simantics.g3d/src/org/simantics/g3d/math/MathTools.java
@@ -147,6 +147,16 @@ public class MathTools {
                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;
+           
+       }
+       
        public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Tuple3d intersectPoint) {
                intersectPoint.set(planePoint);
                intersectPoint.sub(linePoint);
@@ -746,60 +756,97 @@ public class MathTools {
        
        public static boolean createRotation(Vector3d original, Vector3d rotated, AxisAngle4d result) {
                
-                       if (rotated.lengthSquared() > 0.01)
-                               rotated.normalize();
-                       else
-                               return false;
-                       double d = original.dot(rotated);
-                       if (d > 0.9999) {
-                               // original and rotated are parallel, pointing at the same direction
-                               result.angle = 0.0;
-                               result.x = 0.0;
-                               result.y = 1.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;
-                               result.set(a, Math.PI);
-                       } else {
-                               double angle = original.angle(rotated);
-                               Vector3d axis = new Vector3d();
-                               axis.cross(original, rotated);
-                               result.set(axis,angle);
-                       }
-                       return true;
+               if (rotated.lengthSquared() > 0.01)
+                       rotated.normalize();
+               else
+                       return false;
+               double d = original.dot(rotated);
+               if (d > 0.9999) {
+                       // original and rotated are parallel, pointing at the same direction
+                       result.angle = 0.0;
+                       result.x = 0.0;
+                       result.y = 1.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;
+                       result.set(a, Math.PI);
+               } else {
+                       double angle = original.angle(rotated);
+                       Vector3d axis = new Vector3d();
+                       axis.cross(original, rotated);
+                       result.set(axis,angle);
                }
+               return true;
+       }
        
        public static boolean createRotation(Vector3d original, Vector3d rotated, Quat4d result) {
                
-                       if (rotated.lengthSquared() > 0.01)
-                               rotated.normalize();
-                       else
-                               return false;
-                       double d = original.dot(rotated);
-                       if (d > 0.9999) {
-                               // original and rotated are parallel, pointing at the same direction
-                               result.w = 1.0;
-                               result.x = 0.0;
-                               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);
-                               
-                       } else {
-                               double angle = original.angle(rotated);
-                               Vector3d axis = new Vector3d();
-                               axis.cross(original, rotated);
-                               getQuat(axis, angle, result);
-                       }
-                       return true;
+               if (rotated.lengthSquared() > 0.01)
+                       rotated.normalize();
+               else
+                       return false;
+               double d = original.dot(rotated);
+               if (d > 0.9999) {
+                       // original and rotated are parallel, pointing at the same direction
+                       result.w = 1.0;
+                       result.x = 0.0;
+                       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);
+                       
+               } else {
+                       double angle = original.angle(rotated);
+                       Vector3d axis = new Vector3d();
+                       axis.cross(original, rotated);
+                       getQuat(axis, angle, result);
                }
+               return true;
+       }
+       
+       public static boolean createRotation(Vector3d original, Vector3d rotated, Vector3d axis, AxisAngle4d result) {
+        
+        if (rotated.lengthSquared() > 0.01)
+            rotated.normalize();
+        else
+            return false;
+        if (original.lengthSquared() > 0.01)
+            original.normalize();
+        else
+            return false;
+        if (axis.lengthSquared() > 0.01)
+            axis.normalize();
+        else
+            return false;
+        double d = original.dot(rotated);
+        if (d > 0.9999) {
+            // original and rotated are parallel, pointing at the same direction
+            result.angle = 0.0;
+            result.x = axis.x;
+            result.y = axis.y;
+            result.z = axis.z;
+        } else if (d < -0.9999) {
+            // original and rotated are parallel, pointing at the opposite direction 
+            result.angle = Math.PI;
+            result.x = axis.x;
+            result.y = axis.y;
+            result.z = axis.z;
+        } else {
+            Vector3d p1 = projectToPlane(original, axis);
+            Vector3d p2 = projectToPlane(rotated, axis);
+            double angle = p1.angle(p2);
+            result.set(axis,angle);
+            
+        }
+        return true;
+    }
        
        public static void getQuat(Vector3d axis, double angle, Quat4d q)
        {