Fix interpretation of rotation angle to path leg end point vector

gitlab #81

Change-Id: I64745f3b4f99215d77ac2bf44c7b038eb0282a78

diff --git a/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipeControlPoint.java b/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipeControlPoint.java
index 37f118cd05e00bd6b76db739cb661e85efed8ea6..2905d26c4493058b1483705c8a2df8e68a9ba209 100644 (file)
--- a/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipeControlPoint.java
+++ b/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipeControlPoint.java
@@ -514,14 +514,42 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                return type.name();
        }
 
-       public Quat4d getControlPointOrientationQuat(double angle) {
+       public Vector3d getPathLegEndpointVector() {
+               PipeControlPoint a = findPreviousEnd();
+               PipeControlPoint b = findNextEnd();
+               
+               if (a == null || b == null) {
+                       return getPathLegDirection();
+               }
+               
+               Vector3d p1 = a.getWorldPosition();
+               Vector3d p2 = b.getWorldPosition();
+               p2.sub(p1);
+               double l = p2.length();
+               if (l != 0.0) {
+                       p2.scale(1.0 / l);
+                       return p2;
+               }
+               else {
+                       return getPathLegDirection();
+               }
+       }
 
+       public Vector3d getPathLegDirection() {
                if (turnAxis == null) {
-                       Vector3d dir = getPathLegDirection(Direction.NEXT);
-                       return getControlPointOrientationQuat(dir, angle);
+                       return getPathLegDirection(Direction.NEXT);
                } else {
                        Vector3d dir = getPathLegDirection(Direction.PREVIOUS);
                        if (dir != null) dir.negate();
+                       return dir;
+               }
+       }
+       
+       public Quat4d getControlPointOrientationQuat(double angle) {
+               Vector3d dir = getPathLegDirection();
+               if (turnAxis == null) {
+                       return getControlPointOrientationQuat(dir, angle);
+               } else {
                        return getControlPointOrientationQuat(dir, turnAxis, angle);
                }
        }
@@ -545,31 +573,20 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
        }
 
        public Quat4d getControlPointOrientationQuat(double angle, boolean reversed) {
-
-               if (turnAxis == null) {
-                       Vector3d dir = getPathLegDirection(Direction.NEXT);
-                       return getControlPointOrientationQuat(dir, angle, reversed);
-               } else {
-                       Vector3d dir = getPathLegDirection(Direction.PREVIOUS);
-                       dir.negate();
-                       return getControlPointOrientationQuat(dir, angle, reversed);
-               }
+               Vector3d dir = getPathLegDirection();
+               return getControlPointOrientationQuat(dir, angle, reversed);
        }
 
-
-
-       public static Quat4d getControlPointOrientationQuat(Vector3d dir, double angle) {
+       public Quat4d getControlPointOrientationQuat(Vector3d dir, double angle) {
                if (dir == null || dir.lengthSquared() < MathTools.NEAR_ZERO)
                        return MathTools.getIdentityQuat();
 
-
                Vector3d up = new Vector3d(0.0, 1.0, 0.0);
-               double a = up.angle(dir);
+               double a = up.angle(getPathLegEndpointVector());
                if (a < 0.1 || (Math.PI - a) < 0.1) {
                        up.set(1.0, 0.0, 0.0);
                }
 
-
                return getControlPointOrientationQuat(dir, up, angle);
        }
 

diff --git a/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipingRules.java b/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipingRules.java
index 33c832f1b8c4bec41cc22d5c0d1dec791f6f8968..f21047f1674d1d20ebfa9db9901d593cb6a08d5e 100644 (file)
--- a/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipingRules.java
+++ b/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipingRules.java
@@ -1901,7 +1901,7 @@ public class PipingRules {
                                return Math.PI; // FIXME : argh
                        }
                        
-                       Quat4d q = PipeControlPoint.getControlPointOrientationQuat(dir, tcp.getRotationAngle() != null ? tcp.getRotationAngle() : 0.0);
+                       Quat4d q = tcp.getControlPointOrientationQuat(dir, tcp.getRotationAngle() != null ? tcp.getRotationAngle() : 0.0);
                        Vector3d v = new Vector3d();
                        MathTools.rotate(q, MathTools.Y_AXIS,v);
                        tcp.setTurnAxis(v);