From: Marko Luukkainen <marko.luukkainen@semantum.fi>
Date: Fri, 29 Nov 2019 12:34:50 +0000 (+0200)
Subject: Converting variable angle turn turn to fixed angle could fail.
X-Git-Url: https://gerrit.simantics.org/r/gitweb?a=commitdiff_plain;h=b9efd234b46a0c15a9ee034592b90d72f18480fe;p=simantics%2F3d.git

Converting variable angle turn turn to fixed angle could fail.

Calculating rotating angle from input and output directions was using 0
- PI restricted angle calculation, where expected result was -PI - PI

gitlab #46

Change-Id: I08956dc40009c4fea17883b7a6caf8468db8bf7f
---

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 d761b0ec..aa8b0551 100644
--- a/org.simantics.g3d/src/org/simantics/g3d/math/MathTools.java
+++ b/org.simantics.g3d/src/org/simantics/g3d/math/MathTools.java
@@ -839,10 +839,19 @@ public class MathTools {
             result.y = axis.y;
             result.z = axis.z;
         } else {
+            // Project vectors to Axis plane
             Vector3d p1 = projectToPlane(original, axis);
             Vector3d p2 = projectToPlane(rotated, axis);
-            double angle = p1.angle(p2);
-            result.set(axis,angle);
+            // Create vectors where z-axis is plane normal
+            Quat4d q = getQuat(createRotation(axis, Z_AXIS));
+            Vector3d t1 = new Vector3d();
+            Vector3d t2 = new Vector3d();
+            rotate(q, p1, t1);
+            rotate(q, p2, t2);
+            // Calculate angles on z-axis plane.
+            double a1 = Math.atan2(t1.y, t1.x);
+            double a2 = Math.atan2(t2.y, t2.x);
+            result.set(axis,a2-a1);
             
         }
         return true;