Examples with FieldVector3D org.hipparchus.geometry.euclidean.threed.FieldVector3D used on opensource projects

Example 76 with FieldVector3D

use of org.hipparchus.geometry.euclidean.threed.FieldVector3D in project Orekit by CS-SI.

the class RelativityTest method testAccelerationCircular.

/**
 * Check a nearly circular orbit.
 *
 * @throws OrekitException on error
 */
@Test
public void testAccelerationCircular() throws OrekitException {
    double gm = Constants.EIGEN5C_EARTH_MU;
    double re = Constants.WGS84_EARTH_EQUATORIAL_RADIUS;
    Relativity relativity = new Relativity(gm);
    final CircularOrbit orbit = new CircularOrbit(re + 500e3, 0, 0, FastMath.toRadians(41.2), -1, 3, PositionAngle.TRUE, frame, date, gm);
    SpacecraftState state = new SpacecraftState(orbit);
    // action
    Vector3D acceleration = relativity.acceleration(state, relativity.getParameters());
    // verify
    // force is ~1e-8 so this give ~7 sig figs.
    double tol = 2e-10;
    PVCoordinates pv = state.getPVCoordinates();
    Vector3D p = pv.getPosition();
    Vector3D v = pv.getVelocity();
    Vector3D circularApproximation = p.normalize().scalarMultiply(gm / p.getNormSq() * 3 * v.getNormSq() / (c * c));
    Assert.assertEquals(0, acceleration.subtract(circularApproximation).getNorm(), tol);
    // check derivatives
    FieldSpacecraftState<DerivativeStructure> sDS = toDS(state, new LofOffset(state.getFrame(), LOFType.VVLH));
    FieldVector3D<DerivativeStructure> gradient = relativity.acceleration(sDS, relativity.getParameters(sDS.getDate().getField()));
    Assert.assertEquals(0, gradient.toVector3D().subtract(circularApproximation).getNorm(), tol);
    double r = p.getNorm();
    double s = v.getNorm();
    final double[] actualdx = gradient.getX().getAllDerivatives();
    final double x = p.getX();
    final double vx = v.getX();
    double expectedDxDx = gm / (c * c * r * r * r * r * r) * (-13 * x * x * s * s + 3 * r * r * s * s + 4 * r * r * vx * vx);
    Assert.assertEquals(expectedDxDx, actualdx[1], 2);
}

Example 77 with FieldVector3D

use of org.hipparchus.geometry.euclidean.threed.FieldVector3D in project Orekit by CS-SI.

the class RangeAnalytic method theoreticalEvaluationValidation.

/**
 * Added for validation
 * Compares directly numeric and analytic computations
 * @param iteration
 * @param evaluation
 * @param state
 * @return
 * @throws OrekitException
 */
