From 9e624e8e768f81e8c5edd665ac0bce6717013d5d Mon Sep 17 00:00:00 2001
From: Reino Ruusu <reino.ruusu@semantum.fi>
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.47.1