org.simantics.proconf.g3d/src/org/simantics/proconf/g3d/base/ConstraintDetector.java

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