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
+ \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 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
+ \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
\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
+ 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
+ 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
+ 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
+ }\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
// 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
+ \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
}
\ No newline at end of file