List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D getY
public double getY()
From source file:com.mapr.synth.drive.Producer.java
@Override public void run() { Random rand = new Random(3); GeoPoint start = new GeoPoint((rand.nextDouble() - 0.5) * Math.PI / 2, rand.nextDouble() * Math.PI * 2); final Vector3D east = start.east(); final Vector3D north = start.north(east); GeoPoint end = new GeoPoint(start.as3D().add(east.scalarMultiply(-12.0 / Constants.EARTH_RADIUS_KM)) .add(north.scalarMultiply(7.0 / Constants.EARTH_RADIUS_KM))); Vector3D zz = project(east, north, end.as3D()); System.out.printf("==> %.2f %.2f\n", zz.getX(), zz.getY()); while (true) { double t = 0; final Car car = new Car(); System.out.printf("%.2f\n", start.distance(end)); car.driveTo(rand, t, start, end, new Car.Callback() { @Override//from w w w . jav a 2 s.com void call(double t, Engine eng, GeoPoint position) { final Vector3D here = project(east, north, position.as3D()); try { output.put(new Trails.State(new Engine(eng), here)); } catch (InterruptedException e) { throw new RuntimeException("Interrupted", e); } } }); } }
From source file:edu.mit.fss.hla.FSScartesianVector.java
/** * Sets this vector from a {@link Vector3D} object. * * @param value the new value//from w ww . ja v a 2s .c o m */ public void setValue(Vector3D value) { this.x.setValue(value.getX()); this.y.setValue(value.getY()); this.z.setValue(value.getZ()); }
From source file:jtrace.object.Cube.java
@Override public double getFirstCollisionObjectFrame(Ray ray) { // Cube edges and vertices are the intersections of 6 planes. // Need to find points of intersection with each of these planes. double alpha = Double.POSITIVE_INFINITY; Ray collisionNormal = new Ray(); for (int i = 0; i < 6; i++) { double thisAlpha = normals[i].direction.dotProduct(normals[i].origin.subtract(ray.origin)) / normals[i].direction.dotProduct(ray.direction); if (thisAlpha <= 0) continue; Vector3D collisionLocation = ray.origin.add(thisAlpha, ray.direction); Vector3D delta = collisionLocation.subtract(normals[i].origin); if (Math.abs(delta.getX()) > 0.5 || Math.abs(delta.getY()) > 0.5 || Math.abs(delta.getZ()) > 0.5) { continue; }/*from ww w .j a v a 2s. c o m*/ if (thisAlpha < alpha) { alpha = thisAlpha; collisionNormal.origin = collisionLocation; collisionNormal.direction = normals[i].direction; } } if (alpha < Double.POSITIVE_INFINITY) { incidentRay = ray; normalRay = collisionNormal; } return alpha; }
From source file:com.rvantwisk.cnctools.controls.opengl.ArrowsActor.java
private void addArrow(double[][] arrow, double[] loc, double rA, final Vector3D p1, final Vector3D p2) { try {// w w w . jav a2 s . c o m double angleZ = Point.angleBetween2Lines(new Point(p1.getX(), p1.getY()), new Point(p2.getX(), p2.getY()), new Point(0.0, 0.0), new Point(0.0, 1.0)); // double angleX = Point.angleBetween2Lines(new Point(lastY, lastZ), new Point(rY, rZ), new Point(0.0, 0.0), new Point(0.0, 1.0)); double dx = p1.getX() - p2.getX(); double dy = p1.getY() - p2.getY(); double d = Math.sqrt(dx * dx + dy * dy); double angle; Rotation myRotation; if (d != 0.0) { angle = Point.angleBetween2Lines(new Point(0.0, 0.0), new Point(1.0, 0.0), new Point(0.0, 0.0), new Point(d, p1.getZ() - p2.getZ())); myRotation = new Rotation(new Vector3D(1, 0, 0.0), angle + (0.0 / 360.0 * Math.PI * 2.0)); } else if ((p1.getZ() - p2.getZ()) < 0.0) { angle = (90.0 / 360.0 * Math.PI * 2.0); myRotation = new Rotation(new Vector3D(0, 1, 0.0), angle + (0.0 / 360.0 * Math.PI * 2.0)); } else { angle = (-90.0 / 360.0 * Math.PI * 2.0); myRotation = new Rotation(new Vector3D(0, 1, 0.0), angle + (0.0 / 360.0 * Math.PI * 2.0)); } Rotation myRotationZ = new Rotation(new Vector3D(0, 0, 1.0), angleZ + (-90.0 / 360.0 * Math.PI * 2.0)); Rotation myRotationA = new Rotation(new Vector3D(1.0, 0.0, 0.0), (rA / 360.0 * Math.PI * 2.0)); double[] out = new double[3]; double[] out2 = new double[3]; for (double[] v : arrow) { myRotationZ.applyTo(v, out); myRotation.applyTo(out, out2); out2[0] = out2[0] + loc[0]; out2[1] = out2[1] + loc[1]; out2[2] = out2[2] + loc[2]; myRotationA.applyTo(out2, out2); data.add((float) (out2[0] + 0.0)); data.add((float) (out2[1] + 0.0)); data.add((float) (out2[2] + 0.0)); setMotionColor(machine.getMotionMode()); } } catch (Exception e) { // If for some reason we get a exception, we just don't add the arrow logger.warn("Add arrow : This should normally not happen, please report back.", e); } }
From source file:com.mapr.synth.drive.DriveTest.java
@Test public void testDriving() throws FileNotFoundException { Random rand = new Random(3); GeoPoint start = new GeoPoint((rand.nextDouble() - 0.5) * Math.PI / 2, rand.nextDouble() * Math.PI * 2); GeoPoint end = start.nearby(10, rand); final Vector3D east = start.east(); final Vector3D north = start.north(east); try (PrintWriter out = new PrintWriter(new File("drive-sim.csv"))) { out.printf("i, t, x, y, rpm, throttle, speed, gear"); for (int i = 0; i < 50; i++) { final int trial = i; double t = 0; Car car = new Car(); List<Car.Segment> plan = Car.plan(start, end, rand); assertTrue(plan.size() < 20); final GeoPoint currentPosition = new GeoPoint(start.as3D()); for (Car.Segment segment : plan) { t = car.simulate(t, currentPosition, rand, segment, new Car.Callback() { @Override/*from w ww. j av a 2s . co m*/ void call(double t, Engine arg, GeoPoint position) { final Vector3D here = project(east, north, currentPosition.as3D()); Engine engine = car.getEngine(); out.printf("%d, %.2f, %.2f, %.2f, %.1f, %.1f, %.1f, %d\n", trial, t, here.getX(), here.getY(), engine.getRpm(), engine.getThrottle(), engine.getSpeed(), engine.getGear()); } }); assertEquals(0, currentPosition.distance(segment.getEnd()), 0.04); } // should arrive at our destination! assertEquals(0.0, currentPosition.distance(end), 0.01); } } }
From source file:edu.stanford.cfuller.imageanalysistools.filter.WatershedFilter.java
/** * Gets the seed Image for the watershed segmentation. If no seed Image has been set externally, one is created from * the set of pixels at the lowest greylevel. If a seed Image has been set externally, the seedImage retrieved is the external one. * //from w w w .j a v a 2 s . co m * @param greylevelLookup A hashtable mapping greylevel values in the Image to coordinates in the Image. * @param im The Image being segmented. * @param h A Histogram of the Image being segmented, constructed before the beginning of segmentation. * @return An Image containing the seeds for the watershed algorithm. */ protected WritableImage getSeedImage(java.util.Hashtable<Double, java.util.Vector<Vector3D>> greylevelLookup, Image im, Histogram h) { if (!(this.seedImage == null)) { return ImageFactory.createWritable(this.seedImage); } WritableImage tempSeed = null; if (this.seedImage == null) { tempSeed = ImageFactory.createWritable(im.getDimensionSizes(), 0.0f); } else { tempSeed = ImageFactory.createWritable(this.seedImage); } double minValue = h.getMinValue(); java.util.Vector<Vector3D> minPoints = greylevelLookup.get(minValue); ImageCoordinate ic = ImageCoordinate.createCoordXYZCT(0, 0, 0, 0, 0); for (Vector3D v : minPoints) { ic.set(ImageCoordinate.X, (int) (v.getX())); ic.set(ImageCoordinate.Y, (int) (v.getY())); ic.set(ImageCoordinate.Z, (int) (v.getZ())); tempSeed.setValue(ic, 1); } ic.recycle(); LabelFilter lf = new LabelFilter(); lf.apply(tempSeed); if (this.seedImage != null) this.seedImage = tempSeed; return tempSeed; }
From source file:edu.stanford.cfuller.imageanalysistools.filter.WatershedFilter.java
/** * Applies the WatershedFilter to the specified Image, segmenting it. * @param im The Image to be segmented. *//*from www. j a v a2s . co m*/ @Override public void apply(WritableImage im) { WritableImage imCopy = ImageFactory.createWritable(im); InversionFilter invf = new InversionFilter(); invf.apply(imCopy); Histogram h = new Histogram(imCopy); java.util.Hashtable<Double, java.util.Vector<Vector3D>> greylevelLookup = new Hashtable<Double, java.util.Vector<Vector3D>>(); for (ImageCoordinate ic : imCopy) { double value = imCopy.getValue(ic); if (greylevelLookup.get(value) == null) { greylevelLookup.put(value, new java.util.Vector<Vector3D>()); } greylevelLookup.get(value).add( new Vector3D(ic.get(ImageCoordinate.X), ic.get(ImageCoordinate.Y), ic.get(ImageCoordinate.Z))); } WritableImage processing = getSeedImage(greylevelLookup, imCopy, h); Histogram hSeed = new Histogram(processing); int nextLabel = hSeed.getMaxValue() + 1; ImageCoordinate ic = ImageCoordinate.createCoordXYZCT(0, 0, 0, 0, 0); for (int i = h.getMinValue() + 1; i < h.getMaxValue(); i++) { //java.util.logging.Logger.getLogger("edu.stanford.cfuller.imageanalysistools").info("processing greylevel: " + i); if (h.getCounts(i) == 0) continue; for (Vector3D v : greylevelLookup.get((double) i)) { int x = (int) v.getX(); int y = (int) v.getY(); int z = (int) v.getZ(); ic.set(ImageCoordinate.X, x); ic.set(ImageCoordinate.Y, y); ic.set(ImageCoordinate.Z, z); int label = getCorrectLabel(ic, processing, nextLabel); processing.setValue(ic, label); if (label == nextLabel) nextLabel++; } } ic.recycle(); MaskFilter mf = new MaskFilter(); mf.setReferenceImage(processing); mf.apply(im); LabelFilter lf = new LabelFilter(); lf.apply(im); }
From source file:Engine.WorldMap.java
public double GetGroundPosition(Vector3D position, WorldMap worldMap) { return GetGroundPosition(new double[] { position.getX(), position.getY(), position.getZ() }, worldMap); }
From source file:Engine.WorldMap.java
public double GetGroundPosition(double[] position, WorldMap worldMap) { double retX = 0; double retY = 0; double x = (int) position[0] / GenerateTerrain.Size; double y = (int) position[1] / GenerateTerrain.Size; double[] p11 = { x, y, (double) worldMap.Map[(int) x][(int) y] }; double[] p12 = { x, y + 1, (double) worldMap.Map[(int) x][(int) y + 1] }; double[] p21 = { x + 1, y, (double) worldMap.Map[(int) x + 1][(int) y] }; double[] p22 = { x + 1, y + 1, (double) worldMap.Map[(int) x + 1][(int) y + 1] }; if (PointInTriangle(position, p11, p12, p22)) { Vector3D v1 = new Vector3D(p12[0] - p11[0], p12[1] - p11[1], p12[2] - p11[2]); Vector3D v2 = new Vector3D(p22[0] - p11[0], p22[1] - p11[1], p22[2] - p11[2]); Vector3D n = Vector3D.crossProduct(v1, v2); double z = ((n.getX() * x) + (n.getY() * y) + ((-n.getX() * p11[0]) + (-n.getY() * p11[1]) + (-n.getZ() * p11[2]))) / (-n.getZ()); return z; //return (z3(x-x1)(y-y2) + z1(x-x2)(y-y3) + z2(x-x3)(y-y1) - z2(x-x1)(y-y3) - z3(x-x2)(y-y1) - z1(x-x3)(y-y2)) // / ( (x-x1)(y-y2) + (x-x2)(y-y3) + (x-x3)(y-y1) - (x-x1)(y-y3) - (x-x2)(y-y1) - (x-x3)(y-y2)); } else {//ww w . ja v a2s. c om Vector3D v1 = new Vector3D(p21[0] - p11[0], p21[1] - p11[1], p21[2] - p11[2]); Vector3D v2 = new Vector3D(p22[0] - p11[0], p22[1] - p11[1], p22[2] - p11[2]); Vector3D n = Vector3D.crossProduct(v1, v2); double z = ((n.getX() * x) + (n.getY() * y) + ((-n.getX() * p11[0]) + (-n.getY() * p11[1]) + (-n.getZ() * p11[2]))) / (-n.getZ()); return z; } //screen.worldMap.Map[x][y]; }
From source file:Engine.WorldMap.java
public Vector3D CorrectPosition(Vector3D position, Screen screen) { double x = (double) (position.getX()); double y = (double) (position.getY()); x = DoubleModulo(x, (screen.worldMap.mapSize - 1) * GenerateTerrain.Size); y = DoubleModulo(y, (screen.worldMap.mapSize - 1) * GenerateTerrain.Size); return new Vector3D(x, y, position.getZ()); }