List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Rotation Rotation
public Rotation(double q0, double q1, double q2, double q3, boolean needsNormalization)
From source file:fr.cs.examples.frames.Frames2.java
public static void main(String[] args) { try {// ww w. j a va 2 s . co m // configure Orekit Autoconfiguration.configureOrekit(); // Considering the following Computing/Measurement date in UTC time scale TimeScale utc = TimeScalesFactory.getUTC(); AbsoluteDate date = new AbsoluteDate(2008, 10, 01, 12, 00, 00.000, utc); // The Center of Gravity frame has its origin at the satellite center of gravity (CoG) // and its axes parallel to EME2000. It is derived from EME2000 frame at any moment // by an unknown transform which depends on the current position and the velocity. // Let's initialize this transform by the identity transform. UpdatableFrame cogFrame = new UpdatableFrame(FramesFactory.getEME2000(), Transform.IDENTITY, "LOF", false); // The satellite frame, with origin also at the CoG, depends on attitude. // For the sake of this tutorial, we consider a simple inertial attitude here Transform cogToSat = new Transform(date, new Rotation(0.6, 0.48, 0, 0.64, false)); Frame satFrame = new Frame(cogFrame, cogToSat, "sat", false); // Finally, the GPS antenna frame can be defined from the satellite frame by 2 transforms: // a translation and a rotation Transform translateGPS = new Transform(date, new Vector3D(0, 0, 1)); Transform rotateGPS = new Transform(date, new Rotation(new Vector3D(0, 1, 3), FastMath.toRadians(10))); Frame gpsFrame = new Frame(satFrame, new Transform(date, translateGPS, rotateGPS), "GPS", false); // Let's get the satellite position and velocity in ITRF as measured by GPS antenna at this moment: final Vector3D position = new Vector3D(-6142438.668, 3492467.560, -25767.25680); final Vector3D velocity = new Vector3D(505.8479685, 942.7809215, 7435.922231); System.out.format(Locale.US, "GPS antenna position in ITRF: %12.3f %12.3f %12.3f%n", position.getX(), position.getY(), position.getZ()); System.out.format(Locale.US, "GPS antenna velocity in ITRF: %12.7f %12.7f %12.7f%n", velocity.getX(), velocity.getY(), velocity.getZ()); // The transform from GPS frame to ITRF frame at this moment is defined by // a translation and a rotation. The translation is directly provided by the // GPS measurement above. The rotation is extracted from the existing tree, where // we know all rotations are already up to date, even if one translation is still // unknown. We combine the extracted rotation and the measured translation by // applying the rotation first because the position/velocity vector are given in // ITRF frame not in GPS antenna frame: Transform measuredTranslation = new Transform(date, position, velocity); Transform formerTransform = gpsFrame .getTransformTo(FramesFactory.getITRF(IERSConventions.IERS_2010, true), date); Transform preservedRotation = new Transform(date, formerTransform.getRotation(), formerTransform.getRotationRate()); Transform gpsToItrf = new Transform(date, preservedRotation, measuredTranslation); // So we can update the transform from EME2000 to CoG frame cogFrame.updateTransform(gpsFrame, FramesFactory.getITRF(IERSConventions.IERS_2010, true), gpsToItrf, date); // And we can get the position and velocity of satellite CoG in EME2000 frame PVCoordinates origin = PVCoordinates.ZERO; Transform cogToItrf = cogFrame.getTransformTo(FramesFactory.getITRF(IERSConventions.IERS_2010, true), date); PVCoordinates satItrf = cogToItrf.transformPVCoordinates(origin); System.out.format(Locale.US, "Satellite position in ITRF: %12.3f %12.3f %12.3f%n", satItrf.getPosition().getX(), satItrf.getPosition().getY(), satItrf.getPosition().getZ()); System.out.format(Locale.US, "Satellite velocity in ITRF: %12.7f %12.7f %12.7f%n", satItrf.getVelocity().getX(), satItrf.getVelocity().getY(), satItrf.getVelocity().getZ()); Transform cogToEme2000 = cogFrame.getTransformTo(FramesFactory.getEME2000(), date); PVCoordinates satEME2000 = cogToEme2000.transformPVCoordinates(origin); System.out.format(Locale.US, "Satellite position in EME2000: %12.3f %12.3f %12.3f%n", satEME2000.getPosition().getX(), satEME2000.getPosition().getY(), satEME2000.getPosition().getZ()); System.out.format(Locale.US, "Satellite velocity in EME2000: %12.7f %12.7f %12.7f%n", satEME2000.getVelocity().getX(), satEME2000.getVelocity().getY(), satEME2000.getVelocity().getZ()); } catch (OrekitException oe) { System.err.println(oe.getMessage()); } }
From source file:com.diozero.sandpit.imu.invensense.MPU9150DriverTest.java
public void run() { try (MPU9150Driver mpu = new MPU9150Driver(I2CConstants.BUS_1, I2CConstants.ADDR_SIZE_7, MPU9150Constants.I2C_CLOCK_FREQUENCY_FAST)) { mpuInit(mpu);/*from w ww.ja v a 2 s . c o m*/ System.err.println("Ready."); do { ImuData imu_data = update(mpu); System.out.print("Got IMU data: compass=[" + imu_data.getCompass() + "], temp=" + imu_data.getTemperature() + ", gyro=[" + imu_data.getGyro() + "], accel=[" + imu_data.getAccel() + "], quat=[" + imu_data.getQuaternion().getQ0() + ", " + imu_data.getQuaternion().getQ1() + ", " + imu_data.getQuaternion().getQ2() + ", " + imu_data.getQuaternion().getQ3() + "], timestamp=" + imu_data.getTimestamp() + ", "); Quaternion q = imu_data.getQuaternion(); //double[] ypr = q.toEuler(); //double[] ypr = quat.getYawPitchRoll(); Rotation r = new Rotation(q.getQ0(), q.getQ1(), q.getQ2(), q.getQ3(), true); double[] ypr = null; try { ypr = r.getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR); } catch (CardanEulerSingularityException e) { ypr = new double[] { 2 * Math.atan2(q.getQ1(), q.getQ0()), Math.PI / 2, 0 }; System.out.print("Singularity detected, "); } Logger.info("ypr=[{}, {}, {}]", Double.valueOf(ypr[0]), Double.valueOf(ypr[1]), Double.valueOf(ypr[2])); } while (true); } catch (RuntimeIOException ioe) { Logger.error(ioe, "Error: {}", ioe); } }
From source file:com.diozero.sandpit.imu.invensense.MPU9150DriverMqttPublisher.java
public void run() { try (MPU9150Driver mpu = new MPU9150Driver(I2CConstants.BUS_1, I2CConstants.ADDR_SIZE_7, MPU9150Constants.I2C_CLOCK_FREQUENCY_FAST)) { mpuInit(mpu);//from w w w.ja v a 2 s . c o m mqttInit(); System.err.println("Ready."); do { ImuData imu_data = update(mpu); System.out.print("Got IMU data: compass=[" + imu_data.getCompass() + "], temp=" + imu_data.getTemperature() + ", gyro=[" + imu_data.getGyro() + "], accel=[" + imu_data.getAccel() + "], quat=[" + imu_data.getQuaternion().getQ0() + ", " + imu_data.getQuaternion().getQ1() + ", " + imu_data.getQuaternion().getQ2() + ", " + imu_data.getQuaternion().getQ3() + "], timestamp=" + imu_data.getTimestamp() + ", "); Quaternion q = imu_data.getQuaternion(); //double[] ypr = q.toEuler(); //double[] ypr = quat.getYawPitchRoll(); Rotation r = new Rotation(q.getQ0(), q.getQ1(), q.getQ2(), q.getQ3(), true); double[] ypr = null; try { ypr = r.getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR); } catch (CardanEulerSingularityException e) { ypr = new double[] { 2 * Math.atan2(q.getQ1(), q.getQ0()), Math.PI / 2, 0 }; System.out.print("Singularity detected, "); } Logger.info("ypr=[{}, {}, {}]", Double.valueOf(ypr[0]), Double.valueOf(ypr[1]), Double.valueOf(ypr[2])); mqttPublish(imu_data, ypr); } while (true); } catch (RuntimeIOException ioe) { Logger.error(ioe, "Error: {}", ioe); } catch (MqttException me) { Logger.error(me, "Error: {}", me); } finally { try { mqttClient.disconnect(); } catch (Exception e) { } } }
From source file:nova.core.util.math.RotationUtil.java
/** * Returns the slerp interpolation of Rotations {@code a} and {@code b}, at * time {@code t}.//from w w w . j a v a 2s .c o m * <p> * {@code t} should range in {@code [0,1]}. Result is a when {@code t=0 } and * {@code b} when {@code t=1}. * <p> * When {@code allowFlip} is true (default) the slerp interpolation will * always use the "shortest path" between the Rotations' orientations, by * "flipping" the source Rotation if needed. * @param a the first Rotation * @param b the second Rotation * @param t the t interpolation parameter * @param allowFlip tells whether or not the interpolation allows axis flip */ public static Rotation slerp(Rotation a, Rotation b, double t, boolean allowFlip) { // Warning: this method should not normalize the Rotation double cosAngle = dotProduct(a, b); double c1, c2; // Linear interpolation for close orientations if ((1.0 - FastMath.abs(cosAngle)) < 0.01) { c1 = 1.0f - t; c2 = t; } else { // Spherical interpolation double angle = FastMath.acos(FastMath.abs(cosAngle)); double sinAngle = FastMath.sin(angle); c1 = FastMath.sin(angle * (1.0f - t)) / sinAngle; c2 = FastMath.sin(angle * t) / sinAngle; } // Use the shortest path if (allowFlip && (cosAngle < 0.0)) { c1 = -c1; } return new Rotation(c1 * a.getQ1() + c2 * b.getQ1(), c1 * a.getQ2() + c2 * b.getQ2(), c1 * a.getQ3() + c2 * b.getQ3(), c1 * a.getQ0() + c2 * b.getQ0(), false); }
From source file:org.orekit.attitudes.AttitudeTest.java
@Test public void testZeroRate() throws OrekitException { Attitude attitude = new Attitude(AbsoluteDate.J2000_EPOCH, FramesFactory.getEME2000(), new Rotation(0.48, 0.64, 0.36, 0.48, false), Vector3D.ZERO, Vector3D.ZERO); Assert.assertEquals(Vector3D.ZERO, attitude.getSpin()); double dt = 10.0; Attitude shifted = attitude.shiftedBy(dt); Assert.assertEquals(Vector3D.ZERO, shifted.getRotationAcceleration()); Assert.assertEquals(Vector3D.ZERO, shifted.getSpin()); Assert.assertEquals(0.0, Rotation.distance(attitude.getRotation(), shifted.getRotation()), 1.0e-15); }
From source file:org.orekit.attitudes.AttitudeTest.java
@Test public void testSpin() throws OrekitException { double rate = 2 * FastMath.PI / (12 * 60); Attitude attitude = new Attitude(AbsoluteDate.J2000_EPOCH, FramesFactory.getEME2000(), new Rotation(0.48, 0.64, 0.36, 0.48, false), new Vector3D(rate, Vector3D.PLUS_K), Vector3D.ZERO); Assert.assertEquals(rate, attitude.getSpin().getNorm(), 1.0e-10); double dt = 10.0; Attitude shifted = attitude.shiftedBy(dt); Assert.assertEquals(rate, shifted.getSpin().getNorm(), 1.0e-10); Assert.assertEquals(rate * dt, Rotation.distance(attitude.getRotation(), shifted.getRotation()), 1.0e-10); Vector3D shiftedX = shifted.getRotation().applyInverseTo(Vector3D.PLUS_I); Vector3D shiftedY = shifted.getRotation().applyInverseTo(Vector3D.PLUS_J); Vector3D shiftedZ = shifted.getRotation().applyInverseTo(Vector3D.PLUS_K); Vector3D originalX = attitude.getRotation().applyInverseTo(Vector3D.PLUS_I); Vector3D originalY = attitude.getRotation().applyInverseTo(Vector3D.PLUS_J); Vector3D originalZ = attitude.getRotation().applyInverseTo(Vector3D.PLUS_K); Assert.assertEquals(FastMath.cos(rate * dt), Vector3D.dotProduct(shiftedX, originalX), 1.0e-10); Assert.assertEquals(FastMath.sin(rate * dt), Vector3D.dotProduct(shiftedX, originalY), 1.0e-10); Assert.assertEquals(0.0, Vector3D.dotProduct(shiftedX, originalZ), 1.0e-10); Assert.assertEquals(-FastMath.sin(rate * dt), Vector3D.dotProduct(shiftedY, originalX), 1.0e-10); Assert.assertEquals(FastMath.cos(rate * dt), Vector3D.dotProduct(shiftedY, originalY), 1.0e-10); Assert.assertEquals(0.0, Vector3D.dotProduct(shiftedY, originalZ), 1.0e-10); Assert.assertEquals(0.0, Vector3D.dotProduct(shiftedZ, originalX), 1.0e-10); Assert.assertEquals(0.0, Vector3D.dotProduct(shiftedZ, originalY), 1.0e-10); Assert.assertEquals(1.0, Vector3D.dotProduct(shiftedZ, originalZ), 1.0e-10); Vector3D forward = AngularCoordinates.estimateRate(attitude.getRotation(), shifted.getRotation(), dt); Assert.assertEquals(0.0, forward.subtract(attitude.getSpin()).getNorm(), 1.0e-10); Vector3D reversed = AngularCoordinates.estimateRate(shifted.getRotation(), attitude.getRotation(), dt); Assert.assertEquals(0.0, reversed.add(attitude.getSpin()).getNorm(), 1.0e-10); }
From source file:org.orekit.attitudes.FixedRateTest.java
@Test public void testZeroRate() throws OrekitException { AbsoluteDate date = new AbsoluteDate(new DateComponents(2004, 3, 2), new TimeComponents(13, 17, 7.865), TimeScalesFactory.getUTC()); final Frame frame = FramesFactory.getEME2000(); FixedRate law = new FixedRate(new Attitude(date, frame, new Rotation(0.48, 0.64, 0.36, 0.48, false), Vector3D.ZERO, Vector3D.ZERO)); PVCoordinates pv = new PVCoordinates(new Vector3D(28812595.32012577, 5948437.4640250085, 0), new Vector3D(0, 0, 3680.853673522056)); Orbit orbit = new KeplerianOrbit(pv, frame, date, 3.986004415e14); Rotation attitude0 = law.getAttitude(orbit, date, frame).getRotation(); Assert.assertEquals(0, Rotation.distance(attitude0, law.getReferenceAttitude().getRotation()), 1.0e-10); Rotation attitude1 = law.getAttitude(orbit.shiftedBy(10.0), date.shiftedBy(10.0), frame).getRotation(); Assert.assertEquals(0, Rotation.distance(attitude1, law.getReferenceAttitude().getRotation()), 1.0e-10); Rotation attitude2 = law.getAttitude(orbit.shiftedBy(20.0), date.shiftedBy(20.0), frame).getRotation(); Assert.assertEquals(0, Rotation.distance(attitude2, law.getReferenceAttitude().getRotation()), 1.0e-10); }
From source file:org.orekit.attitudes.FixedRateTest.java
@Test public void testNonZeroRate() throws OrekitException { final AbsoluteDate date = new AbsoluteDate(new DateComponents(2004, 3, 2), new TimeComponents(13, 17, 7.865), TimeScalesFactory.getUTC()); final double rate = 2 * FastMath.PI / (12 * 60); final Frame frame = FramesFactory.getEME2000(); FixedRate law = new FixedRate(new Attitude(date, frame, new Rotation(0.48, 0.64, 0.36, 0.48, false), new Vector3D(rate, Vector3D.PLUS_K), Vector3D.ZERO)); PVCoordinates pv = new PVCoordinates(new Vector3D(28812595.32012577, 5948437.4640250085, 0), new Vector3D(0, 0, 3680.853673522056)); Orbit orbit = new KeplerianOrbit(pv, FramesFactory.getEME2000(), date, 3.986004415e14); Rotation attitude0 = law.getAttitude(orbit, date, frame).getRotation(); Assert.assertEquals(0, Rotation.distance(attitude0, law.getReferenceAttitude().getRotation()), 1.0e-10); Rotation attitude1 = law.getAttitude(orbit.shiftedBy(10.0), date.shiftedBy(10.0), frame).getRotation(); Assert.assertEquals(10 * rate, Rotation.distance(attitude1, law.getReferenceAttitude().getRotation()), 1.0e-10);/*from w ww . j a v a 2 s . c o m*/ Rotation attitude2 = law.getAttitude(orbit.shiftedBy(-20.0), date.shiftedBy(-20.0), frame).getRotation(); Assert.assertEquals(20 * rate, Rotation.distance(attitude2, law.getReferenceAttitude().getRotation()), 1.0e-10); Assert.assertEquals(30 * rate, Rotation.distance(attitude2, attitude1), 1.0e-10); Rotation attitude3 = law.getAttitude(orbit.shiftedBy(0.0), date, frame).getRotation(); Assert.assertEquals(0, Rotation.distance(attitude3, law.getReferenceAttitude().getRotation()), 1.0e-10); }
From source file:org.orekit.attitudes.FixedRateTest.java
@Test public void testSpin() throws OrekitException { AbsoluteDate date = new AbsoluteDate(new DateComponents(1970, 01, 01), new TimeComponents(3, 25, 45.6789), TimeScalesFactory.getUTC()); final double rate = 2 * FastMath.PI / (12 * 60); AttitudeProvider law = new FixedRate(new Attitude(date, FramesFactory.getEME2000(), new Rotation(0.48, 0.64, 0.36, 0.48, false), new Vector3D(rate, Vector3D.PLUS_K), Vector3D.ZERO)); KeplerianOrbit orbit = new KeplerianOrbit(7178000.0, 1.e-4, FastMath.toRadians(50.), FastMath.toRadians(10.), FastMath.toRadians(20.), FastMath.toRadians(30.), PositionAngle.MEAN, FramesFactory.getEME2000(), date, 3.986004415e14); Propagator propagator = new KeplerianPropagator(orbit, law); double h = 0.01; SpacecraftState sMinus = propagator.propagate(date.shiftedBy(-h)); SpacecraftState s0 = propagator.propagate(date); SpacecraftState sPlus = propagator.propagate(date.shiftedBy(h)); // check spin is consistent with attitude evolution double errorAngleMinus = Rotation.distance(sMinus.shiftedBy(h).getAttitude().getRotation(), s0.getAttitude().getRotation()); double evolutionAngleMinus = Rotation.distance(sMinus.getAttitude().getRotation(), s0.getAttitude().getRotation()); Assert.assertEquals(0.0, errorAngleMinus, 1.0e-6 * evolutionAngleMinus); double errorAnglePlus = Rotation.distance(s0.getAttitude().getRotation(), sPlus.shiftedBy(-h).getAttitude().getRotation()); double evolutionAnglePlus = Rotation.distance(s0.getAttitude().getRotation(), sPlus.getAttitude().getRotation()); Assert.assertEquals(0.0, errorAnglePlus, 1.0e-6 * evolutionAnglePlus); Vector3D spin0 = s0.getAttitude().getSpin(); Vector3D reference = AngularCoordinates.estimateRate(sMinus.getAttitude().getRotation(), sPlus.getAttitude().getRotation(), 2 * h); Assert.assertEquals(0.0, spin0.subtract(reference).getNorm(), 1.0e-14); }
From source file:org.orekit.frames.CIRFProvider.java
/** Get the transform from GCRF to CIRF2000 at the specified date. * <p>The transform considers the nutation and precession effects from IERS data.</p> * @param date new value of the date//from w ww . j a v a2s . co m * @return transform at the specified date * @exception OrekitException if the nutation model data embedded in the * library cannot be read */ public Transform getTransform(final AbsoluteDate date) throws OrekitException { final double[] xys = xysPxy2Function.value(date); final double[] dxdy = eopHistory.getNonRotatinOriginNutationCorrection(date); // position of the Celestial Intermediate Pole (CIP) final double xCurrent = xys[0] + dxdy[0]; final double yCurrent = xys[1] + dxdy[1]; // position of the Celestial Intermediate Origin (CIO) final double sCurrent = xys[2] - xCurrent * yCurrent / 2; // set up the bias, precession and nutation rotation final double x2Py2 = xCurrent * xCurrent + yCurrent * yCurrent; final double zP1 = 1 + FastMath.sqrt(1 - x2Py2); final double r = FastMath.sqrt(x2Py2); final double sPe2 = 0.5 * (sCurrent + FastMath.atan2(yCurrent, xCurrent)); final double cos = FastMath.cos(sPe2); final double sin = FastMath.sin(sPe2); final double xPr = xCurrent + r; final double xPrCos = xPr * cos; final double xPrSin = xPr * sin; final double yCos = yCurrent * cos; final double ySin = yCurrent * sin; final Rotation bpn = new Rotation(zP1 * (xPrCos + ySin), -r * (yCos + xPrSin), r * (xPrCos - ySin), zP1 * (yCos - xPrSin), true); return new Transform(date, bpn, Vector3D.ZERO); }