Calculating turn specific coordinates

gitlab #29

Change-Id: I9586a8a2aa26a8ec9f8bb3217639c893726980da

diff --git a/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/TurnComponent.java b/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/TurnComponent.java
index a1374def587c857a3b0327cba4510891896838be..3d5fb5bf7fc27130d078414dde5da2a72bbd17c9 100644 (file)
--- a/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/TurnComponent.java
+++ b/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/TurnComponent.java
@@ -9,6 +9,7 @@ import org.simantics.g3d.math.MathTools;
 import org.simantics.g3d.property.annotations.GetPropertyValue;
 import org.simantics.g3d.property.annotations.SetPropertyValue;
 import org.simantics.g3d.scenegraph.base.ParentNode;
+import org.simantics.g3d.tools.NodeTools;
 import org.simantics.objmap.graph.annotations.DynamicGraphType;
 import org.simantics.objmap.graph.annotations.GetType;
 import org.simantics.objmap.graph.annotations.RelatedGetValue;
@@ -192,4 +193,38 @@ public class TurnComponent extends PipelineComponent {
                else
                        return new double[]{1.0,0.0,0.0};
        }
+       
+       /**
+        * Turn is a section of a circle; this method returns center of that circle.
+        * @return
+        */
+       public Vector3d getCenterPosition() {
+           // getPosition() is control point position, located outside of turn
+        // we need to calculate position in the middle of the turn
+        double t = Math.tan((Math.PI - getControlPoint().getTurnAngle()) * 0.5);
+        double tr = getTurnRadius();
+        double R = 0.0;
+        if (t > MathTools.NEAR_ZERO)
+            R = tr / t;
+        Vector3d localC = new Vector3d(-R, 0.0, -tr);
+        // worldC is center of a circle
+        Vector3d worldC = NodeTools.getWorldPosition(this,localC);
+        return worldC;
+       }
+       
+       /**
+        * Returns position in the middle of turn component
+        * @return
+        */
+       public Vector3d getMiddlePosition() {
+           Vector3d pos = getWorldPosition();
+           Vector3d worldC = getCenterPosition();
+           
+           // calculate vector from center to edge, which is middle of the turn
+        pos.sub(worldC);
+        pos.normalize();
+        pos.scale(getTurnRadius());
+        pos.add(worldC);
+        return pos;
+       }
 }