Let root node determine up direction for rotation angle calculations

[simantics/3d.git] / org.simantics.plant3d / src / org / simantics / plant3d / scenegraph / controlpoint / PipeControlPoint.java

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 2905d26c4493058b1483705c8a2df8e68a9ba209..72ef4e687a0f378d24400f94ff79405585c61179 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
@@ -15,6 +15,7 @@ import javax.vecmath.Vector3d;
 import org.simantics.g3d.math.MathTools;
 import org.simantics.g3d.property.annotations.GetPropertyValue;
 import org.simantics.g3d.scenegraph.G3DNode;
+import org.simantics.g3d.scenegraph.base.INode;
 import org.simantics.plant3d.scenegraph.IP3DNode;
 import org.simantics.plant3d.scenegraph.Nozzle;
 import org.simantics.plant3d.scenegraph.P3DRootNode;
@@ -581,15 +582,24 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                if (dir == null || dir.lengthSquared() < MathTools.NEAR_ZERO)
                        return MathTools.getIdentityQuat();
 
-               Vector3d up = new Vector3d(0.0, 1.0, 0.0);
+               final P3DRootNode root = getRoot();
+               Vector3d up = root != null ? new Vector3d(root.getUpVector()) : new Vector3d(0.0, 1.0, 0.0);
                double a = up.angle(getPathLegEndpointVector());
                if (a < 0.1 || (Math.PI - a) < 0.1) {
-                       up.set(1.0, 0.0, 0.0);
+                       // Rotate components
+                       up.set(up.getY(), up.getZ(), up.getX());
                }
 
                return getControlPointOrientationQuat(dir, up, angle);
        }
 
+       public P3DRootNode getRoot() {
+               INode n = getParent();
+               while (n != null && !(n instanceof P3DRootNode))
+                       n = n.getParent();
+               return (P3DRootNode) n;
+       }
+
        public static Quat4d getControlPointOrientationQuat(Vector3d dir, Vector3d up,  double angle) {
                if (dir == null || dir.lengthSquared() < MathTools.NEAR_ZERO)
                        return MathTools.getIdentityQuat();
@@ -600,7 +610,8 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
 
 
                Vector3d right = new Vector3d();
-
+               
+               up = new Vector3d(up);
                right.cross(dir, up);
                up.cross(right, dir);
                right.normalize();