Examples with DSFactory org.hipparchus.analysis.differentiation.DSFactory used on opensource projects

Example 66 with DSFactory

use of org.hipparchus.analysis.differentiation.DSFactory in project Orekit by CS-SI.

the class PVCoordinates method toDerivativeStructurePV.

/**
 * Transform the instance to a {@link FieldPVCoordinates}<{@link DerivativeStructure}>.
 * <p>
 * The {@link DerivativeStructure} coordinates correspond to time-derivatives up
 * to the user-specified order. As both the instance components {@link #getPosition() position},
 * {@link #getVelocity() velocity} and {@link #getAcceleration() acceleration} and the
 * {@link DerivativeStructure#getPartialDerivative(int...) derivatives} of the components
 * holds time-derivatives, there are several ways to retrieve these derivatives. If for example
 * the {@code order} is set to 2, then both {@code pv.getPosition().getX().getPartialDerivative(2)},
 * {@code pv.getVelocity().getX().getPartialDerivative(1)} and
 * {@code pv.getAcceleration().getX().getValue()} return the exact same value.
 * </p>
 * <p>
 * If derivation order is 1, the first derivative of acceleration will be computed as a
 * Keplerian-only jerk. If derivation order is 2, the second derivative of velocity (which
 * is also the first derivative of acceleration) will be computed as a Keplerian-only jerk,
 * and the second derivative of acceleration will be computed as a Keplerian-only jounce.
 * </p>
 * @param order derivation order for the vector components (must be either 0, 1 or 2)
 * @return pv coordinates with time-derivatives embedded within the coordinates
 * @exception OrekitException if the user specified order is too large
 * @since 9.2
 */
public FieldPVCoordinates<DerivativeStructure> toDerivativeStructurePV(final int order) throws OrekitException {
    final DSFactory factory;
    final DerivativeStructure x0;
    final DerivativeStructure y0;
    final DerivativeStructure z0;
    final DerivativeStructure x1;
    final DerivativeStructure y1;
    final DerivativeStructure z1;
    final DerivativeStructure x2;
    final DerivativeStructure y2;
    final DerivativeStructure z2;
    switch(order) {
        case 0:
            factory = new DSFactory(1, order);
            x0 = factory.build(position.getX());
            y0 = factory.build(position.getY());
            z0 = factory.build(position.getZ());
            x1 = factory.build(velocity.getX());
            y1 = factory.build(velocity.getY());
            z1 = factory.build(velocity.getZ());
            x2 = factory.build(acceleration.getX());
            y2 = factory.build(acceleration.getY());
            z2 = factory.build(acceleration.getZ());
            break;
        case 1:
            {
                factory = new DSFactory(1, order);
                final double r2 = position.getNormSq();
                final double r = FastMath.sqrt(r2);
                final double pvOr2 = Vector3D.dotProduct(position, velocity) / r2;
                final double a = acceleration.getNorm();
                final double aOr = a / r;
                final Vector3D keplerianJerk = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
                x0 = factory.build(position.getX(), velocity.getX());
                y0 = factory.build(position.getY(), velocity.getY());
                z0 = factory.build(position.getZ(), velocity.getZ());
                x1 = factory.build(velocity.getX(), acceleration.getX());
                y1 = factory.build(velocity.getY(), acceleration.getY());
                z1 = factory.build(velocity.getZ(), acceleration.getZ());
                x2 = factory.build(acceleration.getX(), keplerianJerk.getX());
                y2 = factory.build(acceleration.getY(), keplerianJerk.getY());
                z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ());
                break;
            }
        case 2:
            {
                factory = new DSFactory(1, order);
                final double r2 = position.getNormSq();
                final double r = FastMath.sqrt(r2);
                final double pvOr2 = Vector3D.dotProduct(position, velocity) / r2;
                final double a = acceleration.getNorm();
                final double aOr = a / r;
                final Vector3D keplerianJerk = new Vector3D(-3 * pvOr2, acceleration, -aOr, velocity);
                final double v2 = velocity.getNormSq();
                final double pa = Vector3D.dotProduct(position, acceleration);
                final double aj = Vector3D.dotProduct(acceleration, keplerianJerk);
                final Vector3D keplerianJounce = new Vector3D(-3 * (v2 + pa) / r2 + 15 * pvOr2 * pvOr2 - aOr, acceleration, 4 * aOr * pvOr2 - aj / (a * r), velocity);
                x0 = factory.build(position.getX(), velocity.getX(), acceleration.getX());
                y0 = factory.build(position.getY(), velocity.getY(), acceleration.getY());
                z0 = factory.build(position.getZ(), velocity.getZ(), acceleration.getZ());
                x1 = factory.build(velocity.getX(), acceleration.getX(), keplerianJerk.getX());
                y1 = factory.build(velocity.getY(), acceleration.getY(), keplerianJerk.getY());
                z1 = factory.build(velocity.getZ(), acceleration.getZ(), keplerianJerk.getZ());
                x2 = factory.build(acceleration.getX(), keplerianJerk.getX(), keplerianJounce.getX());
                y2 = factory.build(acceleration.getY(), keplerianJerk.getY(), keplerianJounce.getY());
                z2 = factory.build(acceleration.getZ(), keplerianJerk.getZ(), keplerianJounce.getZ());
                break;
            }
        default:
            throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
    }
    return new FieldPVCoordinates<>(new FieldVector3D<>(x0, y0, z0), new FieldVector3D<>(x1, y1, z1), new FieldVector3D<>(x2, y2, z2));
}

