List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D ZERO
Vector3D ZERO
To view the source code for org.apache.commons.math3.geometry.euclidean.threed Vector3D ZERO.
Click Source Link
From source file:org.orekit.utils.PVCoordinatesTest.java
@Test public void testToDerivativeStructureVector1() throws OrekitException { FieldVector3D<DerivativeStructure> fv = new PVCoordinates(new Vector3D(1, 0.1, 10), new Vector3D(-1, -0.1, -10), new Vector3D(10, -1.0, -100)).toDerivativeStructureVector(1); Assert.assertEquals(1, fv.getX().getFreeParameters()); Assert.assertEquals(1, fv.getX().getOrder()); Assert.assertEquals(1.0, fv.getX().getReal(), 1.0e-10); Assert.assertEquals(0.1, fv.getY().getReal(), 1.0e-10); Assert.assertEquals(10.0, fv.getZ().getReal(), 1.0e-10); Assert.assertEquals(-1.0, fv.getX().getPartialDerivative(1), 1.0e-15); Assert.assertEquals(-0.1, fv.getY().getPartialDerivative(1), 1.0e-15); Assert.assertEquals(-10.0, fv.getZ().getPartialDerivative(1), 1.0e-15); checkPV(new PVCoordinates(new Vector3D(1, 0.1, 10), new Vector3D(-1, -0.1, -10), Vector3D.ZERO), new PVCoordinates(fv), 1.0e-15); }
From source file:org.orekit.utils.PVCoordinatesTest.java
@Test @Deprecated // to be removed when PVCoordinates.interpolate is removed public void testInterpolatePolynomialPV() { Random random = new Random(0xae7771c9933407bdl); AbsoluteDate t0 = AbsoluteDate.J2000_EPOCH; for (int i = 0; i < 20; ++i) { PolynomialFunction px = randomPolynomial(5, random); PolynomialFunction py = randomPolynomial(5, random); PolynomialFunction pz = randomPolynomial(5, random); PolynomialFunction pxDot = px.polynomialDerivative(); PolynomialFunction pyDot = py.polynomialDerivative(); PolynomialFunction pzDot = pz.polynomialDerivative(); PolynomialFunction pxDotDot = pxDot.polynomialDerivative(); PolynomialFunction pyDotDot = pyDot.polynomialDerivative(); PolynomialFunction pzDotDot = pzDot.polynomialDerivative(); List<Pair<AbsoluteDate, PVCoordinates>> sample = new ArrayList<Pair<AbsoluteDate, PVCoordinates>>(); for (double dt : new double[] { 0.0, 0.5, 1.0 }) { Vector3D position = new Vector3D(px.value(dt), py.value(dt), pz.value(dt)); Vector3D velocity = new Vector3D(pxDot.value(dt), pyDot.value(dt), pzDot.value(dt)); sample.add(new Pair<AbsoluteDate, PVCoordinates>(t0.shiftedBy(dt), new PVCoordinates(position, velocity, Vector3D.ZERO))); }/*www . j ava 2 s. c o m*/ for (double dt = 0; dt < 1.0; dt += 0.01) { PVCoordinates interpolated = PVCoordinates.interpolate(t0.shiftedBy(dt), true, sample); Vector3D p = interpolated.getPosition(); Vector3D v = interpolated.getVelocity(); Vector3D a = interpolated.getAcceleration(); Assert.assertEquals(px.value(dt), p.getX(), 1.0e-15 * p.getNorm()); Assert.assertEquals(py.value(dt), p.getY(), 1.0e-15 * p.getNorm()); Assert.assertEquals(pz.value(dt), p.getZ(), 1.0e-15 * p.getNorm()); Assert.assertEquals(pxDot.value(dt), v.getX(), 1.0e-15 * v.getNorm()); Assert.assertEquals(pyDot.value(dt), v.getY(), 1.0e-15 * v.getNorm()); Assert.assertEquals(pzDot.value(dt), v.getZ(), 1.0e-15 * v.getNorm()); Assert.assertEquals(pxDotDot.value(dt), a.getX(), 1.0e-14 * a.getNorm()); Assert.assertEquals(pyDotDot.value(dt), a.getY(), 1.0e-14 * a.getNorm()); Assert.assertEquals(pzDotDot.value(dt), a.getZ(), 1.0e-14 * a.getNorm()); } } }
From source file:org.orekit.utils.PVCoordinatesTest.java
@Test public void testGetMomentum() { //setup/*from w w w. jav a 2 s. co m*/ Vector3D p = new Vector3D(1, -2, 3); Vector3D v = new Vector3D(-9, 8, -7); //action + verify Assert.assertEquals(new PVCoordinates(p, v).getMomentum(), p.crossProduct(v)); //check simple cases Assert.assertEquals(new PVCoordinates(Vector3D.PLUS_I, Vector3D.MINUS_I).getMomentum(), Vector3D.ZERO); Assert.assertEquals(new PVCoordinates(Vector3D.PLUS_I, Vector3D.PLUS_J).getMomentum(), Vector3D.PLUS_K); }
From source file:org.orekit.utils.PVCoordinatesTest.java
@Test public void testGetAngularVelocity() { //setup/*from w ww .j av a 2 s . c o m*/ Vector3D p = new Vector3D(1, -2, 3); Vector3D v = new Vector3D(-9, 8, -7); //action + verify Assert.assertEquals(new PVCoordinates(p, v).getAngularVelocity(), p.crossProduct(v).scalarMultiply(1.0 / p.getNormSq())); //check extra simple cases Assert.assertEquals(new PVCoordinates(Vector3D.PLUS_I, Vector3D.MINUS_I).getAngularVelocity(), Vector3D.ZERO); Assert.assertEquals(new PVCoordinates(new Vector3D(2, 0, 0), Vector3D.PLUS_J).getAngularVelocity(), Vector3D.PLUS_K.scalarMultiply(0.5)); }
From source file:org.orekit.utils.TimeStampedAngularCoordinates.java
/** Interpolate angular coordinates. * <p>/*from w w w . j av a 2 s. c om*/ * The interpolated instance is created by polynomial Hermite interpolation * on Rodrigues vector ensuring rotation rate remains the exact derivative of rotation. * </p> * <p> * This method is based on Sergei Tanygin's paper <a * href="http://www.agi.com/downloads/resources/white-papers/Attitude-interpolation.pdf">Attitude * Interpolation</a>, changing the norm of the vector to match the modified Rodrigues * vector as described in Malcolm D. Shuster's paper <a * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A * Survey of Attitude Representations</a>. This change avoids the singularity at . * There is still a singularity at 2, which is handled by slightly offsetting all rotations * when this singularity is detected. * </p> * <p> * Note that even if first and second time derivatives (rotation rates and acceleration) * from sample can be ignored, the interpolated instance always includes * interpolated derivatives. This feature can be used explicitly to * compute these derivatives when it would be too complex to compute them * from an analytical formula: just compute a few sample points from the * explicit formula and set the derivatives to zero in these sample points, * then use interpolation to add derivatives consistent with the rotations. * </p> * @param date interpolation date * @param filter filter for derivatives from the sample to use in interpolation * @param sample sample points on which interpolation should be done * @return a new position-velocity, interpolated at specified date * @exception OrekitException if the number of point is too small for interpolating */ public static TimeStampedAngularCoordinates interpolate(final AbsoluteDate date, final AngularDerivativesFilter filter, final Collection<TimeStampedAngularCoordinates> sample) throws OrekitException { // set up safety elements for 2 singularity avoidance final double epsilon = 2 * FastMath.PI / sample.size(); final double threshold = FastMath.min(-(1.0 - 1.0e-4), -FastMath.cos(epsilon / 4)); // set up a linear model canceling mean rotation rate final Vector3D meanRate; if (filter != AngularDerivativesFilter.USE_R) { Vector3D sum = Vector3D.ZERO; for (final TimeStampedAngularCoordinates datedAC : sample) { sum = sum.add(datedAC.getRotationRate()); } meanRate = new Vector3D(1.0 / sample.size(), sum); } else { if (sample.size() < 2) { throw new OrekitException(OrekitMessages.NOT_ENOUGH_DATA_FOR_INTERPOLATION, sample.size()); } Vector3D sum = Vector3D.ZERO; TimeStampedAngularCoordinates previous = null; for (final TimeStampedAngularCoordinates datedAC : sample) { if (previous != null) { sum = sum.add(estimateRate(previous.getRotation(), datedAC.getRotation(), datedAC.date.durationFrom(previous.date))); } previous = datedAC; } meanRate = new Vector3D(1.0 / (sample.size() - 1), sum); } TimeStampedAngularCoordinates offset = new TimeStampedAngularCoordinates(date, Rotation.IDENTITY, meanRate, Vector3D.ZERO); boolean restart = true; for (int i = 0; restart && i < sample.size() + 2; ++i) { // offset adaptation parameters restart = false; // set up an interpolator taking derivatives into account final HermiteInterpolator interpolator = new HermiteInterpolator(); // add sample points double sign = +1.0; Rotation previous = Rotation.IDENTITY; for (final TimeStampedAngularCoordinates ac : sample) { // remove linear offset from the current coordinates final double dt = ac.date.durationFrom(date); final TimeStampedAngularCoordinates fixed = ac.subtractOffset(offset.shiftedBy(dt)); // make sure all interpolated points will be on the same branch final double dot = MathArrays.linearCombination(fixed.getRotation().getQ0(), previous.getQ0(), fixed.getRotation().getQ1(), previous.getQ1(), fixed.getRotation().getQ2(), previous.getQ2(), fixed.getRotation().getQ3(), previous.getQ3()); sign = FastMath.copySign(1.0, dot * sign); previous = fixed.getRotation(); // check modified Rodrigues vector singularity if (fixed.getRotation().getQ0() * sign < threshold) { // the sample point is close to a modified Rodrigues vector singularity // we need to change the linear offset model to avoid this restart = true; break; } final double[][] rodrigues = fixed.getModifiedRodrigues(sign); switch (filter) { case USE_RRA: // populate sample with rotation, rotation rate and acceleration data interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1], rodrigues[2]); break; case USE_RR: // populate sample with rotation and rotation rate data interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1]); break; case USE_R: // populate sample with rotation data only interpolator.addSamplePoint(dt, rodrigues[0]); break; default: // this should never happen throw new OrekitInternalError(null); } } if (restart) { // interpolation failed, some intermediate rotation was too close to 2 // we need to offset all rotations to avoid the singularity offset = offset.addOffset(new AngularCoordinates(new Rotation(Vector3D.PLUS_I, epsilon), Vector3D.ZERO, Vector3D.ZERO)); } else { // interpolation succeeded with the current offset final DerivativeStructure zero = new DerivativeStructure(1, 2, 0, 0.0); final DerivativeStructure[] p = interpolator.value(zero); final AngularCoordinates ac = createFromModifiedRodrigues( new double[][] { { p[0].getValue(), p[1].getValue(), p[2].getValue() }, { p[0].getPartialDerivative(1), p[1].getPartialDerivative(1), p[2].getPartialDerivative(1) }, { p[0].getPartialDerivative(2), p[1].getPartialDerivative(2), p[2].getPartialDerivative(2) } }); return new TimeStampedAngularCoordinates(offset.getDate(), ac.getRotation(), ac.getRotationRate(), ac.getRotationAcceleration()).addOffset(offset); } } // this should never happen throw new OrekitInternalError(null); }
From source file:org.orekit.utils.TimeStampedAngularCoordinatesTest.java
@Test public void testZeroRate() throws OrekitException { TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, new Rotation(0.48, 0.64, 0.36, 0.48, false), Vector3D.ZERO, Vector3D.ZERO); Assert.assertEquals(Vector3D.ZERO, ac.getRotationRate()); double dt = 10.0; TimeStampedAngularCoordinates shifted = ac.shiftedBy(dt); Assert.assertEquals(Vector3D.ZERO, shifted.getRotationAcceleration()); Assert.assertEquals(Vector3D.ZERO, shifted.getRotationRate()); Assert.assertEquals(0.0, Rotation.distance(ac.getRotation(), shifted.getRotation()), 1.0e-15); }
From source file:org.orekit.utils.TimeStampedAngularCoordinatesTest.java
@Test public void testShift() throws OrekitException { double rate = 2 * FastMath.PI / (12 * 60); TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, Rotation.IDENTITY, new Vector3D(rate, Vector3D.PLUS_K), Vector3D.ZERO); Assert.assertEquals(rate, ac.getRotationRate().getNorm(), 1.0e-10); double dt = 10.0; double alpha = rate * dt; TimeStampedAngularCoordinates shifted = ac.shiftedBy(dt); Assert.assertEquals(rate, shifted.getRotationRate().getNorm(), 1.0e-10); Assert.assertEquals(alpha, Rotation.distance(ac.getRotation(), shifted.getRotation()), 1.0e-10); Vector3D xSat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_I); Assert.assertEquals(0.0, xSat.subtract(new Vector3D(FastMath.cos(alpha), FastMath.sin(alpha), 0)).getNorm(), 1.0e-10);// w w w . j a va2 s . c o m Vector3D ySat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_J); Assert.assertEquals(0.0, ySat.subtract(new Vector3D(-FastMath.sin(alpha), FastMath.cos(alpha), 0)).getNorm(), 1.0e-10); Vector3D zSat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_K); Assert.assertEquals(0.0, zSat.subtract(Vector3D.PLUS_K).getNorm(), 1.0e-10); }
From source file:org.orekit.utils.TimeStampedAngularCoordinatesTest.java
@Test public void testSpin() throws OrekitException { double rate = 2 * FastMath.PI / (12 * 60); TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, new Rotation(0.48, 0.64, 0.36, 0.48, false), new Vector3D(rate, Vector3D.PLUS_K), Vector3D.ZERO); Assert.assertEquals(rate, ac.getRotationRate().getNorm(), 1.0e-10); double dt = 10.0; TimeStampedAngularCoordinates shifted = ac.shiftedBy(dt); Assert.assertEquals(rate, shifted.getRotationRate().getNorm(), 1.0e-10); Assert.assertEquals(rate * dt, Rotation.distance(ac.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 = ac.getRotation().applyInverseTo(Vector3D.PLUS_I); Vector3D originalY = ac.getRotation().applyInverseTo(Vector3D.PLUS_J); Vector3D originalZ = ac.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 = TimeStampedAngularCoordinates.estimateRate(ac.getRotation(), shifted.getRotation(), dt); Assert.assertEquals(0.0, forward.subtract(ac.getRotationRate()).getNorm(), 1.0e-10); Vector3D reversed = TimeStampedAngularCoordinates.estimateRate(shifted.getRotation(), ac.getRotation(), dt); Assert.assertEquals(0.0, reversed.add(ac.getRotationRate()).getNorm(), 1.0e-10); }
From source file:org.orekit.utils.TimeStampedAngularCoordinatesTest.java
@Test public void testNoCommute() { TimeStampedAngularCoordinates ac1 = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, new Rotation(0.48, 0.64, 0.36, 0.48, false), Vector3D.ZERO, Vector3D.ZERO); TimeStampedAngularCoordinates ac2 = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, new Rotation(0.36, -0.48, 0.48, 0.64, false), Vector3D.ZERO, Vector3D.ZERO); TimeStampedAngularCoordinates add12 = ac1.addOffset(ac2); TimeStampedAngularCoordinates add21 = ac2.addOffset(ac1); // the rotations are really different from each other Assert.assertEquals(2.574, Rotation.distance(add12.getRotation(), add21.getRotation()), 1.0e-3); }
From source file:org.orekit.utils.TimeStampedAngularCoordinatesTest.java
@Test public void testInterpolationAroundPI() throws OrekitException { List<TimeStampedAngularCoordinates> sample = new ArrayList<TimeStampedAngularCoordinates>(); // add angular coordinates at t0: 179.999 degrees rotation along X axis AbsoluteDate t0 = new AbsoluteDate("2012-01-01T00:00:00.000", TimeScalesFactory.getTAI()); TimeStampedAngularCoordinates ac0 = new TimeStampedAngularCoordinates(t0, new Rotation(Vector3D.PLUS_I, FastMath.toRadians(179.999)), new Vector3D(FastMath.toRadians(0), 0, 0), Vector3D.ZERO); sample.add(ac0);/*from w ww. j av a2 s .com*/ // add angular coordinates at t1: -179.999 degrees rotation (= 180.001 degrees) along X axis AbsoluteDate t1 = new AbsoluteDate("2012-01-01T00:00:02.000", TimeScalesFactory.getTAI()); TimeStampedAngularCoordinates ac1 = new TimeStampedAngularCoordinates(t1, new Rotation(Vector3D.PLUS_I, FastMath.toRadians(-179.999)), new Vector3D(FastMath.toRadians(0), 0, 0), Vector3D.ZERO); sample.add(ac1); // get interpolated angular coordinates at mid time between t0 and t1 AbsoluteDate t = new AbsoluteDate("2012-01-01T00:00:01.000", TimeScalesFactory.getTAI()); TimeStampedAngularCoordinates interpolated = TimeStampedAngularCoordinates.interpolate(t, AngularDerivativesFilter.USE_R, sample); Assert.assertEquals(FastMath.toRadians(180), interpolated.getRotation().getAngle(), 1.0e-12); }