From 27fb679643fab3ef9aa46d04c5ced2ea82cf60c2 Mon Sep 17 00:00:00 2001 From: Marko Luukkainen Date: Wed, 16 Oct 2019 18:51:50 +0300 Subject: [PATCH] ConstraintDetector / Snapping tool for pipeline routing gitlab #32 Change-Id: I4c5304bc481eedfe853ffbfd2b2157397c52e920 --- .../org/simantics/g3d/vtk/shape/vtkShape.java | 37 + .../g3d/vtk/swt/ConstraintDetector.java | 87 ++ .../g3d/tools/ConstraintDetector.java | 838 +++++++++--------- .../plant3d/actions/RoutePipeAction.java | 34 +- .../controlpoint/PipeControlPoint.java | 90 +- 5 files changed, 619 insertions(+), 467 deletions(-) create mode 100644 org.simantics.g3d.vtk/src/org/simantics/g3d/vtk/swt/ConstraintDetector.java 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 8157dfe3..99a9e6e8 100644 --- 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 index 00000000..fa2f6961 --- /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 parts = new ArrayList(); + + 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 fe8a4b2d..51e1a7c3 100644 --- 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 @@ -/******************************************************************************* - * 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 { - - private static final int X = 0; - private static final int Y = 1; - private static final int Z = 2; - - -// private ThreeDimensionalEditorBase editor; - //private G3DNode constraintReference = null; - private IG3DNode constraintReference = null; - private ArrayList constraintPoints = new ArrayList(); - private ArrayList constraintDirections = new ArrayList(); -// private MaterialState ms; - - private Color4d xColor = new Color4d(1.f,0.f,0.f,1.f); - private Color4d yColor = new Color4d(0.f,1.f,0.f,1.f); - private 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 getConstraintPoints() { - return constraintPoints; - } - - public ArrayList 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 = ""; - -// private ArrayList constraintHighlights = new ArrayList(); - - 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 distances = new ArrayList(); - List snapPoints = new ArrayList(); - List snapStrings = new ArrayList(); - List snapColors = new ArrayList(); - 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 p1s = new ArrayList(); - List p2s = new ArrayList(); - List axes = new ArrayList(); - - 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; - } +/******************************************************************************* + * 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 constraintPoints = new ArrayList(); + private ArrayList constraintDirections = new ArrayList(); +// 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 getConstraintPoints() { + return constraintPoints; + } + + public ArrayList 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 constraintHighlights = new ArrayList(); + + 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 distances = new ArrayList(); + List snapPoints = new ArrayList(); + List snapStrings = new ArrayList(); + List snapColors = new ArrayList(); + 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 p1s = new ArrayList(); + List p2s = new ArrayList(); + List axes = new ArrayList(); + + 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 24b6a14b..9773a9ea 100644 --- 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 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 6e6a595b..93186779 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 @@ -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) { -- 2.47.1