Example usage for org.apache.commons.math3.geometry.euclidean.threed FieldVector3D getNorm

List of usage examples for org.apache.commons.math3.geometry.euclidean.threed FieldVector3D getNorm

Introduction

In this page you can find the example usage for org.apache.commons.math3.geometry.euclidean.threed FieldVector3D getNorm.

Prototype

public T getNorm() 

Source Link

Document

Get the L2 norm for the vector.

Usage

From source file:org.orekit.forces.BoxAndSolarArraySpacecraft.java

/** Compute contribution of one facet to force.
 * <p>This method implements equation 8-44 from David A. Vallado's
 * Fundamentals of Astrodynamics and Applications, third edition,
 * 2007, Microcosm Press.</p>//from   w w  w.  j a  v  a  2 s. c  o  m
 * @param normal facet normal
 * @param area facet area
 * @param fluxSat radiation pressure flux in spacecraft frame
 * @param dot dot product of facet and fluxSat (must be negative)
 * @return contribution of the facet to force in spacecraft frame
 */
private FieldVector3D<DerivativeStructure> facetRadiationAcceleration(
        final FieldVector3D<DerivativeStructure> normal, final double area,
        final FieldVector3D<DerivativeStructure> fluxSat, final DerivativeStructure dot) {
    final DerivativeStructure psr = fluxSat.getNorm();

    // Vallado's equation 8-44 uses different parameters which are related to our parameters as:
    // cos (phi) = -dot / (psr * area)
    // n         = facet / area
    // s         = -fluxSat / psr
    final DerivativeStructure cN = dot.multiply(-2 * area)
            .multiply(dot.divide(psr).multiply(specularReflectionCoeff).subtract(diffuseReflectionCoeff / 3));
    final DerivativeStructure cS = dot.divide(psr).multiply(area * (specularReflectionCoeff - 1));
    return new FieldVector3D<DerivativeStructure>(cN, normal, cS, fluxSat);

}

From source file:org.orekit.forces.SphericalSpacecraft.java

/** {@inheritDoc} */
public FieldVector3D<DerivativeStructure> dragAcceleration(final AbsoluteDate date, final Frame frame,
        final FieldVector3D<DerivativeStructure> position, final FieldRotation<DerivativeStructure> rotation,
        final DerivativeStructure mass, final double density,
        final FieldVector3D<DerivativeStructure> relativeVelocity) {
    return new FieldVector3D<DerivativeStructure>(
            relativeVelocity.getNorm().multiply(density * dragCoeff * crossSection / 2).divide(mass),
            relativeVelocity);//from  w  w  w .  j a v a  2 s. com
}

From source file:org.orekit.propagation.events.ElevationExtremumDetector.java

/** Compute the value of the detection function.
 * <p>/*from ww w.  ja v a2  s.co m*/
 * The value is the spacecraft elevation first time derivative.
 * </p>
 * @param s the current state information: date, kinematics, attitude
 * @return spacecraft elevation first time derivative
 * @exception OrekitException if some specific error occurs
 */
public double g(final SpacecraftState s) throws OrekitException {

    // get position, velocity acceleration of spacecraft in topocentric frame
    final Transform inertToTopo = s.getFrame().getTransformTo(topo, s.getDate());
    final TimeStampedPVCoordinates pvTopo = inertToTopo.transformPVCoordinates(s.getPVCoordinates());

    // convert the coordinates to DerivativeStructure based vector
    // instead of having vector position, then vector velocity then vector acceleration
    // we get one vector and each coordinate is a DerivativeStructure containing
    // value, first time derivative (we don't need second time derivative here)
    final FieldVector3D<DerivativeStructure> pvDS = pvTopo.toDerivativeStructureVector(1);

    // compute elevation and its first time derivative
    final DerivativeStructure elevation = pvDS.getZ().divide(pvDS.getNorm()).asin();

    // return elevation first time derivative
    return elevation.getPartialDerivative(1);

}

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)));
        }/*  w  w  w  . java2  s  . co m*/

        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.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)));
        }/*  w  ww.ja va2  s. c  o m*/

        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.PVCoordinatesDSTest.java

