Allow insertion of reducers in the middle of pipe runs

[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 8616e70352815a50256a1b16e32afdbe1c52c787..796b5de99a5e5f6f289b382add6fed414074cab1 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.ParentNode;
 import org.simantics.plant3d.scenegraph.IP3DNode;
 import org.simantics.plant3d.scenegraph.Nozzle;
 import org.simantics.plant3d.scenegraph.P3DRootNode;
@@ -331,9 +332,9 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
         // We need to calculate turnAngle and rotationAngle
            Vector3d dirOut = getPathLegDirection(direction == Direction.NEXT ? Direction.NEXT : Direction.PREVIOUS);
         Vector3d dir = getPathLegDirection(direction == Direction.NEXT ? Direction.PREVIOUS : Direction.NEXT);
+        if (dir == null || dirOut == null)
+            return;
         dir.negate();
-        dirOut.normalize();
-        dir.normalize();
         double angle = dir.angle(dirOut);
         //super._setNext(null);
         if (direction == Direction.NEXT)
@@ -529,26 +530,34 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                        return getControlPointOrientationQuat(dir, turnAxis, angle);
                }
        }
+       
+       public Quat4d getControlPointOrientationQuat(Vector3d dir, double angle, boolean reversed) {
+           if (turnAxis == null) {
+            if (dir.lengthSquared() > MathTools.NEAR_ZERO)
+                dir.normalize();
+            Quat4d q =  getControlPointOrientationQuat(dir, angle);
+            if (reversed) {
+                Quat4d q2 = new Quat4d();
+                q2.set(new AxisAngle4d(MathTools.Y_AXIS, Math.PI));
+                q.mulInverse(q2);
+            }
+            return q;
+        } else {
+            if (dir.lengthSquared() > MathTools.NEAR_ZERO)
+                dir.normalize();
+            return getControlPointOrientationQuat(dir, turnAxis, angle);
+        }
+       }
 
        public Quat4d getControlPointOrientationQuat(double angle, boolean reversed) {
 
                if (turnAxis == null) {
                        Vector3d dir = getPathLegDirection(Direction.NEXT);
-                       if (dir.lengthSquared() > MathTools.NEAR_ZERO)
-                               dir.normalize();
-                       Quat4d q =  getControlPointOrientationQuat(dir, angle);
-                       if (reversed) {
-                               Quat4d q2 = new Quat4d();
-                               q2.set(new AxisAngle4d(MathTools.Y_AXIS, Math.PI));
-                               q.mulInverse(q2);
-                       }
-                       return q;
+                       return getControlPointOrientationQuat(dir, angle, reversed);
                } else {
                        Vector3d dir = getPathLegDirection(Direction.PREVIOUS);
                        dir.negate();
-                       if (dir.lengthSquared() > MathTools.NEAR_ZERO)
-                               dir.normalize();
-                       return getControlPointOrientationQuat(dir, turnAxis, angle);
+                       return getControlPointOrientationQuat(dir, angle, reversed);
                }
        }
 
@@ -612,8 +621,8 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                if (isDualSub())
                        throw new RuntimeException("Dual sub points cannot be inserted.");
                // size change control point cannot be inserted this way, because it ends PipeRun
-               if (isSizeChange())
-                       throw new RuntimeException("Size change points cannot be inserted.");
+//             if (isSizeChange())
+//                     throw new RuntimeException("Size change points cannot be inserted.");
                PipeRun piperun = previous.getPipeRun();
                // and just to make sure that control point structure is not corrupted
                if (getPipeRun() != null) {
@@ -743,6 +752,14 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                return dir;
        }
        
+       /**
+        * Returns direction vector. 
+        * 
+        * For directed control points, always returns outwards pointing vector.
+        * 
+        * @param direction
+        * @return normalized vector, or null
+        */
        public Vector3d getDirection(Direction direction) {
         if (isDirected())
             return getDirectedControlPointDirection();
@@ -763,6 +780,7 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                     Vector3d offset = new Vector3d();
                     MathTools.rotate(q2, v, offset);
                     MathTools.rotate(q, offset, dir);
+                    dir.normalize();
                     return dir;
                 }
             } else {
@@ -781,6 +799,7 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                     Vector3d offset = new Vector3d();
                     MathTools.rotate(q2, v, offset);
                     MathTools.rotate(q, offset, dir);
+                    dir.normalize();
                     return dir;
                 }
             }
@@ -788,6 +807,14 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
         return null;
     }
 
