- 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 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 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;