Example 67 with DSFactory

use of org.hipparchus.analysis.differentiation.DSFactory in project Orekit by CS-SI.

the class FramesFactoryTest method doTestDerivatives.

private void doTestDerivatives(AbsoluteDate ref, double duration, double step, boolean forbidInterpolation, double cartesianTolerance, double cartesianDotTolerance, double cartesianDotDotTolerance, double rodriguesTolerance, double rodriguesDotTolerance, double rodriguesDotDotTolerance) throws OrekitException {
    final DSFactory factory = new DSFactory(1, 2);
    final FieldAbsoluteDate<DerivativeStructure> refDS = new FieldAbsoluteDate<>(factory.getDerivativeField(), ref);
    FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 60.0);
    for (final Predefined predefined : Predefined.values()) {
        final Frame frame = FramesFactory.getFrame(predefined);
        final Frame parent = frame.getParent();
        if (parent != null) {
            UnivariateDifferentiableVectorFunction dCartesian = differentiator.differentiate(new UnivariateVectorFunction() {

                @Override
                public double[] value(double t) {
                    try {
                        return forbidInterpolation ? FramesFactory.getNonInterpolatingTransform(parent, frame, ref.shiftedBy(t)).getTranslation().toArray() : parent.getTransformTo(frame, ref.shiftedBy(t)).getTranslation().toArray();
                    } catch (OrekitException oe) {
                        throw new OrekitExceptionWrapper(oe);
                    }
                }
            });
            UnivariateDifferentiableVectorFunction dOrientation = differentiator.differentiate(new UnivariateVectorFunction() {

                double sign = +1.0;

                Rotation previous = Rotation.IDENTITY;

                @Override
                public double[] value(double t) {
                    try {
                        AngularCoordinates ac = forbidInterpolation ? FramesFactory.getNonInterpolatingTransform(parent, frame, ref.shiftedBy(t)).getAngular() : parent.getTransformTo(frame, ref.shiftedBy(t)).getAngular();
                        final double dot = MathArrays.linearCombination(ac.getRotation().getQ0(), previous.getQ0(), ac.getRotation().getQ1(), previous.getQ1(), ac.getRotation().getQ2(), previous.getQ2(), ac.getRotation().getQ3(), previous.getQ3());
                        sign = FastMath.copySign(1.0, dot * sign);
                        previous = ac.getRotation();
                        return ac.getModifiedRodrigues(sign)[0];
                    } catch (OrekitException oe) {
                        throw new OrekitExceptionWrapper(oe);
                    }
                }
            });
            double maxCartesianError = 0;
            double maxCartesianDotError = 0;
            double maxCartesianDotDotError = 0;
            double maxRodriguesError = 0;
            double maxRodriguesDotError = 0;
            double maxRodriguesDotDotError = 0;
            for (double dt = 0; dt < duration; dt += step) {
                final DerivativeStructure dtDS = factory.variable(0, dt);
                final FieldTransform<DerivativeStructure> tDS = forbidInterpolation ? FramesFactory.getNonInterpolatingTransform(parent, frame, refDS.shiftedBy(dtDS)) : parent.getTransformTo(frame, refDS.shiftedBy(dtDS));
                final DerivativeStructure[] refCart = dCartesian.value(dtDS);
                final DerivativeStructure[] fieldCart = tDS.getTranslation().toArray();
                for (int i = 0; i < 3; ++i) {
                    maxCartesianError = FastMath.max(maxCartesianError, FastMath.abs(refCart[i].getValue() - fieldCart[i].getValue()));
                    maxCartesianDotError = FastMath.max(maxCartesianDotError, FastMath.abs(refCart[i].getPartialDerivative(1) - fieldCart[i].getPartialDerivative(1)));
                    maxCartesianDotDotError = FastMath.max(maxCartesianDotDotError, FastMath.abs(refCart[i].getPartialDerivative(2) - fieldCart[i].getPartialDerivative(2)));
                }
                final DerivativeStructure[] refOr = dOrientation.value(dtDS);
                DerivativeStructure[] fieldOr = tDS.getAngular().getModifiedRodrigues(1.0)[0];
                final double dot = refOr[0].linearCombination(refOr, fieldOr).getReal();
                if (dot < 0 || Double.isNaN(dot)) {
                    fieldOr = tDS.getAngular().getModifiedRodrigues(-1.0)[0];
                }
                for (int i = 0; i < 3; ++i) {
                    maxRodriguesError = FastMath.max(maxRodriguesError, FastMath.abs(refOr[i].getValue() - fieldOr[i].getValue()));
                    maxRodriguesDotError = FastMath.max(maxRodriguesDotError, FastMath.abs(refOr[i].getPartialDerivative(1) - fieldOr[i].getPartialDerivative(1)));
                    maxRodriguesDotDotError = FastMath.max(maxRodriguesDotDotError, FastMath.abs(refOr[i].getPartialDerivative(2) - fieldOr[i].getPartialDerivative(2)));
                }
            }
            Assert.assertEquals(frame.getName(), 0, maxCartesianError, cartesianTolerance);
            Assert.assertEquals(frame.getName(), 0, maxCartesianDotError, cartesianDotTolerance);
            Assert.assertEquals(frame.getName(), 0, maxCartesianDotDotError, cartesianDotDotTolerance);
            Assert.assertEquals(frame.getName(), 0, maxRodriguesError, rodriguesTolerance);
            Assert.assertEquals(frame.getName(), 0, maxRodriguesDotError, rodriguesDotTolerance);
            Assert.assertEquals(frame.getName(), 0, maxRodriguesDotDotError, rodriguesDotDotTolerance);
        }
    }
}

