From b9efd234b46a0c15a9ee034592b90d72f18480fe Mon Sep 17 00:00:00 2001 From: Marko Luukkainen Date: Fri, 29 Nov 2019 14:34:50 +0200 Subject: [PATCH] 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 --- .../src/org/simantics/g3d/math/MathTools.java | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 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 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; -- 2.47.1