+       /**
+        * Returns path leg direction of the control point.
+        * 
+        * This method differs from getDirection by also returning inward pointing vectors for directed control points.
+        * 
+        * @param direction
+        * @return
+        */
        public Vector3d getPathLegDirection(Direction direction) {
                if (direction == Direction.NEXT) {
                        if (next != null) {
@@ -797,6 +824,10 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                                }
                                Vector3d v = new Vector3d();
                                v.sub(next.getWorldPosition(),pcp.getWorldPosition());
+                               if (v.lengthSquared() > MathTools.NEAR_ZERO)
+                    v.normalize();
+                else
+                    return null;
                                return v;
                        } else {
                                if (previous == null) {
@@ -814,12 +845,20 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                                                }
                                                Vector3d v = new Vector3d();
                                                v.sub(pcp.getWorldPosition(),previous.getWorldPosition());
+                                               if (v.lengthSquared() > MathTools.NEAR_ZERO)
+                               v.normalize();
+                                               else
+                                                   return null;
                                                return v;
                                        } else if (isDirected()) {
                                                return getDirectedControlPointDirection();
                                        } else if (isEnd()) {
                                                Vector3d v = new Vector3d();
                                                v.sub(getWorldPosition(),previous.getWorldPosition());
+                                               if (v.lengthSquared() > MathTools.NEAR_ZERO)
+                            v.normalize();
+                        else
+                            return null;
                                                return v;
                                        } else if (isTurn() && asFixedAngle() && !_getReversed()) {
                                                return getDirection(Direction.NEXT);
@@ -834,6 +873,10 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                                        pcp = getParentPoint();
                                Vector3d v = new Vector3d();
                                v.sub(previous.getWorldPosition(),pcp.getWorldPosition());
+                               if (v.lengthSquared() > MathTools.NEAR_ZERO)
+                    v.normalize();
+                else
+                    return null;
                                return v;
                        } else {
                                if (next == null)  {
@@ -852,6 +895,10 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                                                }
                                                Vector3d v = new Vector3d();
                                                v.sub(pcp.getWorldPosition(),next.getWorldPosition());
+                                               if (v.lengthSquared() > MathTools.NEAR_ZERO)
+                            v.normalize();
+                        else
+                            return null;
                                                return v;
                                        } else if (isDirected()) {
                                                Vector3d v = getDirectedControlPointDirection();
@@ -860,6 +907,10 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                                        } else if (isEnd()) {
                                                Vector3d v = new Vector3d();
                                                v.sub(getWorldPosition(),next.getWorldPosition());
+                                               if (v.lengthSquared() > MathTools.NEAR_ZERO)
+                            v.normalize();
+                        else
+                            return null;
                                                return v;
                                        } else if (isTurn() && asFixedAngle() && _getReversed()) {
                                                return getDirection(Direction.PREVIOUS);
@@ -890,9 +941,7 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                Vector3d pos = getWorldPosition(), pos2 = sub == this ? pos : sub.getWorldPosition();
                
                Vector3d dir1 = getPathLegDirection(Direction.PREVIOUS);
-               dir1.normalize();
                Vector3d dir2 = sub.getPathLegDirection(Direction.NEXT);
-               dir2.normalize();
                if (isInline()) {
                        dir1.scale(length * 0.5);
                        dir2.scale(length * 0.5);
@@ -910,9 +959,7 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                PipeControlPoint sub = isAxial() ? this : getDualSub();
                
                Vector3d dir1 = getPathLegDirection(Direction.PREVIOUS);
-               dir1.normalize();
                Vector3d dir2 = sub.getPathLegDirection(Direction.NEXT);
-               dir2.normalize();
                v1.set(dir1);
                v2.set(dir2);
        }
@@ -1220,7 +1267,7 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                        } else if (currentNext != null) {
                                if (currentNext.isDualInline()) {
                                        PipeControlPoint sccp = currentNext;
-                                       PipeControlPoint ocp = getDualSub();
+                                       PipeControlPoint ocp = currentNext.getDualSub();
                                        if (ocp == null) {
                                                throw new RuntimeException("Removing PipeControlPoint " + this+ " structure damaged, no offset control point");
                                        }
@@ -1462,6 +1509,8 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
        public void setOrientation(Quat4d orientation) {
                if (MathTools.equals(orientation, getOrientation()))
                        return;
+               if (getPipelineComponent() != null && (getPipelineComponent() instanceof Nozzle))
+                   System.out.println();
                super.setOrientation(orientation);
                if (getParentPoint() == null && component != null)
                        component._setWorldOrientation(getWorldOrientation());