From: Reino Ruusu Date: Tue, 10 Dec 2019 18:30:55 +0000 (+0200) Subject: Fix createRotation() and avoid unnecessary trigonometrics roundtrip X-Git-Url: https://gerrit.simantics.org/r/gitweb?a=commitdiff_plain;h=refs%2Fchanges%2F05%2F3705%2F1;p=simantics%2F3d.git 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 b9941c8b..1b44cc45 100644 --- 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) {