Fix issue with multiple degenerate legs in front of a nozzle

gitlab #91

Change-Id: I458db0787a0b2be4282df4619bcfdaab674e3ef1

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 e8729872d53a0b863cf40e45049b1eefd7fdad96..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
@@ -1362,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 {
@@ -1379,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;
@@ -1856,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);