org.simantics.g3d/src/org/simantics/g3d/tools/ConstraintDetector.java

   1 package org.simantics.g3d.tools;\r
   2 \r
   3 import java.util.ArrayList;\r
   4 import java.util.List;\r
   5 \r
   6 import javax.vecmath.Point3d;\r
   7 import javax.vecmath.Vector3d;\r
   8 \r
   9 import org.simantics.g3d.math.MathTools;\r
  10 import org.simantics.g3d.scenegraph.IG3DNode;\r
  11 import org.simantics.g3d.shape.Color4d;\r
  12 \r
  13 public abstract class ConstraintDetector {\r
  14     \r
  15     private static final int X = 0;\r
  16     private static final int Y = 1;\r
  17     private static final int Z = 2;\r
  18     \r
  19     \r
  20 //    private ThreeDimensionalEditorBase editor;\r
  21     //private G3DNode constraintReference = null;\r
  22     private IG3DNode constraintReference = null;\r
  23     private ArrayList<Point3d> constraintPoints = new ArrayList<Point3d>();\r
  24     private ArrayList<Vector3d> constraintDirections = new ArrayList<Vector3d>();\r
  25 //    private MaterialState ms;\r
  26     \r
  27     private Color4d xColor = new Color4d(1.f,0.f,0.f,1.f);\r
  28     private Color4d yColor = new Color4d(0.f,1.f,0.f,1.f);\r
  29     private Color4d zColor = new Color4d(0.f,0.f,1.f,1.f);\r
  30     \r
  31     \r
  32 //    public ConstraintDetector(ThreeDimensionalEditorBase editor) {\r
  33 //        this.editor = editor;\r
  34 //        ms = editor.getRenderingComponent().getDisplaySystem().getRenderer().createMaterialState();\r
  35 //        ms.setEmissive(new ColorRGBA(1.f,1.f,1.f,1.f));\r
  36 //        ms.setColorMaterial(MaterialState.CM_EMISSIVE);\r
  37 //    }\r
  38     \r
  39 \r
  40     public void clearConstraints() {\r
  41         //System.out.println("ConstraintDetector.clearConstraints()");\r
  42         constraintPoints.clear();\r
  43         constraintDirections.clear();\r
  44     }\r
  45     \r
  46     private void updateConstraints() {\r
  47         clearConstraints();\r
  48         if (constraintReference == null)\r
  49             return;\r
  50         Constraint c = (Constraint)constraintReference.getAdapter(Constraint.class);\r
  51         if (c == null)\r
  52                 return;\r
  53         constraintPoints.addAll(c.points);\r
  54         constraintDirections.addAll(c.dirs);\r
  55     }\r
  56     \r
  57     \r
  58     public ArrayList<Point3d> getConstraintPoints() {\r
  59         return constraintPoints;\r
  60     }\r
  61     \r
  62     public ArrayList<Vector3d> getConstraintDirections() {\r
  63         return constraintDirections;\r
  64     }\r
  65 \r
  66     public void updateConstraintReference(IG3DNode node) {\r
  67         if (constraintReference != null && !constraintReference.equals(node)) {\r
  68                 constraintReference = node;\r
  69                 updateConstraints();\r
  70         } else if (node != null){\r
  71                 constraintReference = node;\r
  72                 updateConstraints();\r
  73         }\r
  74         \r
  75     }\r
  76     \r
  77     public void addContraintPoint(Point3d p) {\r
  78         //System.out.println("ConstraintDetector.addConstraintPoint() " + p);\r
  79         constraintPoints.add(p);\r
  80     }\r
  81     \r
  82     public void addContraintDirection(Vector3d v) {\r
  83         //System.out.println("ConstraintDetector.addConstraintDirection() " + v);\r
  84         constraintDirections.add(v);\r
  85     }\r
  86     \r
  87     private double snapAngle = 0.1;\r
  88     private String snapString = "";\r
  89 \r
  90 //    private ArrayList<Geometry> constraintHighlights = new ArrayList<Geometry>();\r
  91     \r
  92     public Point3d getSnappedPoint(Vector3d pickPoint, Vector3d pickDir, Vector3d requestedPoint) {\r
  93         \r
  94         \r
  95         Vector3d snappedPoint = new Vector3d();\r
  96         Vector3d t = new Vector3d();\r
  97         Point3d currentPoint = null;\r
  98         // TODO : snap to closest angle\r
  99         for (Vector3d constraintDir : constraintDirections) {\r
 100             \r
 101             MathTools.intersectStraightStraight(pickPoint,pickDir, requestedPoint, constraintDir, t, snappedPoint);\r
 102             t.sub(snappedPoint);\r
 103             if (t.lengthSquared() < snapAngle) {\r
 104                 \r
 105                 snapString += "Angle snap ";\r
 106                 currentPoint = new Point3d(snappedPoint);\r
 107                 break;\r
 108             }\r
 109         }\r
 110         if (currentPoint != null) {\r
 111             Vector3d dir = new Vector3d(currentPoint);\r
 112             dir.sub(requestedPoint);\r
 113             Point3d p = getPointSnap(requestedPoint, dir);\r
 114             if (p != null)\r
 115                 currentPoint = p;\r
 116         } else {\r
 117                 List<Double> distances = new ArrayList<Double>();\r
 118                 List<Point3d> snapPoints = new ArrayList<Point3d>();\r
 119                 List<String> snapStrings = new ArrayList<String>();\r
 120                 List<Color4d> snapColors = new ArrayList<Color4d>();\r
 121             for (Point3d constraintPoint : constraintPoints) {\r
 122                 distances.clear();\r
 123                 snapPoints.clear();\r
 124                 snapStrings.clear();\r
 125                 MathTools.intersectStraightStraight(new Vector3d(constraintPoint), new Vector3d(1.0, 0.0, 0.0),\r
 126                         pickPoint, pickDir, snappedPoint, t);\r
 127                 t.sub(snappedPoint);\r
 128                 double distance = t.lengthSquared();\r
 129                 if (distance < snapAngle) {\r
 130                         distances.add(distance);\r
 131                         snapPoints.add(new Point3d(snappedPoint));\r
 132                         snapStrings.add("Point x-snap ");\r
 133                         snapColors.add(xColor);\r
 134                 }\r
 135                 MathTools.intersectStraightStraight(new Vector3d(constraintPoint), new Vector3d(0.0, 1.0, 0.0),\r
 136                         pickPoint, pickDir, snappedPoint, t);\r
 137                 t.sub(snappedPoint);\r
 138                 distance = t.lengthSquared();\r
 139                 if (distance < snapAngle) {\r
 140                         distances.add(distance);\r
 141                         snapPoints.add(new Point3d(snappedPoint));\r
 142                         snapStrings.add("Point y-snap ");\r
 143                         snapColors.add(yColor);\r
 144                 }\r
 145                 MathTools.intersectStraightStraight(new Vector3d(constraintPoint), new Vector3d(0.0, 0.0, 1.0),\r
 146                         pickPoint, pickDir, snappedPoint, t);\r
 147                 t.sub(snappedPoint);\r
 148                 distance = t.lengthSquared();\r
 149                 if (distance < snapAngle) {\r
 150                         distances.add(distance);\r
 151                         snapPoints.add(new Point3d(snappedPoint));\r
 152                         snapStrings.add("Point z-snap ");\r
 153                         snapColors.add(zColor);\r
 154                     \r
 155                 }\r
 156                 if (distances.size() > 0) {\r
 157                     if (distances.size() > 1) {\r
 158                         // more than one axes snape\r
 159                         Vector3d ref = MathTools.closestPointOnStraight(constraintPoint, new Point3d(pickPoint), pickDir);\r
 160                         ref.sub(constraintPoint);\r
 161                         distance = ref.lengthSquared();\r
 162                         if (distance < snapAngle) {\r
 163                                 // we are close enought to point, so we'll just snap there\r
 164                                 currentPoint = new Point3d(constraintPoint);\r
 165                                 snapString += "Point snap ";\r
 166                         } else {\r
 167                                 // select the closest of axes snap to\r
 168                                 int min = 0;\r
 169                                 for (int i = 1; i < distances.size(); i++) {\r
 170                                         if (distances.get(i) < distances.get(min))\r
 171                                                 min = i;\r
 172                                 }\r
 173                                 currentPoint = snapPoints.get(min);\r
 174                                 addConstrainLineHighlight(currentPoint, constraintPoint,snapColors.get(min));\r
 175                                 snapString += snapStrings.get(min);\r
 176                         }\r
 177                     } else {\r
 178                         // only one of the axes snaps\r
 179                         currentPoint = snapPoints.get(0);\r
 180                         addConstrainLineHighlight(currentPoint, constraintPoint,snapColors.get(0));\r
 181                         snapString += snapStrings.get(0);\r
 182                     }\r
 183                     break;\r
 184                 }\r
 185             }\r
 186         }\r
 187         return currentPoint;\r
 188 \r
 189     }\r
 190     \r
 191     public abstract void clearConstraintHighlights();\r
 192     protected abstract void addConstrainLineHighlight(Point3d p1, Point3d p2, Color4d color);\r
 193     protected abstract void addConstrainPlaneHighlight(Point3d p1, Point3d p2, int axis);\r
 194     \r
 195 //    public void clearConstraintHighlights() {\r
 196 //        snapString = "";\r
 197 //\r
 198 //        for (Geometry s : constraintHighlights)\r
 199 //            s.removeFromParent();\r
 200 //        \r
 201 //        constraintHighlights.clear();\r
 202 //    }\r
 203 //    \r
 204 //    private void addConstrainLineHighlight(Point3d p1, Point3d p2, Color4d color) {\r
 205 //\r
 206 //      float coord[] = new float[6];\r
 207 //      ColorRGBA colors[] = new ColorRGBA[2];\r
 208 //      colors[0] = color;\r
 209 //      colors[1] = color;\r
 210 //      coord[0] = (float)p1.x;\r
 211 //      coord[1] = (float)p1.y;\r
 212 //      coord[2] = (float)p1.z;\r
 213 //      coord[3] = (float)p2.x;\r
 214 //      coord[4] = (float)p2.y;\r
 215 //      coord[5] = (float)p2.z;\r
 216 //      Line shape = new Line("",BufferUtils.createFloatBuffer(coord),null,BufferUtils.createFloatBuffer(colors),null);\r
 217 //      editor.getRenderingComponent().getNoShadowRoot().attachChild(shape);\r
 218 //      shape.setRenderState(ms);\r
 219 //      constraintHighlights.add(shape);\r
 220 //    }\r
 221 //    \r
 222 //    private void addConstrainPlaneHighlight(Point3d p1, Point3d p2, int axis) {\r
 223 //\r
 224 //      float coord[] = new float[9];\r
 225 //      ColorRGBA colors[] = new ColorRGBA[3];\r
 226 //      coord[0] = (float)p1.x;\r
 227 //      coord[1] = (float)p1.y;\r
 228 //      coord[2] = (float)p1.z;\r
 229 //      switch (axis) {\r
 230 //      case X:\r
 231 //              coord[3] = (float)p1.x;\r
 232 //              coord[4] = (float)p1.y;\r
 233 //              coord[5] = (float)p2.z;\r
 234 //              colors[0] = colors[1] = colors[2] = xColor;\r
 235 //              break;\r
 236 //      case Y:\r
 237 //              coord[3] = (float)p1.x;\r
 238 //              coord[4] = (float)p1.y;\r
 239 //              coord[5] = (float)p2.z;\r
 240 //              colors[0] = colors[1] = colors[2] = yColor;\r
 241 //              break;\r
 242 //      case Z:\r
 243 //              coord[3] = (float)p1.x;\r
 244 //              coord[4] = (float)p2.y;\r
 245 //              coord[5] = (float)p2.z;\r
 246 //              colors[0] = colors[1] = colors[2] = zColor;\r
 247 //              break;\r
 248 //      \r
 249 //      }\r
 250 //      coord[6] = (float)p2.x;\r
 251 //      coord[7] = (float)p2.y;\r
 252 //      coord[8] = (float)p2.z;\r
 253 //      Line shape = new Line("",BufferUtils.createFloatBuffer(coord),null,BufferUtils.createFloatBuffer(colors),null);\r
 254 //      shape.setMode(Line.CONNECTED);\r
 255 //      editor.getRenderingComponent().getNoShadowRoot().attachChild(shape);\r
 256 //      shape.setRenderState(ms);\r
 257 //        constraintHighlights.add(shape);\r
 258 //    }\r
 259     \r
 260     /**\r
 261      * Snaps position to axis-aligned planes defined by constraint points\r
 262      * Form of position is p+v, meaning that the position that is snapped is requestedPoint + requestedDir\r
 263      * @param requestedPoint one part of the position to be snapped\r
 264      * @param requestedDir second part of the position to be snapped and direction that the position is allowed to move\r
 265      * @return\r
 266      */\r
 267     public Point3d getPointSnap(Vector3d requestedPoint, Vector3d requestedDir) {\r
 268         \r
 269         Vector3d snappedPoint = new Vector3d();\r
 270         Point3d currentPoint = null;\r
 271         double u[] = new double[1];\r
 272         List<Point3d> p1s = new ArrayList<Point3d>();\r
 273         List<Point3d> p2s = new ArrayList<Point3d>();\r
 274         List<Integer> axes = new ArrayList<Integer>();\r
 275         \r
 276         for (Point3d constraintPoint : constraintPoints) {\r
 277             boolean snap = false;\r
 278             \r
 279             if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), getAxialVector(X), snappedPoint,u) && Math.abs(1.0 - u[0]) < snapAngle) {\r
 280                 currentPoint = new Point3d(snappedPoint);\r
 281                 //snapString += "Point/Plane x-snap ";\r
 282                 snap = true;\r
 283                 //addConstrainPlaneHighlight(constraintPoint, currentPoint,X);\r
 284                 p1s.add(constraintPoint);\r
 285                 p2s.add(currentPoint);\r
 286                 axes.add(X);\r
 287             }\r
 288             \r
 289             if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), getAxialVector(Y), snappedPoint,u) && Math.abs(1.0 - u[0]) < snapAngle) {\r
 290                 currentPoint = new Point3d(snappedPoint);\r
 291                 //snapString += "Point/Plane y-snap ";\r
 292                 snap = true;\r
 293                 //addConstrainPlaneHighlight(constraintPoint, currentPoint,Y);\r
 294                 p1s.add(constraintPoint);\r
 295                 p2s.add(currentPoint);\r
 296                 axes.add(Y);\r
 297             }\r
 298             \r
 299             if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), getAxialVector(Z), snappedPoint,u) && Math.abs(1.0 - u[0]) < snapAngle) {\r
 300                 currentPoint = new Point3d(snappedPoint);\r
 301                 //snapString += "Point/Plane z-snap ";\r
 302                 snap = true;\r
 303                 //addConstrainPlaneHighlight(constraintPoint, currentPoint,Z);\r
 304                 p1s.add(constraintPoint);\r
 305                 p2s.add(currentPoint);\r
 306                 axes.add(Z);\r
 307             }\r
 308             if (snap)\r
 309                 break;\r
 310         }\r
 311         if (p1s.size() == 0)\r
 312                 return null;\r
 313         if (p1s.size() == 1) {\r
 314                 snapString += "Point/Plane ";\r
 315                 switch (axes.get(0)) {\r
 316                 case X:\r
 317                         snapString += "x";\r
 318                         break;\r
 319                 case Y:\r
 320                         snapString += "y";\r
 321                         break;\r
 322                 case Z:\r
 323                         snapString += "z";\r
 324                         break;\r
 325                 }\r
 326                 snapString += "-snap ";\r
 327                 addConstrainPlaneHighlight(p1s.get(0), p2s.get(0),axes.get(0));\r
 328                 return currentPoint;\r
 329         } else if (p1s.size() == 3){\r
 330                 // all axial planes are intersecting, snapping point must be the constraint point\r
 331                 // all constraint points are the same, so just pick the first in the list\r
 332                 snapString += "Point/Point ";\r
 333                 return p1s.get(0);\r
 334         } else {\r
 335                 Vector3d dir = new Vector3d();\r
 336                 dir.cross(getAxialVector(axes.get(0)), getAxialVector(axes.get(1)));\r
 337                 currentPoint = new Point3d(MathTools.closestPointOnStraight(currentPoint, p1s.get(0), dir));\r
 338                 addConstrainLineHighlight(p1s.get(0), currentPoint, xColor);\r
 339                 snapString += "Point/Line ";\r
 340                 return currentPoint;\r
 341         }\r
 342         \r
 343     }\r
 344     \r
 345     private Vector3d getAxialVector(int axis) {\r
 346         switch (axis) {\r
 347         case X:\r
 348                 return new Vector3d(1.0,0.0,0.0);\r
 349         case Y:\r
 350                 return new Vector3d(0.0,1.0,0.0);\r
 351         case Z:\r
 352                 return new Vector3d(0.0,0.0,1.0);\r
 353         }\r
 354         throw new RuntimeException("Unknown axis " + axis);     \r
 355     }\r
 356     \r
 357     /**\r
 358      * Snaps the position to axis-aligned planes defined by constraint points\r
 359      * @param requestedPoint point that is snapped\r
 360      * @param requestedDir direction that point is allowed to move\r
 361      * @return\r
 362      */\r
 363     \r
 364     public Point3d getPointSnap2(Vector3d requestedPoint, Vector3d requestedDir) {\r
 365         \r
 366         Vector3d snappedPoint = new Vector3d();\r
 367         Point3d currentPoint = null;\r
 368         double u[] = new double[1];\r
 369         //System.out.println(requestedPoint + "  " + requestedDir);\r
 370         for (Point3d constraintPoint : constraintPoints) {\r
 371             boolean snap = false;\r
 372             //System.out.print(constraintPoint + " ");\r
 373             if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), new Vector3d(1.0,0.0,0.0), snappedPoint,u) && Math.abs(u[0]) < snapAngle) {\r
 374                 currentPoint = new Point3d(snappedPoint);\r
 375                 snapString += "Point/Plane x-snap ";\r
 376                 snap = true;\r
 377                 addConstrainPlaneHighlight(constraintPoint, currentPoint,X);\r
 378                 //System.out.print(" x " + u[0]);\r
 379             }\r
 380             \r
 381             if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), new Vector3d(0.0,1.0,0.0), snappedPoint,u) && Math.abs(u[0]) < snapAngle) {\r
 382                 currentPoint = new Point3d(snappedPoint);\r
 383                 snapString += "Point/Plane y-snap ";\r
 384                 snap = true;\r
 385                 addConstrainPlaneHighlight(constraintPoint, currentPoint,Y);\r
 386                 //System.out.print(" y " + u[0]);\r
 387             }\r
 388            \r
 389             \r
 390             if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), new Vector3d(0.0,0.0,1.0), snappedPoint,u) && Math.abs(u[0]) < snapAngle) {\r
 391                 currentPoint = new Point3d(snappedPoint);\r
 392                 snapString += "Point/Plane z-snap ";\r
 393                 snap = true;\r
 394                 addConstrainPlaneHighlight(constraintPoint, currentPoint,Z);\r
 395                 //System.out.print(" z " + u[0]);\r
 396             }\r
 397             //System.out.println();\r
 398             if (snap)\r
 399                 break;\r
 400         }\r
 401         return currentPoint;\r
 402     }\r
 403     \r
 404     public String getSnapString() {\r
 405         return snapString;\r
 406     }\r
 407 }