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

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