Fix directed path leg update failing to update inline lengths

[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 c5d780c36b8d5f2dc3b4d0abf5642410509614f3..eec074a68b88693812f7c8f686a08036ea72a548 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
@@ -331,9 +331,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)
@@ -751,6 +751,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();
@@ -771,6 +779,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 {
@@ -789,6 +798,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;
                 }
             }
@@ -796,6 +806,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) {
@@ -805,6 +823,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) {
@@ -822,12 +844,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);
@@ -842,6 +872,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)  {
@@ -860,6 +894,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();
@@ -868,6 +906,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);
@@ -898,9 +940,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);
@@ -918,9 +958,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);
        }
@@ -1470,6 +1508,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());