From 7e518e75a4de0f84f2b0d1b60640e2ed6c530fcd Mon Sep 17 00:00:00 2001 From: Reino Ruusu Date: Fri, 14 Feb 2020 14:19:52 +0200 Subject: [PATCH] Fix handling of offsets in directed path leg updates gitlab #79 Change-Id: Ib2c15821a3a147249c0465469973cb81c454fa18 --- .../scenegraph/controlpoint/PipingRules.java | 137 ++++++++++-------- 1 file changed, 80 insertions(+), 57 deletions(-) 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 50b22571..d00e4018 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 @@ -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 list = new ArrayList(); + 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); -- 2.47.1