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

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