protected EstimatedMeasurement<Range> theoreticalEvaluationValidation(final int iteration, final int evaluation, final SpacecraftState state) throws OrekitException {
    // Station & DSFactory attributes from parent Range class
    final GroundStation groundStation = getStation();
    // get the number of parameters used for derivation
    int nbParams = 6;
    final Map<String, Integer> indices = new HashMap<>();
    for (ParameterDriver driver : getParametersDrivers()) {
        if (driver.isSelected()) {
            indices.put(driver.getName(), nbParams++);
        }
    }
    final DSFactory dsFactory = new DSFactory(nbParams, 1);
    final Field<DerivativeStructure> field = dsFactory.getDerivativeField();
    final FieldVector3D<DerivativeStructure> zero = FieldVector3D.getZero(field);
    // Range derivatives are computed with respect to spacecraft state in inertial frame
    // and station position in station's offset frame
    // -------
    // 
    // Parameters:
    // - 0..2 - Px, Py, Pz   : Position of the spacecraft in inertial frame
    // - 3..5 - Vx, Vy, Vz   : Velocity of the spacecraft in inertial frame
    // - 6..8 - QTx, QTy, QTz: Position of the station in station's offset frame
    // Coordinates of the spacecraft expressed as a derivative structure
    final TimeStampedFieldPVCoordinates<DerivativeStructure> pvaDS = getCoordinates(state, 0, dsFactory);
    // transform between station and inertial frame, expressed as a derivative structure
    // The components of station's position in offset frame are the 3 last derivative parameters
    final AbsoluteDate downlinkDate = getDate();
    final FieldAbsoluteDate<DerivativeStructure> downlinkDateDS = new FieldAbsoluteDate<>(field, downlinkDate);
    final FieldTransform<DerivativeStructure> offsetToInertialDownlink = groundStation.getOffsetToInertial(state.getFrame(), downlinkDateDS, dsFactory, indices);
    // Station position in inertial frame at end of the downlink leg
    final TimeStampedFieldPVCoordinates<DerivativeStructure> stationDownlink = offsetToInertialDownlink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(downlinkDateDS, zero, zero, zero));
    // Compute propagation times
    // (if state has already been set up to pre-compensate propagation delay,
    // we will have offset == downlinkDelay and transitState will be
    // the same as state)
    // Downlink delay
    final DerivativeStructure tauD = signalTimeOfFlight(pvaDS, stationDownlink.getPosition(), downlinkDateDS);
    // Transit state
    final double delta = downlinkDate.durationFrom(state.getDate());
    final DerivativeStructure tauDMDelta = tauD.negate().add(delta);
    final SpacecraftState transitState = state.shiftedBy(tauDMDelta.getValue());
    // Transit state position (re)computed with derivative structures
    final TimeStampedFieldPVCoordinates<DerivativeStructure> transitStateDS = pvaDS.shiftedBy(tauDMDelta);
    // Station at transit state date (derivatives of tauD taken into account)
    final TimeStampedFieldPVCoordinates<DerivativeStructure> stationAtTransitDate = stationDownlink.shiftedBy(tauD.negate());
    // Uplink delay
    final DerivativeStructure tauU = signalTimeOfFlight(stationAtTransitDate, transitStateDS.getPosition(), transitStateDS.getDate());
    // Prepare the evaluation
    final EstimatedMeasurement<Range> estimated = new EstimatedMeasurement<Range>(this, iteration, evaluation, new SpacecraftState[] { transitState }, null);
    // Range value
    final DerivativeStructure tau = tauD.add(tauU);
    final double cOver2 = 0.5 * Constants.SPEED_OF_LIGHT;
    final DerivativeStructure range = tau.multiply(cOver2);
    estimated.setEstimatedValue(range.getValue());
    // Range partial derivatives with respect to state
    final double[] derivatives = range.getAllDerivatives();
    estimated.setStateDerivatives(0, Arrays.copyOfRange(derivatives, 1, 7));
    // (beware element at index 0 is the value, not a derivative)
    for (final ParameterDriver driver : getParametersDrivers()) {
        final Integer index = indices.get(driver.getName());
        if (index != null) {
            estimated.setParameterDerivatives(driver, derivatives[index + 1]);
        }
    }
    // ----------
    // VALIDATION
    // -----------
    // Computation of the value without DS
    // ----------------------------------
    // Time difference between t (date of the measurement) and t' (date tagged in spacecraft state)
    // Station position at signal arrival
    final Transform topoToInertDownlink = groundStation.getOffsetToInertial(state.getFrame(), downlinkDate);
    final PVCoordinates QDownlink = topoToInertDownlink.transformPVCoordinates(PVCoordinates.ZERO);
    // Downlink time of flight from spacecraft to station
    final double td = signalTimeOfFlight(state.getPVCoordinates(), QDownlink.getPosition(), downlinkDate);
    final double dt = delta - td;
    // Transit state position
    final AbsoluteDate transitT = state.getDate().shiftedBy(dt);
    final SpacecraftState transit = state.shiftedBy(dt);
    final Vector3D transitP = transitState.getPVCoordinates().getPosition();
    // Station position at signal departure
    // First guess
    // AbsoluteDate uplinkDate = downlinkDate.shiftedBy(-getObservedValue()[0] / cOver2);
    // final Transform topoToInertUplink =
    // station.getOffsetFrame().getTransformTo(state.getFrame(), uplinkDate);
    // TimeStampedPVCoordinates QUplink = topoToInertUplink.
    // transformPVCoordinates(new TimeStampedPVCoordinates(uplinkDate, PVCoordinates.ZERO));
    // Station position at transit state date
    final Transform topoToInertAtTransitDate = groundStation.getOffsetToInertial(state.getFrame(), transitT);
    TimeStampedPVCoordinates QAtTransitDate = topoToInertAtTransitDate.transformPVCoordinates(new TimeStampedPVCoordinates(transitT, PVCoordinates.ZERO));
    // Uplink time of flight
    final double tu = signalTimeOfFlight(QAtTransitDate, transitP, transitT);
    // Total time of flight
    final double t = td + tu;
    // Real date and position of station at signal departure
    AbsoluteDate uplinkDate = downlinkDate.shiftedBy(-t);
    TimeStampedPVCoordinates QUplink = topoToInertDownlink.shiftedBy(-t).transformPVCoordinates(new TimeStampedPVCoordinates(uplinkDate, PVCoordinates.ZERO));
    // Range value
    double r = t * cOver2;
    double dR = r - range.getValue();
    // td derivatives / state
    // -----------------------
    // Qt = Master station position at tmeas = t = signal arrival at master station
    final Vector3D vel = state.getPVCoordinates().getVelocity();
    final Vector3D Qt_V = QDownlink.getVelocity();
    final Vector3D Ptr = transit.getPVCoordinates().getPosition();
    final Vector3D Ptr_Qt = QDownlink.getPosition().subtract(Ptr);
    final double dDown = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * td - Vector3D.dotProduct(Ptr_Qt, vel);
    // Derivatives of the downlink time of flight
    final double dtddPx = -Ptr_Qt.getX() / dDown;
    final double dtddPy = -Ptr_Qt.getY() / dDown;
    final double dtddPz = -Ptr_Qt.getZ() / dDown;
    final double dtddVx = dtddPx * dt;
    final double dtddVy = dtddPy * dt;
    final double dtddVz = dtddPz * dt;
    // From the DS
    final double dtddPxDS = tauD.getPartialDerivative(1, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtddPyDS = tauD.getPartialDerivative(0, 1, 0, 0, 0, 0, 0, 0, 0);
    final double dtddPzDS = tauD.getPartialDerivative(0, 0, 1, 0, 0, 0, 0, 0, 0);
    final double dtddVxDS = tauD.getPartialDerivative(0, 0, 0, 1, 0, 0, 0, 0, 0);
    final double dtddVyDS = tauD.getPartialDerivative(0, 0, 0, 0, 1, 0, 0, 0, 0);
    final double dtddVzDS = tauD.getPartialDerivative(0, 0, 0, 0, 0, 1, 0, 0, 0);
    // Difference
    final double d_dtddPx = dtddPxDS - dtddPx;
    final double d_dtddPy = dtddPyDS - dtddPy;
    final double d_dtddPz = dtddPzDS - dtddPz;
    final double d_dtddVx = dtddVxDS - dtddVx;
    final double d_dtddVy = dtddVyDS - dtddVy;
    final double d_dtddVz = dtddVzDS - dtddVz;
    // tu derivatives / state
    // -----------------------
    final Vector3D Qt2_Ptr = Ptr.subtract(QUplink.getPosition());
    final double dUp = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tu - Vector3D.dotProduct(Qt2_Ptr, Qt_V);
    // test
    // // Speed of the station at tmeas-t
    // // Note: Which one to use in the calculation of dUp ???
    // final Vector3D Qt2_V    = QUplink.getVelocity();
    // final double   dUp      = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tu -
    // Vector3D.dotProduct(Qt2_Ptr, Qt2_V);
    // test
    // tu derivatives
    final double dtudPx = 1. / dUp * Qt2_Ptr.dotProduct(Vector3D.PLUS_I.add((Qt_V.subtract(vel)).scalarMultiply(dtddPx)));
    final double dtudPy = 1. / dUp * Qt2_Ptr.dotProduct(Vector3D.PLUS_J.add((Qt_V.subtract(vel)).scalarMultiply(dtddPy)));
    final double dtudPz = 1. / dUp * Qt2_Ptr.dotProduct(Vector3D.PLUS_K.add((Qt_V.subtract(vel)).scalarMultiply(dtddPz)));
    final double dtudVx = dtudPx * dt;
    final double dtudVy = dtudPy * dt;
    final double dtudVz = dtudPz * dt;
    // From the DS
    final double dtudPxDS = tauU.getPartialDerivative(1, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtudPyDS = tauU.getPartialDerivative(0, 1, 0, 0, 0, 0, 0, 0, 0);
    final double dtudPzDS = tauU.getPartialDerivative(0, 0, 1, 0, 0, 0, 0, 0, 0);
    final double dtudVxDS = tauU.getPartialDerivative(0, 0, 0, 1, 0, 0, 0, 0, 0);
    final double dtudVyDS = tauU.getPartialDerivative(0, 0, 0, 0, 1, 0, 0, 0, 0);
    final double dtudVzDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 1, 0, 0, 0);
    // Difference
    final double d_dtudPx = dtudPxDS - dtudPx;
    final double d_dtudPy = dtudPyDS - dtudPy;
    final double d_dtudPz = dtudPzDS - dtudPz;
    final double d_dtudVx = dtudVxDS - dtudVx;
    final double d_dtudVy = dtudVyDS - dtudVy;
    final double d_dtudVz = dtudVzDS - dtudVz;
    // Range derivatives / state
    // -----------------------
    // R = Range
    double dRdPx = (dtddPx + dtudPx) * cOver2;
    double dRdPy = (dtddPy + dtudPy) * cOver2;
    double dRdPz = (dtddPz + dtudPz) * cOver2;
    double dRdVx = (dtddVx + dtudVx) * cOver2;
    double dRdVy = (dtddVy + dtudVy) * cOver2;
    double dRdVz = (dtddVz + dtudVz) * cOver2;
    // With DS
    double dRdPxDS = range.getPartialDerivative(1, 0, 0, 0, 0, 0, 0, 0, 0);
    double dRdPyDS = range.getPartialDerivative(0, 1, 0, 0, 0, 0, 0, 0, 0);
    double dRdPzDS = range.getPartialDerivative(0, 0, 1, 0, 0, 0, 0, 0, 0);
    double dRdVxDS = range.getPartialDerivative(0, 0, 0, 1, 0, 0, 0, 0, 0);
    double dRdVyDS = range.getPartialDerivative(0, 0, 0, 0, 1, 0, 0, 0, 0);
    double dRdVzDS = range.getPartialDerivative(0, 0, 0, 0, 0, 1, 0, 0, 0);
    // Diff
    final double d_dRdPx = dRdPxDS - dRdPx;
    final double d_dRdPy = dRdPyDS - dRdPy;
    final double d_dRdPz = dRdPzDS - dRdPz;
    final double d_dRdVx = dRdVxDS - dRdVx;
    final double d_dRdVy = dRdVyDS - dRdVy;
    final double d_dRdVz = dRdVzDS - dRdVz;
    // td derivatives / station
    // -----------------------
    final AngularCoordinates ac = topoToInertDownlink.getAngular().revert();
    final Rotation rotTopoToInert = ac.getRotation();
    final Vector3D omega = ac.getRotationRate();
    final Vector3D dtddQI = Ptr_Qt.scalarMultiply(1. / dDown);
    final double dtddQIx = dtddQI.getX();
    final double dtddQIy = dtddQI.getY();
    final double dtddQIz = dtddQI.getZ();
    final Vector3D dtddQ = rotTopoToInert.applyTo(dtddQI);
    // With DS
    double dtddQxDS = tauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0);
    double dtddQyDS = tauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0);
    double dtddQzDS = tauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1);
    // Diff
    final double d_dtddQx = dtddQxDS - dtddQ.getX();
    final double d_dtddQy = dtddQyDS - dtddQ.getY();
    final double d_dtddQz = dtddQzDS - dtddQ.getZ();
    // tu derivatives / station
    // -----------------------
    // Inertial frame
    final double dtudQIx = 1 / dUp * Qt2_Ptr.dotProduct(Vector3D.MINUS_I.add((Qt_V.subtract(vel)).scalarMultiply(dtddQIx)).subtract(Vector3D.PLUS_I.crossProduct(omega).scalarMultiply(t)));
    final double dtudQIy = 1 / dUp * Qt2_Ptr.dotProduct(Vector3D.MINUS_J.add((Qt_V.subtract(vel)).scalarMultiply(dtddQIy)).subtract(Vector3D.PLUS_J.crossProduct(omega).scalarMultiply(t)));
    final double dtudQIz = 1 / dUp * Qt2_Ptr.dotProduct(Vector3D.MINUS_K.add((Qt_V.subtract(vel)).scalarMultiply(dtddQIz)).subtract(Vector3D.PLUS_K.crossProduct(omega).scalarMultiply(t)));
    // // test
    // final double dtudQIx = 1/dUp*Qt2_Ptr
    // //                        .dotProduct(Vector3D.MINUS_I);
    // //                                    .dotProduct((Qt_V.subtract(vel)).scalarMultiply(dtddQIx));
    // .dotProduct(Vector3D.MINUS_I.crossProduct(omega).scalarMultiply(t));
    // final double dtudQIy = 1/dUp*Qt2_Ptr
    // //                        .dotProduct(Vector3D.MINUS_J);
    // //                                    .dotProduct((Qt_V.subtract(vel)).scalarMultiply(dtddQIy));
    // .dotProduct(Vector3D.MINUS_J.crossProduct(omega).scalarMultiply(t));
    // final double dtudQIz = 1/dUp*Qt2_Ptr
    // //                        .dotProduct(Vector3D.MINUS_K);
    // //                                    .dotProduct((Qt_V.subtract(vel)).scalarMultiply(dtddQIz));
    // .dotProduct(Vector3D.MINUS_K.crossProduct(omega).scalarMultiply(t));
    // 
    // double dtu_dQxDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0);
    // double dtu_dQyDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0);
    // double dtu_dQzDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1);
    // final Vector3D dtudQDS = new Vector3D(dtu_dQxDS, dtu_dQyDS, dtu_dQzDS);
    // final Vector3D dtudQIDS = rotTopoToInert.applyInverseTo(dtudQDS);
    // double dtudQIxDS = dtudQIDS.getX();
    // double dtudQIyDS = dtudQIDS.getY();
    // double dtudQIxzS = dtudQIDS.getZ();
    // // test
    // Topocentric frame
    final Vector3D dtudQI = new Vector3D(dtudQIx, dtudQIy, dtudQIz);
    final Vector3D dtudQ = rotTopoToInert.applyTo(dtudQI);
    // With DS
    double dtudQxDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0);
    double dtudQyDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0);
    double dtudQzDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1);
    // Diff
    final double d_dtudQx = dtudQxDS - dtudQ.getX();
    final double d_dtudQy = dtudQyDS - dtudQ.getY();
    final double d_dtudQz = dtudQzDS - dtudQ.getZ();
    // Range derivatives / station
    // -----------------------
    double dRdQx = (dtddQ.getX() + dtudQ.getX()) * cOver2;
    double dRdQy = (dtddQ.getY() + dtudQ.getY()) * cOver2;
    double dRdQz = (dtddQ.getZ() + dtudQ.getZ()) * cOver2;
    // With DS
    double dRdQxDS = range.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0);
    double dRdQyDS = range.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0);
    double dRdQzDS = range.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1);
    // Diff
    final double d_dRdQx = dRdQxDS - dRdQx;
    final double d_dRdQy = dRdQyDS - dRdQy;
    final double d_dRdQz = dRdQzDS - dRdQz;
    // Print results to avoid warning
    final boolean printResults = false;
    if (printResults) {
        System.out.println("dR = " + dR);
        System.out.println("d_dtddPx = " + d_dtddPx);
        System.out.println("d_dtddPy = " + d_dtddPy);
        System.out.println("d_dtddPz = " + d_dtddPz);
        System.out.println("d_dtddVx = " + d_dtddVx);
        System.out.println("d_dtddVy = " + d_dtddVy);
        System.out.println("d_dtddVz = " + d_dtddVz);
        System.out.println("d_dtudPx = " + d_dtudPx);
        System.out.println("d_dtudPy = " + d_dtudPy);
        System.out.println("d_dtudPz = " + d_dtudPz);
        System.out.println("d_dtudVx = " + d_dtudVx);
        System.out.println("d_dtudVy = " + d_dtudVy);
        System.out.println("d_dtudVz = " + d_dtudVz);
        System.out.println("d_dRdPx = " + d_dRdPx);
        System.out.println("d_dRdPy = " + d_dRdPy);
        System.out.println("d_dRdPz = " + d_dRdPz);
        System.out.println("d_dRdVx = " + d_dRdVx);
        System.out.println("d_dRdVy = " + d_dRdVy);
        System.out.println("d_dRdVz = " + d_dRdVz);
        System.out.println("d_dtddQx = " + d_dtddQx);
        System.out.println("d_dtddQy = " + d_dtddQy);
        System.out.println("d_dtddQz = " + d_dtddQz);
        System.out.println("d_dtudQx = " + d_dtudQx);
        System.out.println("d_dtudQy = " + d_dtudQy);
        System.out.println("d_dtudQz = " + d_dtudQz);
        System.out.println("d_dRdQx = " + d_dRdQx);
        System.out.println("d_dRdQy = " + d_dRdQy);
        System.out.println("d_dRdQz = " + d_dRdQz);
    }
    // Dummy return
    return estimated;
}

