Fix handling of offsets in directed path leg updates

gitlab #79

Change-Id: Ib2c15821a3a147249c0465469973cb81c454fa18

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 50b22571874c46482359ec8b2472e4ce7673f28f..d00e4018de1407887cb1982e578bb2b3d967a4a5 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
@@ -392,7 +392,7 @@ public class PipingRules {
                                dir.scale(1.0/Math.sqrt(l));
                        
                        int iter = 100;
-                       while (iter >= 0) {
+                       while (true) {
                                iter--;
                                offset.set(0.0, 0.0, 0.0);
                                
@@ -406,7 +406,7 @@ public class PipingRules {
                                
                                Point3d nep = new Point3d(endPoint);
                                nep.sub(offset);
-                               if (nep.distance(ep) < 0.0000000001) {
+                               if (nep.distance(ep) < 0.0000000001 || iter <= 0) {
                                        break;
                                }
                                
@@ -477,6 +477,21 @@ public class PipingRules {
                return new UpdateStruct2(start, startPoint, list, end, endPoint, dir, offset, hasOffsets, iter, direction == Direction.PREVIOUS, toRemove, updated);
        }
        
+       private static Vector3d pathLegDirection(PipeControlPoint start) {
+               ArrayList<PipeControlPoint> list = new ArrayList<PipeControlPoint>();
+               PipeControlPoint end = start.findNextEnd(list);
+               if (start == end) {
+                       return start.getDirection(Direction.NEXT);
+               }
+               
+               Vector3d offset = new Vector3d();
+               Vector3d startPoint = start.getWorldPosition();
+               Vector3d endPoint = end.getWorldPosition();
+               Vector3d dir = new Vector3d();
+               calculateOffset(startPoint, endPoint, start, list, end, dir, offset);
+               return dir;
+       }
+       
        private static boolean asDirected(PipeControlPoint pcp, Direction direction) {
                if (pcp.isDirected())
                        return true;
@@ -1047,22 +1062,27 @@ public class PipingRules {
                                return;
                        }
                }
-               Point3d directedEndPoint = new Point3d(u.endPoint);
-               if (u.hasOffsets)
-                       directedEndPoint.sub(u.offset);
+               
+               Point3d otherPosition = new Point3d(dcpStart ? u.endPoint : u.startPoint);
+               if (u.hasOffsets) {
+                       Vector3d dir = new Vector3d(), offset = new Vector3d();
+                       calculateDirectedOffset(u.startPoint, u.endPoint, u.start, u.list, u.end, dir, offset);
+                       u.dir = dir;
+                       u.offset = offset;
+                       
+                       if (dcpStart)
+                               otherPosition.add(offset);
+                       else
+                               otherPosition.sub(offset);
+               }
 
                double mu[] = new double[2];
 
                Vector3d closest;
                Vector3d t = new Vector3d();
 
-               if (dcpStart) {
-                       closest = MathTools.closestPointOnStraight(directedEndPoint, u.startPoint, directedDirection, mu);
-                       t.sub(closest, directedEndPoint);
-               } else {
-                       closest = MathTools.closestPointOnStraight(u.startPoint, directedEndPoint, directedDirection, mu);
-                       t.sub(closest, u.startPoint);
-               }
+               closest = MathTools.closestPointOnStraight(otherPosition, position, directedDirection, mu);
+               t.sub(closest, otherPosition);
 
                double distance = t.length();
                boolean aligned = (distance < ALLOWED_OFFSET);
@@ -1139,7 +1159,6 @@ public class PipingRules {
                                                // TODO : calculate needed space from next run end.
                                                if (allowInsertRemove)
                                                        insertElbowUpdate(u, dcp, nextToMoved, dcpStart, position, directedDirection);
-                                                       
                                                else
                                                        triedIR = true;
                                        }
@@ -1191,8 +1210,6 @@ public class PipingRules {
                                }
                        }
                }
-               
-               
        }
 
        private static void updateDualDirectedPathLeg(UpdateStruct2 u, PathLegUpdateType lengthChange) throws Exception {
@@ -1317,34 +1334,49 @@ public class PipingRules {
            if (other == null)
                return tr; // space for 90 deg
            Vector3d dir = dcp.getDirectedControlPointDirection();
-           Vector3d dp = dcp.getWorldPosition();
-           Vector3d op = other.getWorldPosition();
-           double u[] = new double[1];
-           Vector3d closest = MathTools.closestPointOnStraight(op, dp, dir,u);
-           if (MathTools.distanceSquared(closest, op) <= MIN_INLINE_LENGTH) {
-               if (u[0] > -MIN_INLINE_LENGTH)
-                   return 0.0; // point following turn is directly in the front of the nozzle.
-               else
-                   return tr*2.0; // point following turn is directly behind the nozzle, in theory, we should return Double.Inf...
+           Vector3d dir2;
+           if (other == ne) {
+               dir2 = pathLegDirection(tcp);
+           } else {
+               dir2 = pathLegDirection(pe);
+               dir2.negate();
            }
+
+           double d = dir.dot(dir2);
+           if (d > 0.9999)
+               return 0.0; // point following turn is directly in the front of the nozzle.
+           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; 
            int iter = 10;
-           Vector3d v1 = new Vector3d();
-           Vector3d v2 = new Vector3d();
-           while (iter > 0) {
-               Vector3d tp = new Vector3d(dp);
-               MathTools.mad(tp, dir, curr);
-               v1.sub(tp, dp); // Vector from nozzle to turn 
-               v2.sub(op,tp);  // Vector from turn to other 
-               double a = v1.angle(v2);
-               double t = Math.tan((Math.PI - a) * 0.5);
-               double R = 0.0;
-               if (t > MathTools.NEAR_ZERO)
-                   R = tr / t;
-               if (R <= curr)
-                   break;
-               curr = R*1.001;
-               iter--;
+           Vector3d tp0 = tcp.getPosition();
+           try {
+                   Vector3d dp = dcp.getWorldPosition();
+                   while (iter > 0) {
+                       Vector3d tp = new Vector3d(dir);
+                       tp.scaleAdd(curr, dp);
+                       tcp.setPosition(tp);
+                           if (other == ne) {
+                               dir2 = pathLegDirection(tcp);
+                           } else {
+                               dir2 = pathLegDirection(pe);
+                               dir2.negate();
+                           }
+                           
+                       double a = dir.angle(dir2);
+                       double t = Math.tan(a * 0.5);
+                       double R = 0.0;
+                       if (t > MathTools.NEAR_ZERO)
+                           R = tr * t;
+                       if (R <= curr)
+                           break;
+                       curr = R*1.001;
+                       iter--;
+                   }
+           }
+           finally {
+               tcp.setPosition(tp0);
            }
            return curr;
        }
@@ -1356,26 +1388,19 @@ public class PipingRules {
 //             closest.add(directedDirection);
                
                PipeControlPoint tcp = null;
-               Vector3d closest;
+               Vector3d closest = new Vector3d(directedDirection);
+               closest.scaleAdd(dcp.getPipeRun().getTurnRadius(), position);
                if (dcpStart) {
-                       closest = MathTools.closestPointOnStraight(next.getWorldPosition(), position, directedDirection);
                        tcp = insertElbow(dcp, next, closest);
                } else {
-                       closest = MathTools.closestPointOnStraight(dcp.getWorldPosition(), position, directedDirection);
                        tcp = insertElbow(next, dcp, closest);
                }
-               double d = MathTools.distance(position, closest);
-               double s = spaceForTurn(tcp,dcp);
-               if (d < s)  {
-                       d = s - d;
-                       Vector3d p = new Vector3d(directedDirection);
-                       p.scale(d);
-                       p.add(closest);
-                       tcp.setPosition(p);
-                       closest = p;
-               }
-               
                
+               double s = spaceForTurn(tcp,dcp);
+               Vector3d p = new Vector3d(directedDirection);
+               p.scaleAdd(s, position);
+               tcp.setPosition(p);
+               closest = p;
 
                if (DEBUG)
                        System.out.println("PipingRules.updateDirectedPipeRun() inserted " + tcp);
@@ -1819,13 +1844,11 @@ public class PipingRules {
                        }
                        double turnAngle = prev.angle(next);
        
-                       double angle = Math.PI - turnAngle;
-       
                        Vector3d turnAxis = new Vector3d();
                        turnAxis.cross(prev, next);
                        if (turnAxis.lengthSquared() > MathTools.NEAR_ZERO) {
                                double elbowRadius = ((TurnComponent)tcp.getPipelineComponent()).getTurnRadius();
-                               double R = elbowRadius / Math.tan(angle * 0.5);
+                               double R = elbowRadius * Math.tan(turnAngle * 0.5);
                                
                                turnAxis.normalize();
                                tcp.setTurnAngle(turnAngle);