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