@Test
public void testInterpolatePolynomialPositionOnly() {
    Random random = new Random(0x88740a12e4299003l);
    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.2, 0.4, 0.6, 0.8, 1.0 }) {
            FieldVector3D<DerivativeStructure> position = createVector(px.value(dt), py.value(dt), pz.value(dt),
                    6);//from   w  w  w.  j ava 2  s . c  om
            sample.add(new Pair<AbsoluteDate, FieldPVCoordinates<DerivativeStructure>>(t0.shiftedBy(dt),
                    new FieldPVCoordinates<DerivativeStructure>(position, createVector(0, 0, 0, 6))));
        }

        for (double dt = 0; dt < 1.0; dt += 0.01) {
            FieldPVCoordinates<DerivativeStructure> interpolated = FieldPVCoordinates
                    .interpolate(t0.shiftedBy(dt), false, sample);
            FieldVector3D<DerivativeStructure> p = interpolated.getPosition();
            FieldVector3D<DerivativeStructure> v = interpolated.getVelocity();
            Assert.assertEquals(px.value(dt), p.getX().getValue(), 1.0e-14 * p.getNorm().getValue());
            Assert.assertEquals(py.value(dt), p.getY().getValue(), 1.0e-14 * p.getNorm().getValue());
            Assert.assertEquals(pz.value(dt), p.getZ().getValue(), 1.0e-14 * p.getNorm().getValue());
            Assert.assertEquals(pxDot.value(dt), v.getX().getValue(), 1.0e-14 * v.getNorm().getValue());
            Assert.assertEquals(pyDot.value(dt), v.getY().getValue(), 1.0e-14 * v.getNorm().getValue());
            Assert.assertEquals(pzDot.value(dt), v.getZ().getValue(), 1.0e-14 * v.getNorm().getValue());
        }

    }
}

From source file:org.orekit.utils.PVCoordinatesDSTest.java

@Test
public void testInterpolateNonPolynomial() {
    AbsoluteDate t0 = AbsoluteDate.J2000_EPOCH;

    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 = createVector(FastMath.cos(dt), FastMath.sin(dt), 0.0, 6);
        FieldVector3D<DerivativeStructure> velocity = createVector(-FastMath.sin(dt), FastMath.cos(dt), 0.0, 6);
        sample.add(new Pair<AbsoluteDate, FieldPVCoordinates<DerivativeStructure>>(t0.shiftedBy(dt),
                new FieldPVCoordinates<DerivativeStructure>(position, velocity)));
    }/*ww  w . ja  v a  2 s  .  c o m*/

    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(FastMath.cos(dt), p.getX().getValue(), 3.0e-6 * p.getNorm().getValue());
        Assert.assertEquals(FastMath.sin(dt), p.getY().getValue(), 3.0e-6 * p.getNorm().getValue());
        Assert.assertEquals(0, p.getZ().getValue(), 3.0e-6 * p.getNorm().getValue());
        Assert.assertEquals(-FastMath.sin(dt), v.getX().getValue(), 3.0e-5 * v.getNorm().getValue());
        Assert.assertEquals(FastMath.cos(dt), v.getY().getValue(), 3.0e-5 * v.getNorm().getValue());
        Assert.assertEquals(0, v.getZ().getValue(), 3.0e-5 * v.getNorm().getValue());
    }

}

From source file:org.orekit.utils.TimeStampedFieldPVCoordinatesTest.java