Example 78 with FieldVector3D

use of org.hipparchus.geometry.euclidean.threed.FieldVector3D in project Orekit by CS-SI.

the class FieldNumericalPropagatorTest method doTestResetStateEvent.

private <T extends RealFieldElement<T>> void doTestResetStateEvent(Field<T> field) throws OrekitException {
    T zero = field.getZero();
    // setup
    final FieldAbsoluteDate<T> initDate = FieldAbsoluteDate.getJ2000Epoch(field);
    FieldSpacecraftState<T> initialState;
    FieldNumericalPropagator<T> propagator;
    final FieldVector3D<T> position = new FieldVector3D<>(zero.add(7.0e6), zero.add(1.0e6), zero.add(4.0e6));
    final FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-500.0), zero.add(8000.0), zero.add(1000.0));
    final FieldOrbit<T> orbit = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(position, velocity), FramesFactory.getEME2000(), initDate, mu);
    initialState = new FieldSpacecraftState<>(orbit);
    OrbitType type = OrbitType.EQUINOCTIAL;
    double[][] tolerance = NumericalPropagator.tolerances(0.001, orbit.toOrbit(), type);
    AdaptiveStepsizeFieldIntegrator<T> integrator = new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
    integrator.setInitialStepSize(zero.add(60));
    propagator = new FieldNumericalPropagator<>(field, integrator);
    propagator.setOrbitType(type);
    propagator.setInitialState(initialState);
    final FieldAbsoluteDate<T> resetDate = initDate.shiftedBy(1000);
    CheckingHandler<FieldDateDetector<T>, T> checking = new CheckingHandler<FieldDateDetector<T>, T>(Action.RESET_STATE) {

        public FieldSpacecraftState<T> resetState(FieldDateDetector<T> detector, FieldSpacecraftState<T> oldState) {
            return new FieldSpacecraftState<>(oldState.getOrbit(), oldState.getAttitude(), oldState.getMass().subtract(200.0));
        }
    };
    propagator.addEventDetector(new FieldDateDetector<>(resetDate).withHandler(checking));
    checking.assertEvent(false);
    final FieldSpacecraftState<T> finalState = propagator.propagate(initDate.shiftedBy(3200));
    checking.assertEvent(true);
    Assert.assertEquals(initialState.getMass().getReal() - 200, finalState.getMass().getReal(), 1.0e-10);
}

