Get inline component direction irrespectively of connectivity

[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 796b5de99a5e5f6f289b382add6fed414074cab1..e240dc585426b972dd93a42174ce99e24f4403c1 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
@@ -519,14 +519,10 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
 
                if (turnAxis == null) {
                        Vector3d dir = getPathLegDirection(Direction.NEXT);
-                       if (dir.lengthSquared() > MathTools.NEAR_ZERO)
-                               dir.normalize();
                        return getControlPointOrientationQuat(dir, angle);
                } else {
                        Vector3d dir = getPathLegDirection(Direction.PREVIOUS);
-                       dir.negate();
-                       if (dir.lengthSquared() > MathTools.NEAR_ZERO)
-                               dir.normalize();
+                       if (dir != null) dir.negate();
                        return getControlPointOrientationQuat(dir, turnAxis, angle);
                }
        }
@@ -564,7 +560,7 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
 
 
        public static Quat4d getControlPointOrientationQuat(Vector3d dir, double angle) {
-               if (dir.lengthSquared() < MathTools.NEAR_ZERO)
+               if (dir == null || dir.lengthSquared() < MathTools.NEAR_ZERO)
                        return MathTools.getIdentityQuat();
 
 
@@ -579,7 +575,7 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
        }
 
        public static Quat4d getControlPointOrientationQuat(Vector3d dir, Vector3d up,  double angle) {
-               if (dir.lengthSquared() < MathTools.NEAR_ZERO)
+               if (dir == null || dir.lengthSquared() < MathTools.NEAR_ZERO)
                        return MathTools.getIdentityQuat();
 
                final Vector3d front = new Vector3d(1.0,0.0,0.0);
@@ -926,9 +922,8 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
 
                PipeControlPoint sub = isAxial() ? this : getDualSub();
                Vector3d pos = getWorldPosition(), pos2 = sub == this ? pos : sub.getWorldPosition();
-               Vector3d dir = sub.getPathLegDirection(Direction.NEXT);
+               Vector3d dir = sub.getInlineDir();
                
-               dir.normalize();
                dir.scale(length * 0.5);
                p1.set(pos);
                p2.set(pos2);
@@ -940,12 +935,16 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                PipeControlPoint sub = isAxial() || isDirected() || isTurn() ? this : getChildPoints().get(0);
                Vector3d pos = getWorldPosition(), pos2 = sub == this ? pos : sub.getWorldPosition();
                
-               Vector3d dir1 = getPathLegDirection(Direction.PREVIOUS);
-               Vector3d dir2 = sub.getPathLegDirection(Direction.NEXT);
+               Vector3d dir1;
+               Vector3d dir2;
                if (isInline()) {
-                       dir1.scale(length * 0.5);
+                       dir2 = getInlineDir();
                        dir2.scale(length * 0.5);
+                       dir1 = new Vector3d(dir2);
+                       dir1.negate();
                } else {
+                       dir1 = getPathLegDirection(Direction.PREVIOUS);
+                       dir2 = sub.getPathLegDirection(Direction.NEXT);
                        dir1.scale(length);
                        dir2.scale(length);
                }
@@ -955,20 +954,34 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                p2.add(dir2);
        }
 
+       /**
+        * Get both path leg directions, with (0,0,0) if no connection exists. The returned vectors are not normalized.
+        * 
+        * @param v1  Set to the direction towards the previous control point on output
+        * @param v2  Set to the direction towards the next control point on output
+        */
        public void getEndDirections(Tuple3d v1, Tuple3d v2) {
                PipeControlPoint sub = isAxial() ? this : getDualSub();
                
                Vector3d dir1 = getPathLegDirection(Direction.PREVIOUS);
                Vector3d dir2 = sub.getPathLegDirection(Direction.NEXT);
-               v1.set(dir1);
-               v2.set(dir2);
+               
+               if (dir1 != null)
+                       v1.set(dir1);
+               else
+                       v1.set(0,0,0);
+               
+               if (dir2 != null)
+                       v2.set(dir2);
+               else
+                       v2.set(0,0,0);
        }
 
        public void getInlineControlPointEnds(Tuple3d p1, Tuple3d p2, Vector3d dir) {
                assert (isInline());
 
                Vector3d pos = getWorldPosition();
-               dir.set(getPathLegDirection(Direction.NEXT));
+               dir.set(getInlineDir());
                dir.normalize();
                dir.scale(length * 0.5);
                p1.set(pos);
@@ -982,7 +995,7 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
 
                Vector3d pos = getWorldPosition();
                center.set(pos);
-               dir.set(getPathLegDirection(Direction.NEXT));
+               dir.set(getInlineDir());
                dir.normalize();
                dir.scale(length * 0.5);
                p1.set(pos);
@@ -991,6 +1004,22 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                p2.add(dir);
        }
 
+       public Vector3d getInlineDir() {
+               Vector3d dir = getPathLegDirection(Direction.NEXT);
+               if (dir == null) {
+                       dir = getPathLegDirection(Direction.PREVIOUS);
+                       if (dir != null) {
+                               // Use reverse direction
+                               dir.scale(-1.0);
+                       } else {
+                               // Control point is not connected at all, use current orientation
+                               dir = new Vector3d(1,0,0);
+                               MathTools.rotate(getWorldOrientation(), dir, dir);
+                       }
+               }
+               return dir;
+       }
+
        public double getInlineLength() {
                if (type == PointType.TURN)
                        return length;
@@ -999,22 +1028,27 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                return 0;
        }
 
+       /**
+        * Return the position indicated by the argument. If the indicated direction is not connected, the
+        * control point's wolrd position is returned instead.
+        * 
+        * @param type  A selector for the position to be returned
+        * @return  The selected position
+        */
        public Vector3d getRealPosition(PositionType type) {
                Vector3d pos = getWorldPosition();
                switch (type) {
                case NEXT: {
-                       Vector3d dir = getPathLegDirection(Direction.NEXT);
+                       Vector3d dir = getInlineDir();
                        double length = getInlineLength();
-                       dir.normalize();
                        dir.scale(length);
                        pos.add(dir);
                        break;
                }
                case PREVIOUS: {
-                       Vector3d dir = getPathLegDirection(Direction.PREVIOUS);
+                       Vector3d dir = getInlineDir();
                        double length = getInlineLength();
-                       dir.normalize();
-                       dir.scale(length);
+                       dir.scale(-length);
                        pos.add(dir);
                        break;
                }