Rotation angle fully in context of the path leg delta

[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 b67562459016b3dddcb1ba730a99a1a5bb016beb..9e9beb9c37711e63012c006b4221839de8cfb819 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;
@@ -514,14 +515,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,34 +574,37 @@ 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);
+               final P3DRootNode root = getRoot();
+               Vector3d up = root != null ? new Vector3d(root.getUpVector()) : new Vector3d(0.0, 1.0, 0.0);
+               final Vector3d legDir = getPathLegEndpointVector();
+               double a = up.angle(legDir);
                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());
                }
-
+               
+               // Project up vector into a normal of the leg direction before applying to 'dir'
+               MathTools.mad(up, legDir, -legDir.dot(up)/legDir.lengthSquared());
+               up.normalize();
 
                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();
@@ -583,7 +615,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();
@@ -1212,9 +1245,7 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                                if (link) {
                                    if (currentPrev.isDirected() && currentNext.isDirected())
                                        link = false;
-                                   else if (this.isDualInline()) {
-                                       link = false;
-                                   } else if (this.isDualSub()) {
+                                   else if (this.isDualSub()) {
                                        throw new RuntimeException("_remove() is called for parent point, somehow got to child point. " + this);
                                    }
                                }