X-Git-Url: https://gerrit.simantics.org/r/gitweb?a=blobdiff_plain;ds=sidebyside;f=org.simantics.g3d%2Fsrc%2Forg%2Fsimantics%2Fg3d%2Fmath%2FMathTools.java;h=d761b0ec8350450e8f45829fc8446ffb823bff01;hb=eb67906ff85a83b2f71b823110e5c3d12da8bfc2;hp=0fbd110f3025e96ad7856ef4c17fc5cf40887b15;hpb=a1e1faa6915445e786f482170576b9c9d0f5d982;p=simantics%2F3d.git

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 0fbd110f..d761b0ec 100644
--- 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)
 	{