@Test
public void testInterpolatePolynomialPVA() {
    Random random = new Random(0xfe3945fcb8bf47cel);
    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<TimeStampedFieldPVCoordinates<DerivativeStructure>> sample = new ArrayList<TimeStampedFieldPVCoordinates<DerivativeStructure>>();
        for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
            FieldVector3D<DerivativeStructure> position = createVector(px.value(dt), py.value(dt), pz.value(dt),
                    4);//  www.jav a2s  . c o m
            FieldVector3D<DerivativeStructure> velocity = createVector(pxDot.value(dt), pyDot.value(dt),
                    pzDot.value(dt), 4);
            FieldVector3D<DerivativeStructure> acceleration = createVector(pxDotDot.value(dt),
                    pyDotDot.value(dt), pzDotDot.value(dt), 4);
            sample.add(new TimeStampedFieldPVCoordinates<DerivativeStructure>(t0.shiftedBy(dt), position,
                    velocity, acceleration));
        }

        for (double dt = 0; dt < 1.0; dt += 0.01) {
            TimeStampedFieldPVCoordinates<DerivativeStructure> interpolated = TimeStampedFieldPVCoordinates
                    .interpolate(t0.shiftedBy(dt), CartesianDerivativesFilter.USE_PVA, sample);
            FieldVector3D<DerivativeStructure> p = interpolated.getPosition();
            FieldVector3D<DerivativeStructure> v = interpolated.getVelocity();
            FieldVector3D<DerivativeStructure> a = interpolated.getAcceleration();
            Assert.assertEquals(px.value(dt), p.getX().getReal(), 4.0e-16 * p.getNorm().getReal());
            Assert.assertEquals(py.value(dt), p.getY().getReal(), 4.0e-16 * p.getNorm().getReal());
            Assert.assertEquals(pz.value(dt), p.getZ().getReal(), 4.0e-16 * p.getNorm().getReal());
            Assert.assertEquals(pxDot.value(dt), v.getX().getReal(), 9.0e-16 * v.getNorm().getReal());
            Assert.assertEquals(pyDot.value(dt), v.getY().getReal(), 9.0e-16 * v.getNorm().getReal());
            Assert.assertEquals(pzDot.value(dt), v.getZ().getReal(), 9.0e-16 * v.getNorm().getReal());
            Assert.assertEquals(pxDotDot.value(dt), a.getX().getReal(), 6.0e-15 * a.getNorm().getReal());
            Assert.assertEquals(pyDotDot.value(dt), a.getY().getReal(), 6.0e-15 * a.getNorm().getReal());
            Assert.assertEquals(pzDotDot.value(dt), a.getZ().getReal(), 6.0e-15 * a.getNorm().getReal());
        }

    }

}

From source file:org.orekit.utils.TimeStampedFieldPVCoordinatesTest.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();
        PolynomialFunction pxDotDot = pxDot.polynomialDerivative();
        PolynomialFunction pyDotDot = pyDot.polynomialDerivative();
        PolynomialFunction pzDotDot = pzDot.polynomialDerivative();

        List<TimeStampedFieldPVCoordinates<DerivativeStructure>> sample = new ArrayList<TimeStampedFieldPVCoordinates<DerivativeStructure>>();
        for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
            FieldVector3D<DerivativeStructure> position = createVector(px.value(dt), py.value(dt), pz.value(dt),
                    4);/*from   w  w w . j a  va  2 s. co  m*/
            FieldVector3D<DerivativeStructure> velocity = createVector(pxDot.value(dt), pyDot.value(dt),
                    pzDot.value(dt), 4);
            sample.add(new TimeStampedFieldPVCoordinates<DerivativeStructure>(t0.shiftedBy(dt), position,
                    velocity, createVector(0, 0, 0, 4)));
        }

        for (double dt = 0; dt < 1.0; dt += 0.01) {
            TimeStampedFieldPVCoordinates<DerivativeStructure> interpolated = TimeStampedFieldPVCoordinates
                    .interpolate(t0.shiftedBy(dt), CartesianDerivativesFilter.USE_PV, sample);
            FieldVector3D<DerivativeStructure> p = interpolated.getPosition();
            FieldVector3D<DerivativeStructure> v = interpolated.getVelocity();
            FieldVector3D<DerivativeStructure> a = interpolated.getAcceleration();
            Assert.assertEquals(px.value(dt), p.getX().getReal(), 4.0e-16 * p.getNorm().getReal());
            Assert.assertEquals(py.value(dt), p.getY().getReal(), 4.0e-16 * p.getNorm().getReal());
            Assert.assertEquals(pz.value(dt), p.getZ().getReal(), 4.0e-16 * p.getNorm().getReal());
            Assert.assertEquals(pxDot.value(dt), v.getX().getReal(), 9.0e-16 * v.getNorm().getReal());
            Assert.assertEquals(pyDot.value(dt), v.getY().getReal(), 9.0e-16 * v.getNorm().getReal());
            Assert.assertEquals(pzDot.value(dt), v.getZ().getReal(), 9.0e-16 * v.getNorm().getReal());
            Assert.assertEquals(pxDotDot.value(dt), a.getX().getReal(), 1.0e-14 * a.getNorm().getReal());
            Assert.assertEquals(pyDotDot.value(dt), a.getY().getReal(), 1.0e-14 * a.getNorm().getReal());
            Assert.assertEquals(pzDotDot.value(dt), a.getZ().getReal(), 1.0e-14 * a.getNorm().getReal());
        }

    }

}

