Fix issue with multiple degenerate legs in front of a nozzle

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

diff --git a/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipingRules.java b/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipingRules.java
index f21047f1674d1d20ebfa9db9901d593cb6a08d5e..b2376341985d60247dece769a5be4921be26a78b 100644 (file)
--- a/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipingRules.java
+++ b/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipingRules.java
@@ -759,7 +759,7 @@ public class PipingRules {
                        continue;
                    double curr = gapObj.d;
                    int d = 1;
-                   while (curr < -MIN_INLINE_LENGTH) {
+                   while (d < gaps.size() && curr < -MIN_INLINE_LENGTH) {
                        GapObj next = i+d >= 0 ? gaps.get(i+d) : null;
                     GapObj prev = i-d >= 0 ? gaps.get(i-d) : null;
                        if (next != null && next.gap == Gap.SPACE) {
@@ -778,12 +778,12 @@ public class PipingRules {
                                pcp.first.setWorldPosition(p);
                            }
                        }
-                       if (curr < -MIN_INLINE_LENGTH && prev != null && prev.gap == Gap.SPACE) {
+                       else if (prev != null && prev.gap == Gap.SPACE) {
                            double move = Math.min(-curr, prev.d);
                         curr+= move;
-                        next.d -= move;
-                        if (next.d < MIN_INLINE_LENGTH)
-                            next.gap = Gap.ATTACHED;
+                        prev.d -= move;
+                        if (prev.d < MIN_INLINE_LENGTH)
+                            prev.gap = Gap.ATTACHED;
                         Vector3d mv = new Vector3d(dir);
                         mv.normalize();
                         mv.scale(-move);
@@ -794,6 +794,9 @@ public class PipingRules {
                             pcp.first.setWorldPosition(p);
                         }
                        }
+                       else {
+                           d++;
+                       }
                    }
                }
            } else {
@@ -1359,7 +1362,7 @@ public class PipingRules {
            else if (d < -0.9999)
             return tr*2.0; // point following turn is directly behind the nozzle, in theory, we should return Double.Inf...
            
-           double curr = tr*0.1; 
+           double curr = 0.0;
            int iter = 10;
            Vector3d tp0 = tcp.getPosition();
            try {
@@ -1367,7 +1370,7 @@ public class PipingRules {
                    while (iter > 0) {
                        Vector3d tp = new Vector3d(dir);
                        tp.scaleAdd(curr, dp);
-                       tcp.setPosition(tp);
+                       tcp._setPosition(tp); // no firing of listeners here
                            if (other == ne) {
                                dir2 = pathLegDirection(tcp);
                            } else {
@@ -1376,10 +1379,12 @@ public class PipingRules {
                            }
                            
                        double a = dir.angle(dir2);
-                       double t = Math.tan(a * 0.5);
-                       double R = 0.0;
-                       if (t > MathTools.NEAR_ZERO)
-                           R = tr * t;
+                       
+                       // other is directly between dcp and tcp, a zero angle turn should do
+                       if (Math.PI - a <= MathTools.NEAR_ZERO)
+                           return 0.0;
+                       
+                       double R = tr * Math.tan(a * 0.5);
                        if (R <= curr)
                            break;
                        curr = R*1.001;
@@ -1387,7 +1392,7 @@ public class PipingRules {
                    }
            }
            finally {
-               tcp.setPosition(tp0);
+               tcp._setPosition(tp0); // return the original value
            }
            return curr;
        }
@@ -1853,7 +1858,9 @@ public class PipingRules {
                                        return tcp.getTurnAngle();
                                return Math.PI; // FIXME : argh
                        }
-                       double turnAngle = prev.angle(next);
+                       
+                       final boolean isDegenerate = prev.lengthSquared() < MathTools.NEAR_ZERO || next.lengthSquared() < MathTools.NEAR_ZERO;
+                       double turnAngle = isDegenerate ? 0.0 : prev.angle(next);
        
                        Vector3d turnAxis = new Vector3d();
                        turnAxis.cross(prev, next);