ConstraintDetector / Snapping tool for pipeline routing

[simantics/3d.git] / org.simantics.plant3d / src / org / simantics / plant3d / scenegraph / controlpoint / PipeControlPoint.java

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 cb4b57eb898eefcb69d2797ccb3a182570d6c2a9..93186779e60d580aa31d23a95585ce4886ff9f7b 100644 (file)
--- a/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipeControlPoint.java
+++ b/org.simantics.plant3d/src/org/simantics/plant3d/scenegraph/controlpoint/PipeControlPoint.java
@@ -167,6 +167,10 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                return children.size() == 1 && children.get(0).isDualSub();
        }
 
+       public boolean isAxial() {
+               return isInline() && !isDualInline();
+       }
+
        public boolean isSizeChange() {
                return isSizeChange;
                //              if (children.size() == 0)
@@ -483,51 +487,6 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                return q1;
        }
 
-       public Vector3d getDirection(Direction direction) {
-               if (isDirected())
-                       return getDirectedControlPointDirection();
-               if (isTurn() && isFixed()) {
-                       if (direction == Direction.NEXT) {
-                               if (previous != null) {
-                                       PipeControlPoint pcp = this;
-                                       Vector3d dir = new Vector3d();
-                                       dir.sub(pcp.getWorldPosition(),previous.getWorldPosition());
-                                       if (dir.lengthSquared() > MathTools.NEAR_ZERO)
-                                               dir.normalize();
-                                       else
-                                               return null;
-                                       Quat4d q = getControlPointOrientationQuat(dir, pcp.getRotationAngle() != null ? pcp.getRotationAngle() : 0.0);
-                                       AxisAngle4d aa = new AxisAngle4d(MathTools.Y_AXIS,pcp.getTurnAngle() == null ? 0.0 : pcp.getTurnAngle());
-                                       Quat4d q2 = MathTools.getQuat(aa);
-                                       Vector3d v = new Vector3d(1.,0.,0.);
-                                       Vector3d offset = new Vector3d();
-                                       MathTools.rotate(q2, v, offset);
-                                       MathTools.rotate(q, offset, dir);
-                                       return dir;
-                               }
-                       } else {
-                               if (next != null) {
-                                       PipeControlPoint pcp = this;
-                                       Vector3d dir = new Vector3d();
-                                       dir.sub(next.getWorldPosition(),pcp.getWorldPosition());
-                                       if (dir.lengthSquared() > MathTools.NEAR_ZERO)
-                                               dir.normalize();
-                                       else
-                                               return null;
-                                       Quat4d q = getControlPointOrientationQuat(dir, pcp.getRotationAngle() != null ? pcp.getRotationAngle() : 0.0);
-                                       AxisAngle4d aa = new AxisAngle4d(MathTools.Y_AXIS,pcp.getTurnAngle() == null ? 0.0 : pcp.getTurnAngle());
-                                       Quat4d q2 = MathTools.getQuat(aa);
-                                       Vector3d v = new Vector3d(1.,0.,0.);
-                                       Vector3d offset = new Vector3d();
-                                       MathTools.rotate(q2, v, offset);
-                                       MathTools.rotate(q, offset, dir);
-                                       return dir;
-                               }
-                       }
-               }
-               return null;
-       }
-
        public void insert(PipeControlPoint previous, PipeControlPoint next) {
                // inserting an offsetpoint is error, 
                if (isDualSub())
@@ -663,6 +622,51 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                dir.normalize();
                return dir;
        }