Example 68 with DSFactory

use of org.hipparchus.analysis.differentiation.DSFactory in project Orekit by CS-SI.

the class EquinoctialOrbitTest method differentiate.

private <S extends Function<EquinoctialOrbit, Double>> double differentiate(TimeStampedPVCoordinates pv, Frame frame, double mu, S picker) {
    final DSFactory factory = new DSFactory(1, 1);
    FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
    UnivariateDifferentiableFunction diff = differentiator.differentiate(new UnivariateFunction() {

        public double value(double dt) {
            return picker.apply(new EquinoctialOrbit(pv.shiftedBy(dt), frame, mu));
        }
    });
    return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
}

Example 69 with DSFactory

use of org.hipparchus.analysis.differentiation.DSFactory in project Orekit by CS-SI.

the class FieldCartesianOrbitTest method differentiate.

private <T extends RealFieldElement<T>, S extends Function<FieldCartesianOrbit<T>, T>> double differentiate(TimeStampedFieldPVCoordinates<T> pv, Frame frame, double mu, S picker) {
    final DSFactory factory = new DSFactory(1, 1);
    FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
    UnivariateDifferentiableFunction diff = differentiator.differentiate(new UnivariateFunction() {

        public double value(double dt) {
            return picker.apply(new FieldCartesianOrbit<>(pv.shiftedBy(dt), frame, mu)).getReal();
        }
    });
    return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
}

Example 70 with DSFactory

use of org.hipparchus.analysis.differentiation.DSFactory in project Orekit by CS-SI.

the class FieldKeplerianOrbitTest method differentiate.

private <T extends RealFieldElement<T>, S extends Function<FieldKeplerianOrbit<T>, T>> double differentiate(FieldKeplerianOrbit<T> orbit, S picker) {
    final DSFactory factory = new DSFactory(1, 1);
    FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
    UnivariateDifferentiableFunction diff = differentiator.differentiate(new UnivariateFunction() {

        public double value(double dt) {
            return picker.apply(orbit.shiftedBy(orbit.getDate().getField().getZero().add(dt))).getReal();
        }
    });
    return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
}