List of usage examples for org.apache.commons.math3.geometry.euclidean.threed FieldVector3D FieldVector3D
public FieldVector3D(final T x, final T y, final T z)
From source file:org.orekit.utils.FieldAngularCoordinates.java
/** Builds a rotation/rotation rate pair. * @param rotation rotation// w w w . ja va 2 s . c om * @param rotationRate rotation rate (rad/s) */ public FieldAngularCoordinates(final FieldRotation<T> rotation, final FieldVector3D<T> rotationRate) { this(rotation, rotationRate, new FieldVector3D<T>(rotation.getQ0().getField().getZero(), rotation.getQ0().getField().getZero(), rotation.getQ0().getField().getZero())); }
From source file:org.orekit.utils.FieldAngularCoordinatesTest.java
private FieldVector3D<DerivativeStructure> createVector(double x, double y, double z, int params) { return new FieldVector3D<DerivativeStructure>(new DerivativeStructure(params, 1, 0, x), new DerivativeStructure(params, 1, 1, y), new DerivativeStructure(params, 1, 2, z)); }
From source file:org.orekit.utils.FieldPVCoordinates.java
/** Builds a PVCoordinates triplet with zero acceleration. * @param position the position vector (m) * @param velocity the velocity vector (m/s) *//*from w w w . j a v a 2 s.co m*/ public FieldPVCoordinates(final FieldVector3D<T> position, final FieldVector3D<T> velocity) { this.position = position; this.velocity = velocity; final T zero = position.getX().getField().getZero(); this.acceleration = new FieldVector3D<T>(zero, zero, zero); }
From source file:org.orekit.utils.FieldPVCoordinatesTest.java
@Test public void testGetMomentum() { //setup/*ww w. j a v a2 s .co m*/ DerivativeStructure oneDS = new DerivativeStructure(1, 1, 1); DerivativeStructure zeroDS = new DerivativeStructure(1, 1, 0); FieldVector3D<DerivativeStructure> zero = new FieldVector3D<DerivativeStructure>(zeroDS, zeroDS, zeroDS); FieldVector3D<DerivativeStructure> i = new FieldVector3D<DerivativeStructure>(oneDS, zeroDS, zeroDS); FieldVector3D<DerivativeStructure> j = new FieldVector3D<DerivativeStructure>(zeroDS, oneDS, zeroDS); FieldVector3D<DerivativeStructure> k = new FieldVector3D<DerivativeStructure>(zeroDS, zeroDS, oneDS); FieldVector3D<DerivativeStructure> p = new FieldVector3D<DerivativeStructure>(oneDS, new DerivativeStructure(1, 1, -2), new DerivativeStructure(1, 1, 3)); FieldVector3D<DerivativeStructure> v = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(1, 1, -9), new DerivativeStructure(1, 1, 8), new DerivativeStructure(1, 1, -7)); //action + verify Assert.assertEquals(new FieldPVCoordinates<DerivativeStructure>(p, v).getMomentum(), p.crossProduct(v)); //check simple cases Assert.assertEquals(new FieldPVCoordinates<DerivativeStructure>(i, i.scalarMultiply(-1)).getMomentum(), zero); Assert.assertEquals(new FieldPVCoordinates<DerivativeStructure>(i, j).getMomentum(), k); }
From source file:org.orekit.utils.FieldPVCoordinatesTest.java
@Test public void testGetAngularVelocity() { //setup//from ww w. ja v a 2 s . c om DerivativeStructure oneDS = new DerivativeStructure(1, 1, 1); DerivativeStructure zeroDS = new DerivativeStructure(1, 1, 0); FieldVector3D<DerivativeStructure> zero = new FieldVector3D<DerivativeStructure>(zeroDS, zeroDS, zeroDS); FieldVector3D<DerivativeStructure> i = new FieldVector3D<DerivativeStructure>(oneDS, zeroDS, zeroDS); FieldVector3D<DerivativeStructure> j = new FieldVector3D<DerivativeStructure>(zeroDS, oneDS, zeroDS); FieldVector3D<DerivativeStructure> k = new FieldVector3D<DerivativeStructure>(zeroDS, zeroDS, oneDS); FieldVector3D<DerivativeStructure> p = new FieldVector3D<DerivativeStructure>(oneDS, new DerivativeStructure(1, 1, -2), new DerivativeStructure(1, 1, 3)); FieldVector3D<DerivativeStructure> v = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(1, 1, -9), new DerivativeStructure(1, 1, 8), new DerivativeStructure(1, 1, -7)); //action + verify Assert.assertEquals(new FieldPVCoordinates<DerivativeStructure>(p, v).getAngularVelocity(), p.crossProduct(v).scalarMultiply(p.getNormSq().reciprocal())); //check extra simple cases Assert.assertEquals( new FieldPVCoordinates<DerivativeStructure>(i, i.scalarMultiply(-1)).getAngularVelocity(), zero); Assert.assertEquals( new FieldPVCoordinates<DerivativeStructure>(i.scalarMultiply(2), j).getAngularVelocity(), k.scalarMultiply(0.5)); }
From source file:org.orekit.utils.FieldPVCoordinatesTest.java
@Test @Deprecated // to be removed when FieldPVCoordinates.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(); List<Pair<AbsoluteDate, FieldPVCoordinates<DerivativeStructure>>> sample = new ArrayList<Pair<AbsoluteDate, FieldPVCoordinates<DerivativeStructure>>>(); for (double dt : new double[] { 0.0, 0.5, 1.0 }) { FieldVector3D<DerivativeStructure> position = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(3, 1, 0, px.value(dt)), new DerivativeStructure(3, 1, 1, py.value(dt)), new DerivativeStructure(3, 1, 2, pz.value(dt))); FieldVector3D<DerivativeStructure> velocity = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(3, 1, pxDot.value(dt)), new DerivativeStructure(3, 1, pyDot.value(dt)), new DerivativeStructure(3, 1, pzDot.value(dt))); sample.add(new Pair<AbsoluteDate, FieldPVCoordinates<DerivativeStructure>>(t0.shiftedBy(dt), new FieldPVCoordinates<DerivativeStructure>(position, velocity))); }//from w w w . j a va2s .c om for (double dt = 0; dt < 1.0; dt += 0.01) { FieldPVCoordinates<DerivativeStructure> interpolated = FieldPVCoordinates .interpolate(t0.shiftedBy(dt), true, sample); FieldVector3D<DerivativeStructure> p = interpolated.getPosition(); FieldVector3D<DerivativeStructure> v = interpolated.getVelocity(); Assert.assertEquals(px.value(dt), p.getX().getValue(), 1.0e-15 * p.getNorm().getValue()); Assert.assertEquals(1, p.getX().getPartialDerivative(1, 0, 0), 1.0e-15); Assert.assertEquals(0, p.getX().getPartialDerivative(0, 1, 0), 1.0e-15); Assert.assertEquals(0, p.getX().getPartialDerivative(0, 0, 1), 1.0e-15); Assert.assertEquals(py.value(dt), p.getY().getValue(), 1.0e-15 * p.getNorm().getValue()); Assert.assertEquals(0, p.getY().getPartialDerivative(1, 0, 0), 1.0e-15); Assert.assertEquals(1, p.getY().getPartialDerivative(0, 1, 0), 1.0e-15); Assert.assertEquals(0, p.getY().getPartialDerivative(0, 0, 1), 1.0e-15); Assert.assertEquals(pz.value(dt), p.getZ().getValue(), 1.0e-15 * p.getNorm().getValue()); Assert.assertEquals(0, p.getZ().getPartialDerivative(1, 0, 0), 1.0e-15); Assert.assertEquals(0, p.getZ().getPartialDerivative(0, 1, 0), 1.0e-15); Assert.assertEquals(1, p.getZ().getPartialDerivative(0, 0, 1), 1.0e-15); Assert.assertEquals(pxDot.value(dt), v.getX().getValue(), 1.0e-15 * v.getNorm().getValue()); Assert.assertEquals(pyDot.value(dt), v.getY().getValue(), 1.0e-15 * v.getNorm().getValue()); Assert.assertEquals(pzDot.value(dt), v.getZ().getValue(), 1.0e-15 * v.getNorm().getValue()); } } }
From source file:org.orekit.utils.PVCoordinates.java
/** Transform the instance to a {@link FieldVector3D}<{@link DerivativeStructure}>. * <p>/*from ww w . j a v a 2 s. c o m*/ * The {@link DerivativeStructure} coordinates correspond to time-derivatives up * to the user-specified order. * </p> * @param order derivation order for the vector components (must be either 0, 1 or 2) * @return vector with time-derivatives embedded within the coordinates * @exception OrekitException if the user specified order is too large */ public FieldVector3D<DerivativeStructure> toDerivativeStructureVector(final int order) throws OrekitException { final DerivativeStructure x; final DerivativeStructure y; final DerivativeStructure z; switch (order) { case 0: x = new DerivativeStructure(1, 0, position.getX()); y = new DerivativeStructure(1, 0, position.getY()); z = new DerivativeStructure(1, 0, position.getZ()); break; case 1: x = new DerivativeStructure(1, 1, position.getX(), velocity.getX()); y = new DerivativeStructure(1, 1, position.getY(), velocity.getY()); z = new DerivativeStructure(1, 1, position.getZ(), velocity.getZ()); break; case 2: x = new DerivativeStructure(1, 2, position.getX(), velocity.getX(), acceleration.getX()); y = new DerivativeStructure(1, 2, position.getY(), velocity.getY(), acceleration.getY()); z = new DerivativeStructure(1, 2, position.getZ(), velocity.getZ(), acceleration.getZ()); break; default: throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order); } return new FieldVector3D<DerivativeStructure>(x, y, z); }
From source file:org.orekit.utils.PVCoordinatesDSTest.java
@Test 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(); List<Pair<AbsoluteDate, FieldPVCoordinates<DerivativeStructure>>> sample = new ArrayList<Pair<AbsoluteDate, FieldPVCoordinates<DerivativeStructure>>>(); for (double dt : new double[] { 0.0, 0.5, 1.0 }) { FieldVector3D<DerivativeStructure> position = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(3, 1, 0, px.value(dt)), new DerivativeStructure(3, 1, 1, py.value(dt)), new DerivativeStructure(3, 1, 2, pz.value(dt))); FieldVector3D<DerivativeStructure> velocity = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(3, 1, pxDot.value(dt)), new DerivativeStructure(3, 1, pyDot.value(dt)), new DerivativeStructure(3, 1, pzDot.value(dt))); sample.add(new Pair<AbsoluteDate, FieldPVCoordinates<DerivativeStructure>>(t0.shiftedBy(dt), new FieldPVCoordinates<DerivativeStructure>(position, velocity))); }/*from w w w .j a v a 2 s.c om*/ for (double dt = 0; dt < 1.0; dt += 0.01) { FieldPVCoordinates<DerivativeStructure> interpolated = FieldPVCoordinates .interpolate(t0.shiftedBy(dt), true, sample); FieldVector3D<DerivativeStructure> p = interpolated.getPosition(); FieldVector3D<DerivativeStructure> v = interpolated.getVelocity(); Assert.assertEquals(px.value(dt), p.getX().getValue(), 1.0e-15 * p.getNorm().getValue()); Assert.assertEquals(1, p.getX().getPartialDerivative(1, 0, 0), 1.0e-15); Assert.assertEquals(0, p.getX().getPartialDerivative(0, 1, 0), 1.0e-15); Assert.assertEquals(0, p.getX().getPartialDerivative(0, 0, 1), 1.0e-15); Assert.assertEquals(py.value(dt), p.getY().getValue(), 1.0e-15 * p.getNorm().getValue()); Assert.assertEquals(0, p.getY().getPartialDerivative(1, 0, 0), 1.0e-15); Assert.assertEquals(1, p.getY().getPartialDerivative(0, 1, 0), 1.0e-15); Assert.assertEquals(0, p.getY().getPartialDerivative(0, 0, 1), 1.0e-15); Assert.assertEquals(pz.value(dt), p.getZ().getValue(), 1.0e-15 * p.getNorm().getValue()); Assert.assertEquals(0, p.getZ().getPartialDerivative(1, 0, 0), 1.0e-15); Assert.assertEquals(0, p.getZ().getPartialDerivative(0, 1, 0), 1.0e-15); Assert.assertEquals(1, p.getZ().getPartialDerivative(0, 0, 1), 1.0e-15); Assert.assertEquals(pxDot.value(dt), v.getX().getValue(), 1.0e-15 * v.getNorm().getValue()); Assert.assertEquals(pyDot.value(dt), v.getY().getValue(), 1.0e-15 * v.getNorm().getValue()); Assert.assertEquals(pzDot.value(dt), v.getZ().getValue(), 1.0e-15 * v.getNorm().getValue()); } } }
From source file:org.orekit.utils.TimeStampedFieldAngularCoordinates.java
/** Interpolate angular coordinates. * <p>//from w w w .j a v a2 s .c o m * The interpolated instance is created by polynomial Hermite interpolation * on Rodrigues vector ensuring FieldRotation<T> rate remains the exact derivative of FieldRotation<T>. * </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 FieldRotation<T>s * when this singularity is detected. * </p> * <p> * Note that even if first time derivatives (FieldRotation<T> rates) * 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 FieldRotation<T>s. * </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 * @param <T> the type of the field elements * @return a new position-velocity, interpolated at specified date * @exception OrekitException if the number of point is too small for interpolating */ @SuppressWarnings("unchecked") public static <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> interpolate( final AbsoluteDate date, final AngularDerivativesFilter filter, final Collection<TimeStampedFieldAngularCoordinates<T>> sample) throws OrekitException { // get field properties final Field<T> field = sample.iterator().next().getRotation().getQ0().getField(); final T zero = field.getZero(); final T one = field.getOne(); // 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 FieldVector3D<T> meanRate; if (filter != AngularDerivativesFilter.USE_R) { FieldVector3D<T> sum = new FieldVector3D<T>(zero, zero, zero); for (final TimeStampedFieldAngularCoordinates<T> datedAC : sample) { sum = sum.add(datedAC.getRotationRate()); } meanRate = new FieldVector3D<T>(1.0 / sample.size(), sum); } else { if (sample.size() < 2) { throw new OrekitException(OrekitMessages.NOT_ENOUGH_DATA_FOR_INTERPOLATION, sample.size()); } FieldVector3D<T> sum = new FieldVector3D<T>(zero, zero, zero); TimeStampedFieldAngularCoordinates<T> previous = null; for (final TimeStampedFieldAngularCoordinates<T> datedAC : sample) { if (previous != null) { sum = sum.add(estimateRate(previous.getRotation(), datedAC.getRotation(), datedAC.date.durationFrom(previous.getDate()))); } previous = datedAC; } meanRate = new FieldVector3D<T>(1.0 / (sample.size() - 1), sum); } TimeStampedFieldAngularCoordinates<T> offset = new TimeStampedFieldAngularCoordinates<T>(date, new FieldRotation<T>(one, zero, zero, zero, false), meanRate, new FieldVector3D<T>(zero, zero, 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 FieldHermiteInterpolator<T> interpolator = new FieldHermiteInterpolator<T>(); // add sample points final double[] previous = new double[] { 1.0, 0.0, 0.0, 0.0 }; for (final TimeStampedFieldAngularCoordinates<T> ac : sample) { // remove linear offset from the current coordinates final T dt = zero.add(ac.date.durationFrom(date)); final TimeStampedFieldAngularCoordinates<T> fixed = ac .subtractOffset(offset.shiftedBy(dt.getReal())); final T[][] rodrigues = getModifiedRodrigues(fixed, previous, threshold); if (rodrigues == null) { // 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; } 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 FieldAngularCoordinates<T>( new FieldRotation<T>(new FieldVector3D<T>(one, zero, zero), zero.add(epsilon)), new FieldVector3D<T>(zero, zero, zero), new FieldVector3D<T>(zero, zero, zero))); } else { // interpolation succeeded with the current offset final T[][] p = interpolator.derivatives(field.getZero(), 2); return createFromModifiedRodrigues(p, offset); } } // this should never happen throw new OrekitInternalError(null); }
From source file:org.orekit.utils.TimeStampedFieldAngularCoordinates.java
/** Convert a modified Rodrigues vector and derivatives to angular coordinates. * @param r modified Rodrigues vector (with first derivatives) * @param offset linear offset model to add (its date must be consistent with the modified Rodrigues vector) * @param <T> the type of the field elements * @return angular coordinates/*w w w . j a v a 2 s .c o m*/ */ private static <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> createFromModifiedRodrigues( final T[][] r, final TimeStampedFieldAngularCoordinates<T> offset) { // rotation final T rSquared = r[0][0].multiply(r[0][0]).add(r[0][1].multiply(r[0][1])).add(r[0][2].multiply(r[0][2])); final T oPQ0 = rSquared.add(1).reciprocal().multiply(2); final T q0 = oPQ0.subtract(1); final T q1 = oPQ0.multiply(r[0][0]); final T q2 = oPQ0.multiply(r[0][1]); final T q3 = oPQ0.multiply(r[0][2]); // rotation rate final T oPQ02 = oPQ0.multiply(oPQ0); final T q0Dot = oPQ02.negate() .multiply(q0.linearCombination(r[0][0], r[1][0], r[0][1], r[1][1], r[0][2], r[1][2])); final T q1Dot = oPQ0.multiply(r[1][0]).add(r[0][0].multiply(q0Dot)); final T q2Dot = oPQ0.multiply(r[1][1]).add(r[0][1].multiply(q0Dot)); final T q3Dot = oPQ0.multiply(r[1][2]).add(r[0][2].multiply(q0Dot)); final T oX = q1.linearCombination(q1.negate(), q0Dot, q0, q1Dot, q3, q2Dot, q2.negate(), q3Dot).multiply(2); final T oY = q2.linearCombination(q2.negate(), q0Dot, q3.negate(), q1Dot, q0, q2Dot, q1, q3Dot).multiply(2); final T oZ = q3.linearCombination(q3.negate(), q0Dot, q2, q1Dot, q1.negate(), q2Dot, q0, q3Dot).multiply(2); // rotation acceleration final T q0DotDot = q0.getField().getOne().subtract(q0).divide(oPQ0).multiply(q0Dot).multiply(q0Dot) .subtract( oPQ02.multiply(q0.linearCombination(r[0][0], r[2][0], r[0][1], r[2][1], r[0][2], r[2][2]))) .subtract(q1Dot.multiply(q1Dot).add(q2Dot.multiply(q2Dot)).add(q3Dot.multiply(q3Dot))); final T q1DotDot = q1.linearCombination(oPQ0, r[2][0], r[1][0].multiply(2), q0Dot, r[0][0], q0DotDot); final T q2DotDot = q2.linearCombination(oPQ0, r[2][1], r[1][1].multiply(2), q0Dot, r[0][1], q0DotDot); final T q3DotDot = q3.linearCombination(oPQ0, r[2][2], r[1][2].multiply(2), q0Dot, r[0][2], q0DotDot); final T oXDot = q1 .linearCombination(q1.negate(), q0DotDot, q0, q1DotDot, q3, q2DotDot, q2.negate(), q3DotDot) .multiply(2); final T oYDot = q2 .linearCombination(q2.negate(), q0DotDot, q3.negate(), q1DotDot, q0, q2DotDot, q1, q3DotDot) .multiply(2); final T oZDot = q3 .linearCombination(q3.negate(), q0DotDot, q2, q1DotDot, q1.negate(), q2DotDot, q0, q3DotDot) .multiply(2); return new TimeStampedFieldAngularCoordinates<T>(offset.getDate(), new FieldRotation<T>(q0, q1, q2, q3, false), new FieldVector3D<T>(oX, oY, oZ), new FieldVector3D<T>(oXDot, oYDot, oZDot)).addOffset(offset); }