From 9e624e8e768f81e8c5edd665ac0bce6717013d5d Mon Sep 17 00:00:00 2001 From: Reino Ruusu Date: Thu, 20 Feb 2020 15:32:42 +0200 Subject: [PATCH] Fix interpretation of rotation angle to path leg end point vector gitlab #81 Change-Id: I64745f3b4f99215d77ac2bf44c7b038eb0282a78 --- .../controlpoint/PipeControlPoint.java | 53 ++++++++++++------- .../scenegraph/controlpoint/PipingRules.java | 2 +- 2 files changed, 36 insertions(+), 19 deletions(-) diff --git a/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipeControlPoint.java b/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipeControlPoint.java index 37f118cd..2905d26c 100644 --- a/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipeControlPoint.java +++ b/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipeControlPoint.java @@ -514,14 +514,42 @@ public class PipeControlPoint extends G3DNode implements IP3DNode { return type.name(); } - public Quat4d getControlPointOrientationQuat(double angle) { + public Vector3d getPathLegEndpointVector() { + PipeControlPoint a = findPreviousEnd(); + PipeControlPoint b = findNextEnd(); + + if (a == null || b == null) { + return getPathLegDirection(); + } + + Vector3d p1 = a.getWorldPosition(); + Vector3d p2 = b.getWorldPosition(); + p2.sub(p1); + double l = p2.length(); + if (l != 0.0) { + p2.scale(1.0 / l); + return p2; + } + else { + return getPathLegDirection(); + } + } + public Vector3d getPathLegDirection() { if (turnAxis == null) { - Vector3d dir = getPathLegDirection(Direction.NEXT); - return getControlPointOrientationQuat(dir, angle); + return getPathLegDirection(Direction.NEXT); } else { Vector3d dir = getPathLegDirection(Direction.PREVIOUS); if (dir != null) dir.negate(); + return dir; + } + } + + public Quat4d getControlPointOrientationQuat(double angle) { + Vector3d dir = getPathLegDirection(); + if (turnAxis == null) { + return getControlPointOrientationQuat(dir, angle); + } else { return getControlPointOrientationQuat(dir, turnAxis, angle); } } @@ -545,31 +573,20 @@ public class PipeControlPoint extends G3DNode implements IP3DNode { } public Quat4d getControlPointOrientationQuat(double angle, boolean reversed) { - - if (turnAxis == null) { - Vector3d dir = getPathLegDirection(Direction.NEXT); - return getControlPointOrientationQuat(dir, angle, reversed); - } else { - Vector3d dir = getPathLegDirection(Direction.PREVIOUS); - dir.negate(); - return getControlPointOrientationQuat(dir, angle, reversed); - } + Vector3d dir = getPathLegDirection(); + return getControlPointOrientationQuat(dir, angle, reversed); } - - - public static Quat4d getControlPointOrientationQuat(Vector3d dir, double angle) { + public Quat4d getControlPointOrientationQuat(Vector3d dir, double angle) { if (dir == null || dir.lengthSquared() < MathTools.NEAR_ZERO) return MathTools.getIdentityQuat(); - Vector3d up = new Vector3d(0.0, 1.0, 0.0); - double a = up.angle(dir); + double a = up.angle(getPathLegEndpointVector()); if (a < 0.1 || (Math.PI - a) < 0.1) { up.set(1.0, 0.0, 0.0); } - return getControlPointOrientationQuat(dir, up, angle); } 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 33c832f1..f21047f1 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 @@ -1901,7 +1901,7 @@ public class PipingRules { return Math.PI; // FIXME : argh } - Quat4d q = PipeControlPoint.getControlPointOrientationQuat(dir, tcp.getRotationAngle() != null ? tcp.getRotationAngle() : 0.0); + Quat4d q = tcp.getControlPointOrientationQuat(dir, tcp.getRotationAngle() != null ? tcp.getRotationAngle() : 0.0); Vector3d v = new Vector3d(); MathTools.rotate(q, MathTools.Y_AXIS,v); tcp.setTurnAxis(v); -- 2.45.2