X-Git-Url: https://gerrit.simantics.org/r/gitweb?a=blobdiff_plain;f=org.simantics.g3d%2Fsrc%2Forg%2Fsimantics%2Fg3d%2Ftools%2FConstraintDetector.java;fp=org.simantics.g3d%2Fsrc%2Forg%2Fsimantics%2Fg3d%2Ftools%2FConstraintDetector.java;h=fe8a4b2d2e2f1c86da15b9a52152397d0842098b;hb=53d55c24c779745f188bdb18d32f71d20acb61b2;hp=6af54146b47ed726af2d6bf0d6faf61645e3dc07;hpb=f36217aeeb09c0c46f99886ee99772156ce9cfe6;p=simantics%2F3d.git

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 6af54146..fe8a4b2d 100644
--- a/org.simantics.g3d/src/org/simantics/g3d/tools/ConstraintDetector.java
+++ b/org.simantics.g3d/src/org/simantics/g3d/tools/ConstraintDetector.java
@@ -22,187 +22,187 @@ 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 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<Point3d> constraintPoints = new ArrayList<Point3d>();
-    private ArrayList<Vector3d> constraintDirections = new ArrayList<Vector3d>();
+	//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;
-    
-    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);
-    
-    
+	
+	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<Point3d> getConstraintPoints() {
-        return constraintPoints;
-    }
-    
-    public ArrayList<Vector3d> getConstraintDirections() {
-        return constraintDirections;
-    }
+	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 = "";
+	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<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 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 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 = "";
 //
@@ -267,152 +267,152 @@ public abstract class ConstraintDetector {
 //    	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;
-    }
+	
+	/**
+	 * 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