Example 79 with FieldVector3D

use of org.hipparchus.geometry.euclidean.threed.FieldVector3D in project Orekit by CS-SI.

the class FieldNumericalPropagatorTest method createHyperbolicOrbit.

private static <T extends RealFieldElement<T>> FieldCartesianOrbit<T> createHyperbolicOrbit(Field<T> field) throws OrekitException {
    T zero = field.getZero();
    final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, "2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
    final FieldVector3D<T> position = new FieldVector3D<>(zero.add(224267911.905821), zero.add(290251613.109399), zero.add(45534292.777492));
    final FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-1494.068165293), zero.add(1124.771027677), zero.add(526.915286134));
    final TimeStampedFieldPVCoordinates<T> pv = new TimeStampedFieldPVCoordinates<>(date, position, velocity, FieldVector3D.getZero(field));
    final Frame frame = FramesFactory.getEME2000();
    final double mu = Constants.EIGEN5C_EARTH_MU;
    return new FieldCartesianOrbit<>(pv, frame, mu);
}

Example 80 with FieldVector3D

use of org.hipparchus.geometry.euclidean.threed.FieldVector3D in project Orekit by CS-SI.

the class FieldNumericalPropagatorTest method doTestContinueEvent.

private <T extends RealFieldElement<T>> void doTestContinueEvent(Field<T> field) throws OrekitException {
    T zero = field.getZero();
    // setup
    final FieldAbsoluteDate<T> initDate = FieldAbsoluteDate.getJ2000Epoch(field);
    FieldSpacecraftState<T> initialState;
    FieldNumericalPropagator<T> propagator;
    final FieldVector3D<T> position = new FieldVector3D<>(zero.add(7.0e6), zero.add(1.0e6), zero.add(4.0e6));
    final FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-500.0), zero.add(8000.0), zero.add(1000.0));
    final FieldOrbit<T> orbit = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(position, velocity), FramesFactory.getEME2000(), initDate, mu);
    initialState = new FieldSpacecraftState<>(orbit);
    OrbitType type = OrbitType.EQUINOCTIAL;
    double[][] tolerance = NumericalPropagator.tolerances(0.001, orbit.toOrbit(), type);
    AdaptiveStepsizeFieldIntegrator<T> integrator = new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
    integrator.setInitialStepSize(zero.add(60));
    propagator = new FieldNumericalPropagator<>(field, integrator);
    propagator.setOrbitType(type);
    propagator.setInitialState(initialState);
    final FieldAbsoluteDate<T> resetDate = initDate.shiftedBy(1000);
    CheckingHandler<FieldDateDetector<T>, T> checking = new CheckingHandler<FieldDateDetector<T>, T>(Action.CONTINUE);
    propagator.addEventDetector(new FieldDateDetector<>(resetDate).withHandler(checking));
    final double dt = 3200;
    checking.assertEvent(false);
    Assert.assertEquals(0.0, propagator.getInitialState().getDate().durationFrom(initDate).getReal(), 1.0e-10);
    propagator.setResetAtEnd(false);
    final FieldSpacecraftState<T> finalState = propagator.propagate(initDate.shiftedBy(dt));
    Assert.assertEquals(0.0, propagator.getInitialState().getDate().durationFrom(initDate).getReal(), 1.0e-10);
    checking.assertEvent(true);
    final double n = FastMath.sqrt(initialState.getMu() / initialState.getA().getReal()) / initialState.getA().getReal();
    Assert.assertEquals(initialState.getA().getReal(), finalState.getA().getReal(), 1.0e-10);
    Assert.assertEquals(initialState.getEquinoctialEx().getReal(), finalState.getEquinoctialEx().getReal(), 1.0e-10);
    Assert.assertEquals(initialState.getEquinoctialEy().getReal(), finalState.getEquinoctialEy().getReal(), 1.0e-10);
    Assert.assertEquals(initialState.getHx().getReal(), finalState.getHx().getReal(), 1.0e-10);
    Assert.assertEquals(initialState.getHy().getReal(), finalState.getHy().getReal(), 1.0e-10);
    Assert.assertEquals(initialState.getLM().getReal() + n * dt, finalState.getLM().getReal(), 6.0e-10);
}