- 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 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.001;
+ iter--;
+ }
+ return curr;