Prevent elbows overlapping nozzles

gitlab #14

Change-Id: I9ecfa4dced7a48d2e7312986dd10be7b9a88bfae
(cherry picked from commit cc43aabf1f80291c17ece22060fbb0e690fb4864)

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 10cf23e37537561774f08aaf04ed3a57f67a8397..4f95e7964c8194db3df5afc950e043748efba808 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
@@ -989,18 +989,15 @@ public class PipingRules {
                        other = u.end;
                        position = u.startPoint;
                        dcpStart = true;
-                       if (!u.reversed)
-                               canMoveOther = true;
                        inlineEnd = u.end.isInline();
                                
                } else {
                        dcp = u.end;
                        other = u.start;
                        position = u.endPoint;
-                       if (u.reversed)
-                               canMoveOther = true;
                        inlineEnd = u.start.isInline();
                }
+               canMoveOther = !(other == u.updated);
 
                Vector3d directedDirection = direction(dcp, dcpStart ? Direction.NEXT : Direction.PREVIOUS);
                if (directedDirection == null) {
@@ -1030,6 +1027,16 @@ public class PipingRules {
 
                double distance = t.length();
                boolean aligned = (distance < ALLOWED_OFFSET);
+               double requiredSpace  = 0.0;
+               if (other.isVariableAngle()) {
+                   requiredSpace = spaceForTurn(other, dcp);
+               }
+               if (mu[0] < requiredSpace) {
+                   // At the moment, if next component is directly behind the nozzle, we must force moving the other component.
+                   // Trying to solve the situation by adding new turn creates infinite loop...   
+                   aligned = false;
+                   canMoveOther = true;
+               }
                if (aligned) {
                        if (u.start.isInline() || u.end.isInline() || u.start.asFixedAngle() || u.end.asFixedAngle())
                                processPathLeg(u, true, false);
@@ -1053,15 +1060,14 @@ public class PipingRules {
                                if (other.isVariableAngle()) {
 
                                        // TODO calculate needed space from next run end.
-                                   double space = spaceForTurn(other);
-                                       if (mu[0] < space) {
+                                   if (mu[0] < requiredSpace) {
                                                if (dcpStart) {
                                                        closest.set(u.startPoint);
                                                } else {
                                                        closest.set(u.endPoint);
                                                }
                                                Vector3d v = new Vector3d(directedDirection);
-                                               v.scale(space);
+                                               v.scale(requiredSpace);
                                                closest.add(v);
                                        }
 
@@ -1237,11 +1243,55 @@ public class PipingRules {
                
        }
        
-       private static double spaceForTurn(PipeControlPoint tcp) {
-               // TODO : this returns now space for 90 deg turn.
-               // The challenge: position of tcp affects the turn angle, which then affects the required space. Perhaps we need to iterate...
-               // Additionally, if the path legs contain offset, using just positions of opposite path leg ends is not enough,    
-               return ((TurnComponent)tcp.getPipelineComponent()).getTurnRadius();
+       private static double spaceForTurn(PipeControlPoint tcp, PipeControlPoint dcp) {
+               // TODO : if the path legs contain offset, using just positions of opposite path leg ends is not enough.
+           // TODO : current iterative way for calculating required space may return longer length that is required.
+               double tr = ((TurnComponent)tcp.getPipelineComponent()).getTurnRadius();
+           if (dcp == null)
+               return tr; // space for 90 deg
+           PipeControlPoint ne = tcp.findNextEnd();
+           PipeControlPoint pe = tcp.findPreviousEnd();
+           PipeControlPoint other = null;
+           if (dcp == ne)
+               other = pe;
+           else if (dcp == pe)
+               other = ne;
+           else
+               return tr; // space for 90 deg
+           if (other == null)
+               return tr; // space for 90 deg
+           Vector3d dir = dcp.getDirectedControlPointDirection();
+           Vector3d dp = dcp.getWorldPosition();
+           //Vector3d tp = tcp.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...
+           }
+           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.01;
+               iter--;
+           }
+           return curr;
        }
 
        private static void insertElbowUpdate(UpdateStruct2 u, PipeControlPoint dcp, PipeControlPoint next, boolean dcpStart, Vector3d position, Vector3d directedDirection) throws Exception{
@@ -1259,9 +1309,8 @@ public class PipingRules {
                        closest = MathTools.closestPointOnStraight(dcp.getWorldPosition(), position, directedDirection);
                        tcp = insertElbow(next, dcp, closest);
                }
-               // TODO properly calculate required distance between start and inserted elbow.
                double d = MathTools.distance(position, closest);
-               double s = spaceForTurn(tcp);
+               double s = spaceForTurn(tcp,dcp);
                if (d < s)  {
                        d = s - d;
                        Vector3d p = new Vector3d(directedDirection);
@@ -1645,9 +1694,9 @@ public class PipingRules {
        private static void updateEndComponentControlPoint(PipeControlPoint ecp, Vector3d dir) throws Exception {
                if (DEBUG)
                        System.out.println("PipingRules.updateEndComponentControlPoint() " + ecp);
-               //FIXME : end control point cannot be fixed!
-               //if (!ecp.isFixed())
-               updateControlPointOrientation(ecp, dir);
+               
+               if (!ecp.isFixed()) // prevent overriding nozzle orientations..
+                  updateControlPointOrientation(ecp, dir);
 
                for (PipeControlPoint pcp : ecp.getChildPoints()) {
                        // TODO update position