ConstraintDetector / Snapping tool for pipeline routing

gitlab #32

Change-Id: I4c5304bc481eedfe853ffbfd2b2157397c52e920

diff --git a/org.simantics.g3d.vtk/src/org/simantics/g3d/vtk/shape/vtkShape.java b/org.simantics.g3d.vtk/src/org/simantics/g3d/vtk/shape/vtkShape.java
index 8157dfe36c86cd5c8e9b56851277c47e67b11489..99a9e6e8eedbbb3b74947b4889e2009b92647058 100644 (file)
--- a/org.simantics.g3d.vtk/src/org/simantics/g3d/vtk/shape/vtkShape.java
+++ b/org.simantics.g3d.vtk/src/org/simantics/g3d/vtk/shape/vtkShape.java
@@ -11,6 +11,7 @@
  *******************************************************************************/
 package org.simantics.g3d.vtk.shape;
 
+import javax.vecmath.Point3d;
 import javax.vecmath.Tuple3d;
 
 import vtk.vtkActor;
@@ -191,5 +192,41 @@ public class vtkShape {
                
                return aLineActor;
        }
+       
+       public static vtkActor createLineActor(Tuple3d... p) {
+        vtkPoints linePoints = new vtkPoints();
+        linePoints.SetNumberOfPoints(p.length);
+        
+        for (int i = 0; i < p.length; i++) {
+            Tuple3d p1 = p[i];
+            linePoints.InsertPoint(i,p1.x, p1.y, p1.z);
+            
+        }
+        vtkUnstructuredGrid aLineGrid = new vtkUnstructuredGrid();
+        //aLineGrid.Allocate(1, 1);
+        for (int i = 0; i< p.length -1; i++) {
+            vtkLine aLine = new vtkLine();
+            aLine.GetPointIds().SetId(0, i);
+            aLine.GetPointIds().SetId(1, i+1);
+            aLineGrid.InsertNextCell(aLine.GetCellType(), aLine.GetPointIds());
+            aLine.GetPointIds().Delete();
+            aLine.Delete();
+        }
+        
+        aLineGrid.SetPoints(linePoints);
+        vtkDataSetMapper aLineMapper = new vtkDataSetMapper();
+        aLineMapper.SetInputData(aLineGrid);
+        vtkActor aLineActor = new vtkActor();
+        aLineActor.SetMapper(aLineMapper);
+        //aLineActor.GetProperty().SetDiffuseColor(.2, 1, 1);
+        
+        linePoints.Delete();
+        
+        aLineGrid.Delete();
+        aLineMapper.Delete();
+        
+        return aLineActor;
+    }
 
 }
+

diff --git a/org.simantics.g3d.vtk/src/org/simantics/g3d/vtk/swt/ConstraintDetector.java b/org.simantics.g3d.vtk/src/org/simantics/g3d/vtk/swt/ConstraintDetector.java
new file mode 100644 (file)
index 0000000..fa2f696
--- /dev/null
+++ b/org.simantics.g3d.vtk/src/org/simantics/g3d/vtk/swt/ConstraintDetector.java
@@ -0,0 +1,87 @@
+package org.simantics.g3d.vtk.swt;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import javax.vecmath.Point3d;
+
+import org.simantics.g3d.shape.Color4d;
+import org.simantics.g3d.vtk.common.VtkView;
+import org.simantics.g3d.vtk.shape.vtkShape;
+
+import vtk.vtkActor;
+import vtk.vtkProp;
+
+public class ConstraintDetector extends org.simantics.g3d.tools.ConstraintDetector {
+
+    VtkView renderingPart;
+    
+    private List<vtkProp> parts = new ArrayList<vtkProp>();
+    
+    public ConstraintDetector(VtkView renderingPart) {
+        this.renderingPart = renderingPart;
+    }
+    
+    @Override
+    protected void addConstrainLineHighlight(Point3d p1, Point3d p2, Color4d color) {
+        double w = 3;
+        
+        vtkActor lineActorX = vtkShape.createLineActor(p1, p2);
+        lineActorX.GetProperty().SetColor(color.x, color.y, color.z);
+        lineActorX.GetProperty().SetLineWidth(w);
+        lineActorX.GetProperty().Delete();
+        parts.add(lineActorX);
+        renderingPart.lock();
+        renderingPart.getRenderer().AddActor(lineActorX);
+        renderingPart.unlock();
+    }
+    
+    @Override
+    protected void addConstrainPlaneHighlight(Point3d p1, Point3d p2, int axis) {
+        double w = 3;
+        Point3d p12 = new Point3d();
+        Color4d color = xColor;
+        switch (axis) {
+            case ConstraintDetector.X:
+                p12.x = p1.x;
+                p12.y = p1.y;
+                p12.z = p2.z;
+                color = xColor;
+                break;
+            case ConstraintDetector.Y:
+                p12.x = p1.x;
+                p12.y = p1.y;
+                p12.z = p2.z;
+                color = yColor;
+                break;
+            case ConstraintDetector.Z:
+                p12.x = p1.x;
+                p12.y = p2.y;
+                p12.z = p2.z;
+                color = zColor;
+                break;
+        }
+        vtkActor lineActorX = vtkShape.createLineActor(p1, p12, p2);
+        lineActorX.GetProperty().SetColor(color.x, color.y, color.z);
+        lineActorX.GetProperty().SetLineWidth(w);
+        lineActorX.GetProperty().Delete();
+        parts.add(lineActorX);
+        renderingPart.lock();
+        renderingPart.getRenderer().AddActor(lineActorX);
+        renderingPart.unlock();
+    }
+    
+    @Override
+    public void clearConstraintHighlights() {
+        clearSnapString();
+        if (parts.size() == 0)
+            return;
+        renderingPart.lock();
+        for (vtkProp p : parts) {
+            renderingPart.getRenderer().RemoveActor(p);
+            p.Delete();
+        }
+        parts.clear();
+        renderingPart.unlock();
+    }
+}