From source file:org.orekit.utils.TimeStampedFieldPVCoordinatesTest.java

@Test
public void testInterpolatePolynomialPositionOnly() {
    Random random = new Random(0x88740a12e4299003l);
    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<TimeStampedFieldPVCoordinates<DerivativeStructure>> sample = new ArrayList<TimeStampedFieldPVCoordinates<DerivativeStructure>>();
        for (double dt : new double[] { 0.0, 0.2, 0.4, 0.6, 0.8, 1.0 }) {
            FieldVector3D<DerivativeStructure> position = createVector(px.value(dt), py.value(dt), pz.value(dt),
                    4);//from  w  w w. j  a va  2  s  . c  om
            sample.add(new TimeStampedFieldPVCoordinates<DerivativeStructure>(t0.shiftedBy(dt), position,
                    createVector(0, 0, 0, 4), createVector(0, 0, 0, 4)));
        }

        for (double dt = 0; dt < 1.0; dt += 0.01) {
            TimeStampedFieldPVCoordinates<DerivativeStructure> interpolated = TimeStampedFieldPVCoordinates
                    .interpolate(t0.shiftedBy(dt), CartesianDerivativesFilter.USE_P, sample);
            FieldVector3D<DerivativeStructure> p = interpolated.getPosition();
            FieldVector3D<DerivativeStructure> v = interpolated.getVelocity();
            FieldVector3D<DerivativeStructure> a = interpolated.getAcceleration();
            Assert.assertEquals(px.value(dt), p.getX().getReal(), 5.0e-16 * p.getNorm().getReal());
            Assert.assertEquals(py.value(dt), p.getY().getReal(), 5.0e-16 * p.getNorm().getReal());
            Assert.assertEquals(pz.value(dt), p.getZ().getReal(), 5.0e-16 * p.getNorm().getReal());
            Assert.assertEquals(pxDot.value(dt), v.getX().getReal(), 7.0e-15 * v.getNorm().getReal());
            Assert.assertEquals(pyDot.value(dt), v.getY().getReal(), 7.0e-15 * v.getNorm().getReal());
            Assert.assertEquals(pzDot.value(dt), v.getZ().getReal(), 7.0e-15 * v.getNorm().getReal());
            Assert.assertEquals(pxDotDot.value(dt), a.getX().getReal(), 2.0e-13 * a.getNorm().getReal());
            Assert.assertEquals(pyDotDot.value(dt), a.getY().getReal(), 2.0e-13 * a.getNorm().getReal());
            Assert.assertEquals(pzDotDot.value(dt), a.getZ().getReal(), 2.0e-13 * a.getNorm().getReal());
        }

    }
}