Examples with PolynomialFunction org.hipparchus.analysis.polynomials.PolynomialFunction used on opensource projects

Example 16 with PolynomialFunction

use of org.hipparchus.analysis.polynomials.PolynomialFunction in project Orekit by CS-SI.

the class TimeStampedFieldPVCoordinatesTest method testInterpolatePolynomialPVA.

@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);
            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<>(t0.shiftedBy(dt), position, velocity, acceleration));
        }
        Field<DerivativeStructure> field = sample.get(0).getDate().getField();
        for (double dt = 0; dt < 1.0; dt += 0.01) {
            FieldAbsoluteDate<DerivativeStructure> t = new FieldAbsoluteDate<>(field, t0.shiftedBy(dt));
            TimeStampedFieldPVCoordinates<DerivativeStructure> interpolated = TimeStampedFieldPVCoordinates.interpolate(t, 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());
        }
    }
}