diff --git a/org.simantics.g3d/src/org/simantics/g3d/tools/ConstraintDetector.java b/org.simantics.g3d/src/org/simantics/g3d/tools/ConstraintDetector.java
index fe8a4b2d2e2f1c86da15b9a52152397d0842098b..51e1a7c3345f38353b32b5cbe7f92b016343d608 100644 (file)
--- a/org.simantics.g3d/src/org/simantics/g3d/tools/ConstraintDetector.java
+++ b/org.simantics.g3d/src/org/simantics/g3d/tools/ConstraintDetector.java
@@ -1,418 +1,422 @@
-/*******************************************************************************\r
- * Copyright (c) 2012, 2013 Association for Decentralized Information Management in\r
- * Industry THTH ry.\r
- * All rights reserved. This program and the accompanying materials\r
- * are made available under the terms of the Eclipse Public License v1.0\r
- * which accompanies this distribution, and is available at\r
- * http://www.eclipse.org/legal/epl-v10.html\r
- *\r
- * Contributors:\r
- *     VTT Technical Research Centre of Finland - initial API and implementation\r
- *******************************************************************************/\r
-package org.simantics.g3d.tools;\r
-\r
-import java.util.ArrayList;\r
-import java.util.List;\r
-\r
-import javax.vecmath.Point3d;\r
-import javax.vecmath.Vector3d;\r
-\r
-import org.simantics.g3d.math.MathTools;\r
-import org.simantics.g3d.scenegraph.IG3DNode;\r
-import org.simantics.g3d.shape.Color4d;\r
-\r
-public abstract class ConstraintDetector {\r
-       \r
-       private static final int X = 0;\r
-       private static final int Y = 1;\r
-       private static final int Z = 2;\r
-       \r
-       \r
-//    private ThreeDimensionalEditorBase editor;\r
-       //private G3DNode constraintReference = null;\r
-       private IG3DNode constraintReference = null;\r
-       private ArrayList<Point3d> constraintPoints = new ArrayList<Point3d>();\r
-       private ArrayList<Vector3d> constraintDirections = new ArrayList<Vector3d>();\r
-//    private MaterialState ms;\r
-       \r
-       private Color4d xColor = new Color4d(1.f,0.f,0.f,1.f);\r
-       private Color4d yColor = new Color4d(0.f,1.f,0.f,1.f);\r
-       private Color4d zColor = new Color4d(0.f,0.f,1.f,1.f);\r
-       \r
-       \r
-//    public ConstraintDetector(ThreeDimensionalEditorBase editor) {\r
-//        this.editor = editor;\r
-//        ms = editor.getRenderingComponent().getDisplaySystem().getRenderer().createMaterialState();\r
-//        ms.setEmissive(new ColorRGBA(1.f,1.f,1.f,1.f));\r
-//        ms.setColorMaterial(MaterialState.CM_EMISSIVE);\r
-//    }\r
-       \r
-\r
-       public void clearConstraints() {\r
-               //System.out.println("ConstraintDetector.clearConstraints()");\r
-               constraintPoints.clear();\r
-               constraintDirections.clear();\r
-       }\r
-       \r
-       private void updateConstraints() {\r
-               clearConstraints();\r
-               if (constraintReference == null)\r
-                       return;\r
-               Constraint c = (Constraint)constraintReference.getAdapter(Constraint.class);\r
-               if (c == null)\r
-                       return;\r
-               constraintPoints.addAll(c.points);\r
-               constraintDirections.addAll(c.dirs);\r
-       }\r
-       \r
-       \r
-       public ArrayList<Point3d> getConstraintPoints() {\r
-               return constraintPoints;\r
-       }\r
-       \r
-       public ArrayList<Vector3d> getConstraintDirections() {\r
-               return constraintDirections;\r
-       }\r
-\r
-       public void updateConstraintReference(IG3DNode node) {\r
-               if (constraintReference != null && !constraintReference.equals(node)) {\r
-                       constraintReference = node;\r
-                       updateConstraints();\r
-               } else if (node != null){\r
-                       constraintReference = node;\r
-                       updateConstraints();\r
-               }\r
-               \r
-       }\r
-       \r
-       public void addContraintPoint(Point3d p) {\r
-               //System.out.println("ConstraintDetector.addConstraintPoint() " + p);\r
-               constraintPoints.add(p);\r
-       }\r
-       \r
-       public void addContraintDirection(Vector3d v) {\r
-               //System.out.println("ConstraintDetector.addConstraintDirection() " + v);\r
-               constraintDirections.add(v);\r
-       }\r
-       \r
-       private double snapAngle = 0.1;\r
-       private String snapString = "";\r
-\r
-//    private ArrayList<Geometry> constraintHighlights = new ArrayList<Geometry>();\r
-       \r
-       public Point3d getSnappedPoint(Vector3d pickPoint, Vector3d pickDir, Vector3d requestedPoint) {\r
-               \r
-               \r
-               Vector3d snappedPoint = new Vector3d();\r
-               Vector3d t = new Vector3d();\r
-               Point3d currentPoint = null;\r
-               // TODO : snap to closest angle\r
-               for (Vector3d constraintDir : constraintDirections) {\r
-                       \r
-                       MathTools.intersectStraightStraight(pickPoint,pickDir, requestedPoint, constraintDir, t, snappedPoint);\r
-                       t.sub(snappedPoint);\r
-                       if (t.lengthSquared() < snapAngle) {\r
-                               \r
-                               snapString += "Angle snap ";\r
-                               currentPoint = new Point3d(snappedPoint);\r
-                               break;\r
-                       }\r
-               }\r
-               if (currentPoint != null) {\r
-                       Vector3d dir = new Vector3d(currentPoint);\r
-                       dir.sub(requestedPoint);\r
-                       Point3d p = getPointSnap(requestedPoint, dir);\r
-                       if (p != null)\r
-                               currentPoint = p;\r
-               } else {\r
-                       List<Double> distances = new ArrayList<Double>();\r
-                       List<Point3d> snapPoints = new ArrayList<Point3d>();\r
-                       List<String> snapStrings = new ArrayList<String>();\r
-                       List<Color4d> snapColors = new ArrayList<Color4d>();\r
-                       for (Point3d constraintPoint : constraintPoints) {\r
-                               distances.clear();\r
-                               snapPoints.clear();\r
-                               snapStrings.clear();\r
-                               MathTools.intersectStraightStraight(new Vector3d(constraintPoint), new Vector3d(1.0, 0.0, 0.0),\r
-                                               pickPoint, pickDir, snappedPoint, t);\r
-                               t.sub(snappedPoint);\r
-                               double distance = t.lengthSquared();\r
-                               if (distance < snapAngle) {\r
-                                       distances.add(distance);\r
-                                       snapPoints.add(new Point3d(snappedPoint));\r
-                                       snapStrings.add("Point x-snap ");\r
-                                       snapColors.add(xColor);\r
-                               }\r
-                               MathTools.intersectStraightStraight(new Vector3d(constraintPoint), new Vector3d(0.0, 1.0, 0.0),\r
-                                               pickPoint, pickDir, snappedPoint, t);\r
-                               t.sub(snappedPoint);\r
-                               distance = t.lengthSquared();\r
-                               if (distance < snapAngle) {\r
-                                       distances.add(distance);\r
-                                       snapPoints.add(new Point3d(snappedPoint));\r
-                                       snapStrings.add("Point y-snap ");\r
-                                       snapColors.add(yColor);\r
-                               }\r
-                               MathTools.intersectStraightStraight(new Vector3d(constraintPoint), new Vector3d(0.0, 0.0, 1.0),\r
-                                               pickPoint, pickDir, snappedPoint, t);\r
-                               t.sub(snappedPoint);\r
-                               distance = t.lengthSquared();\r
-                               if (distance < snapAngle) {\r
-                                       distances.add(distance);\r
-                                       snapPoints.add(new Point3d(snappedPoint));\r
-                                       snapStrings.add("Point z-snap ");\r
-                                       snapColors.add(zColor);\r
-                                       \r
-                               }\r
-                               if (distances.size() > 0) {\r
-                                       if (distances.size() > 1) {\r
-                                               // more than one axes snape\r
-                                               Vector3d ref = MathTools.closestPointOnStraight(constraintPoint, new Point3d(pickPoint), pickDir);\r
-                                               ref.sub(constraintPoint);\r
-                                               distance = ref.lengthSquared();\r
-                                               if (distance < snapAngle) {\r
-                                                       // we are close enought to point, so we'll just snap there\r
-                                                       currentPoint = new Point3d(constraintPoint);\r
-                                                       snapString += "Point snap ";\r
-                                               } else {\r
-                                                       // select the closest of axes snap to\r
-                                                       int min = 0;\r
-                                                       for (int i = 1; i < distances.size(); i++) {\r
-                                                               if (distances.get(i) < distances.get(min))\r
-                                                                       min = i;\r
-                                                       }\r
-                                                       currentPoint = snapPoints.get(min);\r
-                                                       addConstrainLineHighlight(currentPoint, constraintPoint,snapColors.get(min));\r
-                                                       snapString += snapStrings.get(min);\r
-                                               }\r
-                                       } else {\r
-                                               // only one of the axes snaps\r
-                                               currentPoint = snapPoints.get(0);\r
-                                               addConstrainLineHighlight(currentPoint, constraintPoint,snapColors.get(0));\r
-                                               snapString += snapStrings.get(0);\r
-                                       }\r
-                                       break;\r
-                               }\r
-                       }\r
-               }\r
-               return currentPoint;\r
-\r
-       }\r
-       \r
-       public abstract void clearConstraintHighlights();\r
-       protected abstract void addConstrainLineHighlight(Point3d p1, Point3d p2, Color4d color);\r
-       protected abstract void addConstrainPlaneHighlight(Point3d p1, Point3d p2, int axis);\r
-       \r
-//    public void clearConstraintHighlights() {\r
-//        snapString = "";\r
-//\r
-//        for (Geometry s : constraintHighlights)\r
-//            s.removeFromParent();\r
-//        \r
-//        constraintHighlights.clear();\r
-//    }\r
-//    \r
-//    private void addConstrainLineHighlight(Point3d p1, Point3d p2, Color4d color) {\r
-//\r
-//     float coord[] = new float[6];\r
-//     ColorRGBA colors[] = new ColorRGBA[2];\r
-//     colors[0] = color;\r
-//     colors[1] = color;\r
-//     coord[0] = (float)p1.x;\r
-//     coord[1] = (float)p1.y;\r
-//     coord[2] = (float)p1.z;\r
-//     coord[3] = (float)p2.x;\r
-//     coord[4] = (float)p2.y;\r
-//     coord[5] = (float)p2.z;\r
-//     Line shape = new Line("",BufferUtils.createFloatBuffer(coord),null,BufferUtils.createFloatBuffer(colors),null);\r
-//     editor.getRenderingComponent().getNoShadowRoot().attachChild(shape);\r
-//     shape.setRenderState(ms);\r
-//     constraintHighlights.add(shape);\r
-//    }\r
-//    \r
-//    private void addConstrainPlaneHighlight(Point3d p1, Point3d p2, int axis) {\r
-//\r
-//     float coord[] = new float[9];\r
-//     ColorRGBA colors[] = new ColorRGBA[3];\r
-//     coord[0] = (float)p1.x;\r
-//     coord[1] = (float)p1.y;\r
-//     coord[2] = (float)p1.z;\r
-//     switch (axis) {\r
-//     case X:\r
-//             coord[3] = (float)p1.x;\r
-//             coord[4] = (float)p1.y;\r
-//             coord[5] = (float)p2.z;\r
-//             colors[0] = colors[1] = colors[2] = xColor;\r
-//             break;\r
-//     case Y:\r
-//             coord[3] = (float)p1.x;\r
-//             coord[4] = (float)p1.y;\r
-//             coord[5] = (float)p2.z;\r
-//             colors[0] = colors[1] = colors[2] = yColor;\r
-//             break;\r
-//     case Z:\r
-//             coord[3] = (float)p1.x;\r
-//             coord[4] = (float)p2.y;\r
-//             coord[5] = (float)p2.z;\r
-//             colors[0] = colors[1] = colors[2] = zColor;\r
-//             break;\r
-//     \r
-//     }\r
-//     coord[6] = (float)p2.x;\r
-//     coord[7] = (float)p2.y;\r
-//     coord[8] = (float)p2.z;\r
-//     Line shape = new Line("",BufferUtils.createFloatBuffer(coord),null,BufferUtils.createFloatBuffer(colors),null);\r
-//     shape.setMode(Line.CONNECTED);\r
-//     editor.getRenderingComponent().getNoShadowRoot().attachChild(shape);\r
-//     shape.setRenderState(ms);\r
-//        constraintHighlights.add(shape);\r
-//    }\r
-       \r
-       /**\r
-        * Snaps position to axis-aligned planes defined by constraint points\r
-        * Form of position is p+v, meaning that the position that is snapped is requestedPoint + requestedDir\r
-        * @param requestedPoint one part of the position to be snapped\r
-        * @param requestedDir second part of the position to be snapped and direction that the position is allowed to move\r
-        * @return\r
-        */\r
-       public Point3d getPointSnap(Vector3d requestedPoint, Vector3d requestedDir) {\r
-               \r
-               Vector3d snappedPoint = new Vector3d();\r
-               Point3d currentPoint = null;\r
-               double u[] = new double[1];\r
-               List<Point3d> p1s = new ArrayList<Point3d>();\r
-               List<Point3d> p2s = new ArrayList<Point3d>();\r
-               List<Integer> axes = new ArrayList<Integer>();\r
-               \r
-               for (Point3d constraintPoint : constraintPoints) {\r
-                       boolean snap = false;\r
-                       \r
-                       if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), getAxialVector(X), snappedPoint,u) && Math.abs(1.0 - u[0]) < snapAngle) {\r
-                               currentPoint = new Point3d(snappedPoint);\r
-                               //snapString += "Point/Plane x-snap ";\r
-                               snap = true;\r
-                               //addConstrainPlaneHighlight(constraintPoint, currentPoint,X);\r
-                               p1s.add(constraintPoint);\r
-                               p2s.add(currentPoint);\r
-                               axes.add(X);\r
-                       }\r
-                       \r
-                       if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), getAxialVector(Y), snappedPoint,u) && Math.abs(1.0 - u[0]) < snapAngle) {\r
-                               currentPoint = new Point3d(snappedPoint);\r
-                               //snapString += "Point/Plane y-snap ";\r
-                               snap = true;\r
-                               //addConstrainPlaneHighlight(constraintPoint, currentPoint,Y);\r
-                               p1s.add(constraintPoint);\r
-                               p2s.add(currentPoint);\r
-                               axes.add(Y);\r
-                       }\r
-                       \r
-                       if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), getAxialVector(Z), snappedPoint,u) && Math.abs(1.0 - u[0]) < snapAngle) {\r
-                               currentPoint = new Point3d(snappedPoint);\r
-                               //snapString += "Point/Plane z-snap ";\r
-                               snap = true;\r
-                               //addConstrainPlaneHighlight(constraintPoint, currentPoint,Z);\r
-                               p1s.add(constraintPoint);\r
-                               p2s.add(currentPoint);\r
-                               axes.add(Z);\r
-                       }\r
-                       if (snap)\r
-                               break;\r
-               }\r
-               if (p1s.size() == 0)\r
-                       return null;\r
-               if (p1s.size() == 1) {\r
-                       snapString += "Point/Plane ";\r
-                       switch (axes.get(0)) {\r
-                       case X:\r
-                               snapString += "x";\r
-                               break;\r
-                       case Y:\r
-                               snapString += "y";\r
-                               break;\r
-                       case Z:\r
-                               snapString += "z";\r
-                               break;\r
-                       }\r
-                       snapString += "-snap ";\r
-                       addConstrainPlaneHighlight(p1s.get(0), p2s.get(0),axes.get(0));\r
-                       return currentPoint;\r
-               } else if (p1s.size() == 3){\r
-                       // all axial planes are intersecting, snapping point must be the constraint point\r
-                       // all constraint points are the same, so just pick the first in the list\r
-                       snapString += "Point/Point ";\r
-                       return p1s.get(0);\r
-               } else {\r
-                       Vector3d dir = new Vector3d();\r
-                       dir.cross(getAxialVector(axes.get(0)), getAxialVector(axes.get(1)));\r
-                       currentPoint = new Point3d(MathTools.closestPointOnStraight(currentPoint, p1s.get(0), dir));\r
-                       addConstrainLineHighlight(p1s.get(0), currentPoint, xColor);\r
-                       snapString += "Point/Line ";\r
-                       return currentPoint;\r
-               }\r
-               \r
-       }\r
-       \r
-       private Vector3d getAxialVector(int axis) {\r
-               switch (axis) {\r
-               case X:\r
-                       return new Vector3d(1.0,0.0,0.0);\r
-               case Y:\r
-                       return new Vector3d(0.0,1.0,0.0);\r
-               case Z:\r
-                       return new Vector3d(0.0,0.0,1.0);\r
-               }\r
-               throw new RuntimeException("Unknown axis " + axis);     \r
-       }\r
-       \r
-       /**\r
-        * Snaps the position to axis-aligned planes defined by constraint points\r
-        * @param requestedPoint point that is snapped\r
-        * @param requestedDir direction that point is allowed to move\r
-        * @return\r
-        */\r
-       \r
-       public Point3d getPointSnap2(Vector3d requestedPoint, Vector3d requestedDir) {\r
-               \r
-               Vector3d snappedPoint = new Vector3d();\r
-               Point3d currentPoint = null;\r
-               double u[] = new double[1];\r
-               //System.out.println(requestedPoint + "  " + requestedDir);\r
-               for (Point3d constraintPoint : constraintPoints) {\r
-                       boolean snap = false;\r
-                       //System.out.print(constraintPoint + " ");\r
-                       if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), new Vector3d(1.0,0.0,0.0), snappedPoint,u) && Math.abs(u[0]) < snapAngle) {\r
-                               currentPoint = new Point3d(snappedPoint);\r
-                               snapString += "Point/Plane x-snap ";\r
-                               snap = true;\r
-                               addConstrainPlaneHighlight(constraintPoint, currentPoint,X);\r
-                               //System.out.print(" x " + u[0]);\r
-                       }\r
-                       \r
-                       if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), new Vector3d(0.0,1.0,0.0), snappedPoint,u) && Math.abs(u[0]) < snapAngle) {\r
-                               currentPoint = new Point3d(snappedPoint);\r
-                               snapString += "Point/Plane y-snap ";\r
-                               snap = true;\r
-                               addConstrainPlaneHighlight(constraintPoint, currentPoint,Y);\r
-                               //System.out.print(" y " + u[0]);\r
-                       }\r
-                       \r
-                       \r
-                       if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), new Vector3d(0.0,0.0,1.0), snappedPoint,u) && Math.abs(u[0]) < snapAngle) {\r
-                               currentPoint = new Point3d(snappedPoint);\r
-                               snapString += "Point/Plane z-snap ";\r
-                               snap = true;\r
-                               addConstrainPlaneHighlight(constraintPoint, currentPoint,Z);\r
-                               //System.out.print(" z " + u[0]);\r
-                       }\r
-                       //System.out.println();\r
-                       if (snap)\r
-                               break;\r
-               }\r
-               return currentPoint;\r
-       }\r
-       \r
-       public String getSnapString() {\r
-               return snapString;\r
-       }\r
+/*******************************************************************************
+ * Copyright (c) 2012, 2013 Association for Decentralized Information Management in
+ * Industry THTH ry.
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * which accompanies this distribution, and is available at
+ * http://www.eclipse.org/legal/epl-v10.html
+ *
+ * Contributors:
+ *     VTT Technical Research Centre of Finland - initial API and implementation
+ *******************************************************************************/
+package org.simantics.g3d.tools;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import javax.vecmath.Point3d;
+import javax.vecmath.Vector3d;
+
+import org.simantics.g3d.math.MathTools;
+import org.simantics.g3d.scenegraph.IG3DNode;
+import org.simantics.g3d.shape.Color4d;
+
+public abstract class ConstraintDetector {
+       
+       public static final int X = 0;
+       public static final int Y = 1;
+       public static final int Z = 2;
+       
+       
+//    private ThreeDimensionalEditorBase editor;
+       //private G3DNode constraintReference = null;
+       private IG3DNode constraintReference = null;
+       private ArrayList<Point3d> constraintPoints = new ArrayList<Point3d>();
+       private ArrayList<Vector3d> constraintDirections = new ArrayList<Vector3d>();
+//    private MaterialState ms;
+       
+       protected Color4d xColor = new Color4d(1.f,0.f,0.f,1.f);
+       protected Color4d yColor = new Color4d(0.f,1.f,0.f,1.f);
+       protected Color4d zColor = new Color4d(0.f,0.f,1.f,1.f);
+       
+       
+//    public ConstraintDetector(ThreeDimensionalEditorBase editor) {
+//        this.editor = editor;
+//        ms = editor.getRenderingComponent().getDisplaySystem().getRenderer().createMaterialState();
+//        ms.setEmissive(new ColorRGBA(1.f,1.f,1.f,1.f));
+//        ms.setColorMaterial(MaterialState.CM_EMISSIVE);
+//    }
+       
+
+       public void clearConstraints() {
+               //System.out.println("ConstraintDetector.clearConstraints()");
+               constraintPoints.clear();
+               constraintDirections.clear();
+       }
+       
+       private void updateConstraints() {
+               clearConstraints();
+               if (constraintReference == null)
+                       return;
+               Constraint c = (Constraint)constraintReference.getAdapter(Constraint.class);
+               if (c == null)
+                       return;
+               constraintPoints.addAll(c.points);
+               constraintDirections.addAll(c.dirs);
+       }
+       
+       
+       public ArrayList<Point3d> getConstraintPoints() {
+               return constraintPoints;
+       }
+       
+       public ArrayList<Vector3d> getConstraintDirections() {
+               return constraintDirections;
+       }
+
+       public void updateConstraintReference(IG3DNode node) {
+               if (constraintReference != null && !constraintReference.equals(node)) {
+                       constraintReference = node;
+                       updateConstraints();
+               } else if (node != null){
+                       constraintReference = node;
+                       updateConstraints();
+               }
+               
+       }
+       
+       public void addContraintPoint(Point3d p) {
+               //System.out.println("ConstraintDetector.addConstraintPoint() " + p);
+               constraintPoints.add(p);
+       }
+       
+       public void addContraintDirection(Vector3d v) {
+               //System.out.println("ConstraintDetector.addConstraintDirection() " + v);
+               constraintDirections.add(v);
+       }
+       
+       private double snapAngle = 0.1;
+       private String snapString = "";
+       
+       protected void clearSnapString() {
+           snapString = "";
+       }
+
+//    private ArrayList<Geometry> constraintHighlights = new ArrayList<Geometry>();
+       
+       public Point3d getSnappedPoint(Vector3d pickPoint, Vector3d pickDir, Vector3d requestedPoint) {
+               
+               
+               Vector3d snappedPoint = new Vector3d();
+               Vector3d t = new Vector3d();
+               Point3d currentPoint = null;
+               // TODO : snap to closest angle
+               for (Vector3d constraintDir : constraintDirections) {
+                       
+                       MathTools.intersectStraightStraight(pickPoint,pickDir, requestedPoint, constraintDir, t, snappedPoint);
+                       t.sub(snappedPoint);
+                       if (t.lengthSquared() < snapAngle) {
+                               
+                               snapString += "Angle snap ";
+                               currentPoint = new Point3d(snappedPoint);
+                               break;
+                       }
+               }
+               if (currentPoint != null) {
+                       Vector3d dir = new Vector3d(currentPoint);
+                       dir.sub(requestedPoint);
+                       Point3d p = getPointSnap(requestedPoint, dir);
+                       if (p != null)
+                               currentPoint = p;
+               } else {
+                       List<Double> distances = new ArrayList<Double>();
+                       List<Point3d> snapPoints = new ArrayList<Point3d>();
+                       List<String> snapStrings = new ArrayList<String>();
+                       List<Color4d> snapColors = new ArrayList<Color4d>();
+                       for (Point3d constraintPoint : constraintPoints) {
+                               distances.clear();
+                               snapPoints.clear();
+                               snapStrings.clear();
+                               MathTools.intersectStraightStraight(new Vector3d(constraintPoint), new Vector3d(1.0, 0.0, 0.0),
+                                               pickPoint, pickDir, snappedPoint, t);
+                               t.sub(snappedPoint);
+                               double distance = t.lengthSquared();
+                               if (distance < snapAngle) {
+                                       distances.add(distance);
+                                       snapPoints.add(new Point3d(snappedPoint));
+                                       snapStrings.add("Point x-snap ");
+                                       snapColors.add(xColor);
+                               }
+                               MathTools.intersectStraightStraight(new Vector3d(constraintPoint), new Vector3d(0.0, 1.0, 0.0),
+                                               pickPoint, pickDir, snappedPoint, t);
+                               t.sub(snappedPoint);
+                               distance = t.lengthSquared();
+                               if (distance < snapAngle) {
+                                       distances.add(distance);
+                                       snapPoints.add(new Point3d(snappedPoint));
+                                       snapStrings.add("Point y-snap ");
+                                       snapColors.add(yColor);
+                               }
+                               MathTools.intersectStraightStraight(new Vector3d(constraintPoint), new Vector3d(0.0, 0.0, 1.0),
+                                               pickPoint, pickDir, snappedPoint, t);
+                               t.sub(snappedPoint);
+                               distance = t.lengthSquared();
+                               if (distance < snapAngle) {
+                                       distances.add(distance);
+                                       snapPoints.add(new Point3d(snappedPoint));
+                                       snapStrings.add("Point z-snap ");
+                                       snapColors.add(zColor);
+                                       
+                               }
+                               if (distances.size() > 0) {
+                                       if (distances.size() > 1) {
+                                               // more than one axes snape
+                                               Vector3d ref = MathTools.closestPointOnStraight(constraintPoint, new Point3d(pickPoint), pickDir);
+                                               ref.sub(constraintPoint);
+                                               distance = ref.lengthSquared();
+                                               if (distance < snapAngle) {
+                                                       // we are close enought to point, so we'll just snap there
+                                                       currentPoint = new Point3d(constraintPoint);
+                                                       snapString += "Point snap ";
+                                               } else {
+                                                       // select the closest of axes snap to
+                                                       int min = 0;
+                                                       for (int i = 1; i < distances.size(); i++) {
+                                                               if (distances.get(i) < distances.get(min))
+                                                                       min = i;
+                                                       }
+                                                       currentPoint = snapPoints.get(min);
+                                                       addConstrainLineHighlight(currentPoint, constraintPoint,snapColors.get(min));
+                                                       snapString += snapStrings.get(min);
+                                               }
+                                       } else {
+                                               // only one of the axes snaps
+                                               currentPoint = snapPoints.get(0);
+                                               addConstrainLineHighlight(currentPoint, constraintPoint,snapColors.get(0));
+                                               snapString += snapStrings.get(0);
+                                       }
+                                       break;
+                               }
+                       }
+               }
+               return currentPoint;
+
+       }
+       
+       public abstract void clearConstraintHighlights();
+       protected abstract void addConstrainLineHighlight(Point3d p1, Point3d p2, Color4d color);
+       protected abstract void addConstrainPlaneHighlight(Point3d p1, Point3d p2, int axis);
+       
+//    public void clearConstraintHighlights() {
+//        snapString = "";
+//
+//        for (Geometry s : constraintHighlights)
+//            s.removeFromParent();
+//        
+//        constraintHighlights.clear();
+//    }
+//    
+//    private void addConstrainLineHighlight(Point3d p1, Point3d p2, Color4d color) {
+//
+//     float coord[] = new float[6];
+//     ColorRGBA colors[] = new ColorRGBA[2];
+//     colors[0] = color;
+//     colors[1] = color;
+//     coord[0] = (float)p1.x;
+//     coord[1] = (float)p1.y;
+//     coord[2] = (float)p1.z;
+//     coord[3] = (float)p2.x;
+//     coord[4] = (float)p2.y;
+//     coord[5] = (float)p2.z;
+//     Line shape = new Line("",BufferUtils.createFloatBuffer(coord),null,BufferUtils.createFloatBuffer(colors),null);
+//     editor.getRenderingComponent().getNoShadowRoot().attachChild(shape);
+//     shape.setRenderState(ms);
+//     constraintHighlights.add(shape);
+//    }
+//    
+//    private void addConstrainPlaneHighlight(Point3d p1, Point3d p2, int axis) {
+//
+//     float coord[] = new float[9];
+//     ColorRGBA colors[] = new ColorRGBA[3];
+//     coord[0] = (float)p1.x;
+//     coord[1] = (float)p1.y;
+//     coord[2] = (float)p1.z;
+//     switch (axis) {
+//     case X:
+//             coord[3] = (float)p1.x;
+//             coord[4] = (float)p1.y;
+//             coord[5] = (float)p2.z;
+//             colors[0] = colors[1] = colors[2] = xColor;
+//             break;
+//     case Y:
+//             coord[3] = (float)p1.x;
+//             coord[4] = (float)p1.y;
+//             coord[5] = (float)p2.z;
+//             colors[0] = colors[1] = colors[2] = yColor;
+//             break;
+//     case Z:
+//             coord[3] = (float)p1.x;
+//             coord[4] = (float)p2.y;
+//             coord[5] = (float)p2.z;
+//             colors[0] = colors[1] = colors[2] = zColor;
+//             break;
+//     
+//     }
+//     coord[6] = (float)p2.x;
+//     coord[7] = (float)p2.y;
+//     coord[8] = (float)p2.z;
+//     Line shape = new Line("",BufferUtils.createFloatBuffer(coord),null,BufferUtils.createFloatBuffer(colors),null);
+//     shape.setMode(Line.CONNECTED);
+//     editor.getRenderingComponent().getNoShadowRoot().attachChild(shape);
+//     shape.setRenderState(ms);
+//        constraintHighlights.add(shape);
+//    }
+       
+       /**
+        * Snaps position to axis-aligned planes defined by constraint points
+        * Form of position is p+v, meaning that the position that is snapped is requestedPoint + requestedDir
+        * @param requestedPoint one part of the position to be snapped
+        * @param requestedDir second part of the position to be snapped and direction that the position is allowed to move
+        * @return
+        */
+       public Point3d getPointSnap(Vector3d requestedPoint, Vector3d requestedDir) {
+               
+               Vector3d snappedPoint = new Vector3d();
+               Point3d currentPoint = null;
+               double u[] = new double[1];
+               List<Point3d> p1s = new ArrayList<Point3d>();
+               List<Point3d> p2s = new ArrayList<Point3d>();
+               List<Integer> axes = new ArrayList<Integer>();
+               
+               for (Point3d constraintPoint : constraintPoints) {
+                       boolean snap = false;
+                       
+                       if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), getAxialVector(X), snappedPoint,u) && Math.abs(1.0 - u[0]) < snapAngle) {
+                               currentPoint = new Point3d(snappedPoint);
+                               //snapString += "Point/Plane x-snap ";
+                               snap = true;
+                               //addConstrainPlaneHighlight(constraintPoint, currentPoint,X);
+                               p1s.add(constraintPoint);
+                               p2s.add(currentPoint);
+                               axes.add(X);
+                       }
+                       
+                       if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), getAxialVector(Y), snappedPoint,u) && Math.abs(1.0 - u[0]) < snapAngle) {
+                               currentPoint = new Point3d(snappedPoint);
+                               //snapString += "Point/Plane y-snap ";
+                               snap = true;
+                               //addConstrainPlaneHighlight(constraintPoint, currentPoint,Y);
+                               p1s.add(constraintPoint);
+                               p2s.add(currentPoint);
+                               axes.add(Y);
+                       }
+                       
+                       if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), getAxialVector(Z), snappedPoint,u) && Math.abs(1.0 - u[0]) < snapAngle) {
+                               currentPoint = new Point3d(snappedPoint);
+                               //snapString += "Point/Plane z-snap ";
+                               snap = true;
+                               //addConstrainPlaneHighlight(constraintPoint, currentPoint,Z);
+                               p1s.add(constraintPoint);
+                               p2s.add(currentPoint);
+                               axes.add(Z);
+                       }
+                       if (snap)
+                               break;
+               }
+               if (p1s.size() == 0)
+                       return null;
+               if (p1s.size() == 1) {
+                       snapString += "Point/Plane ";
+                       switch (axes.get(0)) {
+                       case X:
+                               snapString += "x";
+                               break;
+                       case Y:
+                               snapString += "y";
+                               break;
+                       case Z:
+                               snapString += "z";
+                               break;
+                       }
+                       snapString += "-snap ";
+                       addConstrainPlaneHighlight(p1s.get(0), p2s.get(0),axes.get(0));
+                       return currentPoint;
+               } else if (p1s.size() == 3){
+                       // all axial planes are intersecting, snapping point must be the constraint point
+                       // all constraint points are the same, so just pick the first in the list
+                       snapString += "Point/Point ";
+                       return p1s.get(0);
+               } else {
+                       Vector3d dir = new Vector3d();
+                       dir.cross(getAxialVector(axes.get(0)), getAxialVector(axes.get(1)));
+                       currentPoint = new Point3d(MathTools.closestPointOnStraight(currentPoint, p1s.get(0), dir));
+                       addConstrainLineHighlight(p1s.get(0), currentPoint, xColor);
+                       snapString += "Point/Line ";
+                       return currentPoint;
+               }
+               
+       }
+       
+       private Vector3d getAxialVector(int axis) {
+               switch (axis) {
+               case X:
+                       return new Vector3d(1.0,0.0,0.0);
+               case Y:
+                       return new Vector3d(0.0,1.0,0.0);
+               case Z:
+                       return new Vector3d(0.0,0.0,1.0);
+               }
+               throw new RuntimeException("Unknown axis " + axis);     
+       }
+       
+       /**
+        * Snaps the position to axis-aligned planes defined by constraint points
+        * @param requestedPoint point that is snapped
+        * @param requestedDir direction that point is allowed to move
+        * @return
+        */
+       
+       public Point3d getPointSnap2(Vector3d requestedPoint, Vector3d requestedDir) {
+               
+               Vector3d snappedPoint = new Vector3d();
+               Point3d currentPoint = null;
+               double u[] = new double[1];
+               //System.out.println(requestedPoint + "  " + requestedDir);
+               for (Point3d constraintPoint : constraintPoints) {
+                       boolean snap = false;
+                       //System.out.print(constraintPoint + " ");
+                       if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), new Vector3d(1.0,0.0,0.0), snappedPoint,u) && Math.abs(u[0]) < snapAngle) {
+                               currentPoint = new Point3d(snappedPoint);
+                               snapString += "Point/Plane x-snap ";
+                               snap = true;
+                               addConstrainPlaneHighlight(constraintPoint, currentPoint,X);
+                               //System.out.print(" x " + u[0]);
+                       }
+                       
+                       if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), new Vector3d(0.0,1.0,0.0), snappedPoint,u) && Math.abs(u[0]) < snapAngle) {
+                               currentPoint = new Point3d(snappedPoint);
+                               snapString += "Point/Plane y-snap ";
+                               snap = true;
+                               addConstrainPlaneHighlight(constraintPoint, currentPoint,Y);
+                               //System.out.print(" y " + u[0]);
+                       }
+                       
+                       
+                       if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), new Vector3d(0.0,0.0,1.0), snappedPoint,u) && Math.abs(u[0]) < snapAngle) {
+                               currentPoint = new Point3d(snappedPoint);
+                               snapString += "Point/Plane z-snap ";
+                               snap = true;
+                               addConstrainPlaneHighlight(constraintPoint, currentPoint,Z);
+                               //System.out.print(" z " + u[0]);
+                       }
+                       //System.out.println();
+                       if (snap)
+                               break;
+               }
+               return currentPoint;
+       }
+       
+       public String getSnapString() {
+               return snapString;
+       }
 }
