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 }