List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D getZ
public double getZ()
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 {//from w w w . ja v a2 s . c om 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:msi.gama.metamodel.shape.GamaShape.java
/** * Same as above, but applies a (optional) rotation along a given vector and * (optional) translation to the geometry * //from ww w. ja v a2 s . c om * @param source * cannot be null * @param geom * can be null * @param rotation * can be null, expressed in degrees * @param newLocation * can be null */ public GamaShape(final IShape source, final Geometry geom, final Double rotation, final GamaPoint vector, final ILocation newLocation) { this(source, geom); if (!isPoint() && rotation != null) { if (vector == null) { final Coordinate c = geometry.getCentroid().getCoordinate(); geometry.apply(AffineTransform3D.createRotationOz(FastMath.toRadians(rotation), c.x, c.y)); } else { final Vector3D v3D = new Vector3D(vector.getX(), vector.getY(), vector.getZ()); final Rotation rot = new Rotation(v3D, FastMath.toRadians(rotation)); for (final Coordinate c : this.getInnerGeometry().getCoordinates()) { final Vector3D result = rot.applyTo(new Vector3D(c.x, c.y, c.z)); c.x = result.getX(); c.y = result.getY(); c.z = result.getZ(); } } } if (newLocation != null) { setLocation(newLocation); } }
From source file:gin.melec.Cube.java
/** * Set the planes of the cube./* www.ja v a 2 s . c o m*/ */ private void setPlanes() { Vector3D orig = new Vector3D(xMin, yMin, zMin); Vector3D origX = new Vector3D(xMax, yMin, zMin); Vector3D origY = new Vector3D(xMin, yMax, zMin); Vector3D origZ = new Vector3D(xMin, yMin, zMax); Vector3D xZ = new Vector3D(xMax, yMin, zMax); Vector3D xY = new Vector3D(xMax, yMax, zMin); Vector3D yZ = new Vector3D(xMin, yMax, zMax); Vector3D xYZ = new Vector3D(xMax, yMax, zMax); Vector3D constructedVector; constructedVector = new Vector3D((origY.getX() - 100), (origY.getY() + 100), origY.getZ()); frontLeftPlane = new CustomPlane(frontSplit, leftSplit, origY, yZ, constructedVector, 0.001); constructedVector = new Vector3D((xY.getX() - 100), (xY.getY() - 100), xY.getZ()); frontRightPlane = new CustomPlane(frontSplit, rightSplit, xY, xYZ, constructedVector, 0.001); constructedVector = new Vector3D(origY.getX(), (origY.getY() - 100), (origY.getZ() + 100)); frontUpPlane = new CustomPlane(frontSplit, upSplit, origY, xY, constructedVector, 0.001); constructedVector = new Vector3D(yZ.getX(), (yZ.getY() - 100), (yZ.getZ() - 100)); frontDownPlane = new CustomPlane(frontSplit, downSplit, yZ, xYZ, constructedVector, 0.001); constructedVector = new Vector3D((orig.getX() + 100), (orig.getY() + 100), orig.getZ()); backLeftPlane = new CustomPlane(backSplit, leftSplit, orig, origZ, constructedVector, 0.001); constructedVector = new Vector3D((origX.getX() - 100), (origX.getY() + 100), origX.getZ()); backRightPlane = new CustomPlane(backSplit, rightSplit, origX, xZ, constructedVector, 0.001); constructedVector = new Vector3D(orig.getX(), (orig.getY() + 100), (orig.getZ() + 100)); backUpPlane = new CustomPlane(backSplit, upSplit, orig, origX, constructedVector, 0.001); constructedVector = new Vector3D(origZ.getX(), (origZ.getY() + 100), (origZ.getZ() - 100)); backDownPlane = new CustomPlane(backSplit, downSplit, origZ, xZ, constructedVector, 0.001); constructedVector = new Vector3D((orig.getX() + 100), orig.getY(), (orig.getZ() + 100)); leftUpPlane = new CustomPlane(leftSplit, upSplit, orig, origY, constructedVector, 0.001); constructedVector = new Vector3D((origZ.getX() + 100), origZ.getY(), (origZ.getZ() - 100)); leftDownPlane = new CustomPlane(leftSplit, downSplit, origZ, yZ, constructedVector, 0.001); constructedVector = new Vector3D((origX.getX() - 100), origX.getY(), (origX.getZ() + 100)); rightUpPlane = new CustomPlane(rightSplit, upSplit, origX, xY, constructedVector, 0.001); constructedVector = new Vector3D((xZ.getX() - 100), xZ.getY(), (xZ.getZ() - 100)); rightDownPlane = new CustomPlane(rightSplit, downSplit, xZ, xYZ, constructedVector, 0.001); }
From source file:com.diozero.imu.drivers.invensense.MPU9150DataFactory.java
public static ImuData newInstance(MPU9150FIFOData fifoData, short[] compassData, double gyroScaleFactor, double accelScaleFactor, double compassScaleFactor, double quatScaleFactor, float temperature) { Vector3D gyro = ImuDataFactory.createVector(fifoData.getGyro(), gyroScaleFactor); Vector3D accel = ImuDataFactory.createVector(fifoData.getAccel(), accelScaleFactor); // TODO What is the scale factor for the quaternion data?! Quaternion quaternion = ImuDataFactory.createQuaternion(fifoData.getQuat(), quatScaleFactor); Vector3D compass = ImuDataFactory.createVector(compassData, compassScaleFactor); // From https://github.com/sparkfun/MPU-9150_Breakout/blob/master/firmware/MPU6050/Examples/MPU9150_AHRS.ino // The gyros and accelerometers can in principle be calibrated in addition to any factory calibration but they are generally // pretty accurate. You can check the accelerometer by making sure the reading is +1 g in the positive direction for each axis. // The gyro should read zero for each axis when the sensor is at rest. Small or zero adjustment should be needed for these sensors. // The magnetometer is a different thing. Most magnetometers will be sensitive to circuit currents, computers, and // other both man-made and natural sources of magnetic field. The rough way to calibrate the magnetometer is to record // the maximum and minimum readings (generally achieved at the North magnetic direction). The average of the sum divided by two // should provide a pretty good calibration offset. Don't forget that for the MPU9150, the magnetometer x- and y-axes are switched // compared to the gyro and accelerometer! // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of the magnetometer; // the magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro! // We have to make some allowance for this orientation mismatch in feeding the output to the quaternion filter. // For the MPU-9150, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like // in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention. // This is ok by aircraft orientation standards! // From Richards Tech (not sure if this is needed if the orientation has been set correctly by the DMP driver!) // Sort out gyro axes //gyro = new Vector3D(gyro.getX(), -gyro.getY(), -gyro.getZ()); // Sort out accel axes //accel = new Vector3D(-accel.getX(), accel.getY(), accel.getZ()); // Sort out compass axes // TODO Can this be handled by a generic transformation matrix? compass = new Vector3D(compass.getY(), -compass.getX(), -compass.getZ()); long timestamp = fifoData.getTimestamp(); return new ImuData(gyro, accel, quaternion, compass, temperature, timestamp); }
From source file:com.diozero.imu.drivers.invensense.ImuDataFactory.java
public static ImuData newInstance(MPU9150FIFOData fifoData, short[] compassData, double gyroScaleFactor, double accelScaleFactor, double compassScaleFactor, double quatScaleFactor, float temperature) { Vector3D gyro = createVector(fifoData.getGyro(), gyroScaleFactor); Vector3D accel = createVector(fifoData.getAccel(), accelScaleFactor); // TODO What is the scale factor for the quaternion data?! Quaternion quaternion = createQuaternion(fifoData.getQuat(), quatScaleFactor); Vector3D compass = createVector(compassData, compassScaleFactor); // From https://github.com/sparkfun/MPU-9150_Breakout/blob/master/firmware/MPU6050/Examples/MPU9150_AHRS.ino // The gyros and accelerometers can in principle be calibrated in addition to any factory calibration but they are generally // pretty accurate. You can check the accelerometer by making sure the reading is +1 g in the positive direction for each axis. // The gyro should read zero for each axis when the sensor is at rest. Small or zero adjustment should be needed for these sensors. // The magnetometer is a different thing. Most magnetometers will be sensitive to circuit currents, computers, and // other both man-made and natural sources of magnetic field. The rough way to calibrate the magnetometer is to record // the maximum and minimum readings (generally achieved at the North magnetic direction). The average of the sum divided by two // should provide a pretty good calibration offset. Don't forget that for the MPU9150, the magnetometer x- and y-axes are switched // compared to the gyro and accelerometer! // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of the magnetometer; // the magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro! // We have to make some allowance for this orientation mismatch in feeding the output to the quaternion filter. // For the MPU-9150, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like // in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention. // This is ok by aircraft orientation standards! // From Richards Tech (not sure if this is needed if the orientation has been set correctly by the DMP driver!) // Sort out gyro axes //gyro = new Vector3D(gyro.getX(), -gyro.getY(), -gyro.getZ()); // Sort out accel axes //accel = new Vector3D(-accel.getX(), accel.getY(), accel.getZ()); // Sort out compass axes // TODO Can this be handled by a generic transformation matrix? compass = new Vector3D(compass.getY(), -compass.getX(), -compass.getZ()); long timestamp = fifoData.getTimestamp(); return new ImuData(gyro, accel, quaternion, compass, temperature, timestamp); }
From source file:nova.core.network.handler.BlockPacket.java
@Override public void write(Block block, Packet packet) { if (block instanceof Syncable) { Vector3D position = block.position(); packet.writeInt((int) position.getX()); packet.writeInt((int) position.getY()); packet.writeInt((int) position.getZ()); ((Syncable) block).write(packet); return;// w w w . j a v a2s .com } throw new NetworkException("Failed to send packet for block: " + block); }
From source file:nova.core.util.math.MatrixStack.java
/** * Translates current transformation matrix. * * @param translateVector vector of translation. * @return The tranformed matrix// w w w . j a va 2 s . c o m */ public MatrixStack translate(Vector3D translateVector) { translate(translateVector.getX(), translateVector.getY(), translateVector.getZ()); return this; }
From source file:nova.core.util.math.MatrixStack.java
/** * Scales current transformation matrix. * * @param scaleVector scale vector.//from www. j av a2 s . co m * @return The current matrix */ public MatrixStack scale(Vector3D scaleVector) { scale(scaleVector.getX(), scaleVector.getY(), scaleVector.getZ()); return this; }
From source file:nova.core.util.math.Vector3DUtil.java
/** * Calculates maximum of each coordinate. * * @param a first vector./* w w w.ja va 2s . c o m*/ * @param b second vector. * @return new vector that's each coordinate is maximum of coordinates of a and b. */ public static Vector3D max(Vector3D a, Vector3D b) { return new Vector3D(FastMath.max(a.getX(), b.getX()), FastMath.max(a.getY(), b.getY()), FastMath.max(a.getZ(), b.getZ())); }
From source file:nova.core.util.math.Vector3DUtil.java
/** * Calculates minimum of each coordinate. * @param a first vector.//from w w w . j a v a 2s . c om * @param b second vector. * @return new vector that's each coordinate is minimum of coordinates of a and b. */ public static Vector3D min(Vector3D a, Vector3D b) { return new Vector3D(FastMath.min(a.getX(), b.getX()), FastMath.min(a.getY(), b.getY()), FastMath.min(a.getZ(), b.getZ())); }