\ No newline at end of file

diff --git a/org.simantics.plant3d/src/org/simantics/plant3d/actions/RoutePipeAction.java b/org.simantics.plant3d/src/org/simantics/plant3d/actions/RoutePipeAction.java
index 24b6a14bdfcef66fd5c1df8876fa6c14ac2fe0ff..9773a9eae8c77a5c3c3a1bf350ffadaee6a0dc39 100644 (file)
--- a/org.simantics.plant3d/src/org/simantics/plant3d/actions/RoutePipeAction.java
+++ b/org.simantics.plant3d/src/org/simantics/plant3d/actions/RoutePipeAction.java
@@ -69,7 +69,7 @@ public class RoutePipeAction extends vtkSwtAction {
        private enum ToolState{NOT_ACTIVE, INITIALIZING, SELECTING_POSITION, SELECTING_SPLIT, ROUTING};
        private ToolState state = ToolState.NOT_ACTIVE;
        
-       private ConstraintDetector detector = new DummyConstraintDetector();
+       private ConstraintDetector detector;// = new DummyConstraintDetector();
        
        private boolean useDefault = false;
        private Vector3d direction = null;
@@ -95,6 +95,7 @@ public class RoutePipeAction extends vtkSwtAction {
                nodeMap = root.getNodeMap();
                splitPointSelectionGizmo = new SplitPointSelectionGizmo(panel);
                terminalSelectionGizmo = new TerminalSelectionGizmo(panel);
+               detector = new org.simantics.g3d.vtk.swt.ConstraintDetector(panel);
        }
        
        public void setComponent(PipelineComponent component) {
@@ -529,7 +530,7 @@ public class RoutePipeAction extends vtkSwtAction {
                //                    selectionLine = null;
                                                }
                                        } else if (e.getButton() ==MouseEvent.BUTTON2){
-               //                detector.updateConstraintReference();
+                                           updateConstraints();
                                        } else if (e.getButton() == MouseEvent.BUTTON3){      
                                                endPiping();
                                        }
@@ -595,6 +596,27 @@ public class RoutePipeAction extends vtkSwtAction {
                return true;
        }
        
+       private void updateConstraints() {
+           detector.clearConstraints();
+           if (hoverObject == null) {
+               return;
+           }
+           if (hoverObject instanceof Nozzle) {
+            Nozzle n = (Nozzle)hoverObject;
+            detector.addContraintPoint(new Point3d(n.getWorldPosition()));
+           } else if (hoverObject instanceof InlineComponent) {
+               InlineComponent c = (InlineComponent)hoverObject;
+               Point3d p1 = new Point3d();
+               Point3d p2 = new Point3d();
+               c.getEnds(p1, p2);
+               detector.addContraintPoint(p1);
+               detector.addContraintPoint(p2);
+           } else if (hoverObject instanceof TurnComponent) {
+               TurnComponent n = (TurnComponent)hoverObject;
+            detector.addContraintPoint(new Point3d(n.getWorldPosition()));
+           }
+       }
+       
        @Override
        public boolean mouseMoved(MouseEvent e) {
                if (useDefault) {
@@ -627,7 +649,7 @@ public class RoutePipeAction extends vtkSwtAction {
                return nodes;
        }
        
-
+       INode hoverObject = null;
 
        private void updateRouting(double x, double y) {
 //             if(input.keyPressed(KeyEvent.VK_ESCAPE)) {
@@ -660,12 +682,14 @@ public class RoutePipeAction extends vtkSwtAction {
                
                
                
-               INode hoverObject = null;
+               
                
                List<INode> hover = isOverNode((int)x,(int)y);
                if (hover.size() > 0) {
                        hoverObject = hover.get(0);
-               } 
+               } else {
+                   hoverObject = null;
+               }
 //             System.out.println(hoverObject + " " + getLast());
                if (hoverObject != null) {
                        if (hoverObject.equals(getLast()) ) {

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 6e6a595bc3cefddd0fdab8864efec31666ea3618..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
@@ -487,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())
@@ -667,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) {