+       
+       public Vector3d getDirection(Direction direction) {
+        if (isDirected())
+            return getDirectedControlPointDirection();
+        if (isTurn() && isFixed()) {
+            if (direction == Direction.NEXT) {
+                if (previous != null) {
+                    PipeControlPoint pcp = this;
+                    Vector3d dir = new Vector3d();
+                    dir.sub(pcp.getWorldPosition(),previous.getWorldPosition());
+                    if (dir.lengthSquared() > MathTools.NEAR_ZERO)
+                        dir.normalize();
+                    else
+                        return null;
+                    Quat4d q = getControlPointOrientationQuat(dir, pcp.getRotationAngle() != null ? pcp.getRotationAngle() : 0.0);
+                    AxisAngle4d aa = new AxisAngle4d(MathTools.Y_AXIS,pcp.getTurnAngle() == null ? 0.0 : pcp.getTurnAngle());
+                    Quat4d q2 = MathTools.getQuat(aa);
+                    Vector3d v = new Vector3d(1.,0.,0.);
+                    Vector3d offset = new Vector3d();
+                    MathTools.rotate(q2, v, offset);
+                    MathTools.rotate(q, offset, dir);
+                    return dir;
+                }
+            } else {
+                if (next != null) {
+                    PipeControlPoint pcp = this;
+                    Vector3d dir = new Vector3d();
+                    dir.sub(next.getWorldPosition(),pcp.getWorldPosition());
+                    if (dir.lengthSquared() > MathTools.NEAR_ZERO)
+                        dir.normalize();
+                    else
+                        return null;
+                    Quat4d q = getControlPointOrientationQuat(dir, pcp.getRotationAngle() != null ? pcp.getRotationAngle() : 0.0);
+                    AxisAngle4d aa = new AxisAngle4d(MathTools.Y_AXIS,pcp.getTurnAngle() == null ? 0.0 : pcp.getTurnAngle());
+                    Quat4d q2 = MathTools.getQuat(aa);
+                    Vector3d v = new Vector3d(1.,0.,0.);
+                    Vector3d offset = new Vector3d();
+                    MathTools.rotate(q2, v, offset);
+                    MathTools.rotate(q, offset, dir);
+                    return dir;
+                }
+            }
+        }
+        return null;
+    }
 
        public Vector3d getPathLegDirection(Direction direction) {
                if (direction == Direction.NEXT) {
@@ -749,21 +753,25 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
        public void getInlineControlPointEnds(Tuple3d p1, Tuple3d p2) {
                assert (isInline());
 
-               Vector3d pos = getWorldPosition();
-               Vector3d dir = getPathLegDirection(Direction.NEXT);
+               PipeControlPoint sub = isAxial() ? this : getSubPoint().get(0);
+               Vector3d pos = getWorldPosition(), pos2 = sub == this ? pos : sub.getWorldPosition();
+               Vector3d dir = sub.getPathLegDirection(Direction.NEXT);
+               
                dir.normalize();
                dir.scale(length * 0.5);
                p1.set(pos);
-               p2.set(pos);
+               p2.set(pos2);
                p1.sub(dir);
                p2.add(dir);
        }
 
        public void getControlPointEnds(Tuple3d p1, Tuple3d p2) {
-               Vector3d pos = getWorldPosition();
+               PipeControlPoint sub = isAxial() ? this : getSubPoint().get(0);
+               Vector3d pos = getWorldPosition(), pos2 = sub == this ? pos : sub.getWorldPosition();
+               
                Vector3d dir1 = getPathLegDirection(Direction.PREVIOUS);
                dir1.normalize();
-               Vector3d dir2 = getPathLegDirection(Direction.NEXT);
+               Vector3d dir2 = sub.getPathLegDirection(Direction.NEXT);
                dir2.normalize();
                if (isInline()) {
                        dir1.scale(length * 0.5);
@@ -773,11 +781,22 @@ public class PipeControlPoint extends G3DNode implements IP3DNode {
                        dir2.scale(length);
                }
                p1.set(pos);
-               p2.set(pos);
+               p2.set(pos2);
                p1.add(dir1);
                p2.add(dir2);
        }
 
+       public void getEndDirections(Tuple3d v1, Tuple3d v2) {
+               PipeControlPoint sub = isAxial() ? this : getSubPoint().get(0);
+               
+               Vector3d dir1 = getPathLegDirection(Direction.PREVIOUS);
+               dir1.normalize();
+               Vector3d dir2 = sub.getPathLegDirection(Direction.NEXT);
+               dir2.normalize();
+               v1.set(dir1);
+               v2.set(dir2);
+       }
+
        public void getInlineControlPointEnds(Tuple3d p1, Tuple3d p2, Vector3d dir) {
                assert (isInline());