From: Marko Luukkainen <marko.luukkainen@semantum.fi>
Date: Wed, 4 Dec 2019 11:54:06 +0000 (+0200)
Subject: Prevent elbows overlapping nozzles
X-Git-Url: https://gerrit.simantics.org/r/gitweb?a=commitdiff_plain;h=cc43aabf1f80291c17ece22060fbb0e690fb4864;p=simantics%2F3d.git

Prevent elbows overlapping nozzles

gitlab #14

Change-Id: I9ecfa4dced7a48d2e7312986dd10be7b9a88bfae
---

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 10cf23e3..4f95e796 100644
--- 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