FieldAbstractIntegratedPropagator.java

  1. /* Copyright 2002-2022 CS GROUP
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.propagation.integration;

  19. import java.util.ArrayList;
  20. import java.util.Arrays;
  21. import java.util.Collection;
  22. import java.util.Collections;
  23. import java.util.HashMap;
  24. import java.util.LinkedList;
  25. import java.util.List;
  26. import java.util.Map;
  27. import java.util.Queue;

  29. import org.hipparchus.CalculusFieldElement;
  30. import org.hipparchus.Field;
  31. import org.hipparchus.exception.MathIllegalArgumentException;
  32. import org.hipparchus.exception.MathIllegalStateException;
  33. import org.hipparchus.ode.FieldDenseOutputModel;
  34. import org.hipparchus.ode.FieldExpandableODE;
  35. import org.hipparchus.ode.FieldODEIntegrator;
  36. import org.hipparchus.ode.FieldODEState;
  37. import org.hipparchus.ode.FieldODEStateAndDerivative;
  38. import org.hipparchus.ode.FieldOrdinaryDifferentialEquation;
  39. import org.hipparchus.ode.FieldSecondaryODE;
  40. import org.hipparchus.ode.events.Action;
  41. import org.hipparchus.ode.events.FieldEventHandlerConfiguration;
  42. import org.hipparchus.ode.events.FieldODEEventHandler;
  43. import org.hipparchus.ode.sampling.AbstractFieldODEStateInterpolator;
  44. import org.hipparchus.ode.sampling.FieldODEStateInterpolator;
  45. import org.hipparchus.ode.sampling.FieldODEStepHandler;
  46. import org.hipparchus.util.MathArrays;
  47. import org.hipparchus.util.Precision;
  48. import org.orekit.attitudes.AttitudeProvider;
  49. import org.orekit.errors.OrekitException;
  50. import org.orekit.errors.OrekitInternalError;
  51. import org.orekit.errors.OrekitMessages;
  52. import org.orekit.frames.Frame;
  53. import org.orekit.orbits.OrbitType;
  54. import org.orekit.orbits.PositionAngle;
  55. import org.orekit.propagation.FieldAbstractPropagator;
  56. import org.orekit.propagation.FieldBoundedPropagator;
  57. import org.orekit.propagation.FieldEphemerisGenerator;
  58. import org.orekit.propagation.FieldSpacecraftState;
  59. import org.orekit.propagation.PropagationType;
  60. import org.orekit.propagation.events.FieldEventDetector;
  61. import org.orekit.propagation.sampling.FieldOrekitStepHandler;
  62. import org.orekit.propagation.sampling.FieldOrekitStepInterpolator;
  63. import org.orekit.time.FieldAbsoluteDate;
  64. import org.orekit.utils.FieldArrayDictionary;


  67. /** Common handling of {@link org.orekit.propagation.FieldPropagator FieldPropagator}
  68.  *  methods for both numerical and semi-analytical propagators.
  69.  *  @param <T> the type of the field elements
  70.  *  @author Luc Maisonobe
  71.  */
  72. public abstract class FieldAbstractIntegratedPropagator<T extends CalculusFieldElement<T>> extends FieldAbstractPropagator<T> {

  74.     /** Internal name used for complete secondary state dimension.
  75.      * @since 11.1
  76.      */
  77.     private static final String SECONDARY_DIMENSION = "Orekit-secondary-dimension";

  79.     /** Event detectors not related to force models. */
  80.     private final List<FieldEventDetector<T>> detectors;

  82.     /** Step handlers dedicated to ephemeris generation. */
  83.     private final List<FieldStoringStepHandler> ephemerisGenerators;

  85.     /** Integrator selected by the user for the orbital extrapolation process. */
  86.     private final FieldODEIntegrator<T> integrator;

  88.     /** Offsets of secondary states managed by {@link AdditionalEquations}.
  89.      * @since 11.1
  90.      */
  91.     private final Map<String, Integer> secondaryOffsets;

  93.     /** Additional derivatives providers.
  94.      * @since 11.1
  95.      */
  96.     private List<FieldAdditionalDerivativesProvider<T>> additionalDerivativesProviders;

  98.     /** Counter for differential equations calls. */
  99.     private int calls;

  101.     /** Mapper between raw double components and space flight dynamics objects. */
  102.     private FieldStateMapper<T> stateMapper;

  104.     /** Flag for resetting the state at end of propagation. */
  105.     private boolean resetAtEnd;

  107.     /** Type of orbit to output (mean or osculating) <br/>
  108.      * <p>
  109.      * This is used only in the case of semianalitical propagators where there is a clear separation between
  110.      * mean and short periodic elements. It is ignored by the Numerical propagator.
  111.      * </p>
  112.      */
  113.     private PropagationType propagationType;

  115.     /** Build a new instance.
  116.      * @param integrator numerical integrator to use for propagation.
  117.      * @param propagationType type of orbit to output (mean or osculating).
  118.      * @param field Field used by default
  119.      */
  120.     protected FieldAbstractIntegratedPropagator(final Field<T> field, final FieldODEIntegrator<T> integrator, final PropagationType propagationType) {
  121.         super(field);
  122.         detectors                      = new ArrayList<>();
  123.         ephemerisGenerators            = new ArrayList<>();
  124.         additionalDerivativesProviders = new ArrayList<>();
  125.         this.secondaryOffsets          = new HashMap<>();
  126.         this.integrator                = integrator;
  127.         this.propagationType           = propagationType;
  128.         this.resetAtEnd                = true;
  129.     }

  131.     /** Allow/disallow resetting the initial state at end of propagation.
  132.      * <p>
  133.      * By default, at the end of the propagation, the propagator resets the initial state
  134.      * to the final state, thus allowing a new propagation to be started from there without
  135.      * recomputing the part already performed. Calling this method with {@code resetAtEnd} set
  136.      * to false changes prevents such reset.
  137.      * </p>
  138.      * @param resetAtEnd if true, at end of each propagation, the {@link
  139.      * #getInitialState() initial state} will be reset to the final state of
  140.      * the propagation, otherwise the initial state will be preserved
  141.      * @since 9.0
  142.      */
  143.     public void setResetAtEnd(final boolean resetAtEnd) {
  144.         this.resetAtEnd = resetAtEnd;
  145.     }

  147.     /** Initialize the mapper.
  148.      * @param field Field used by default
  149.      */
  150.     protected void initMapper(final Field<T> field) {
  151.         final T zero = field.getZero();
  152.         stateMapper = createMapper(null, zero.add(Double.NaN), null, null, null, null);
  153.     }

  155.     /**  {@inheritDoc} */
  156.     public void setAttitudeProvider(final AttitudeProvider attitudeProvider) {
  157.         super.setAttitudeProvider(attitudeProvider);
  158.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  159.                                    stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  160.                                    attitudeProvider, stateMapper.getFrame());
  161.     }

  163.     /** Set propagation orbit type.
  164.      * @param orbitType orbit type to use for propagation
  165.      */
  166.     protected void setOrbitType(final OrbitType orbitType) {
  167.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  168.                                    orbitType, stateMapper.getPositionAngleType(),
  169.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  170.     }

  172.     /** Get propagation parameter type.
  173.      * @return orbit type used for propagation
  174.      */
  175.     protected OrbitType getOrbitType() {
  176.         return stateMapper.getOrbitType();
  177.     }

  179.     /** Check if only the mean elements should be used in a semianalitical propagation.
  180.      * @return {@link PropagationType MEAN} if only mean elements have to be used or
  181.      *         {@link PropagationType OSCULATING} if osculating elements have to be also used.
  182.      */
  183.     protected PropagationType isMeanOrbit() {
  184.         return propagationType;
  185.     }

  187.     /** Set position angle type.
  188.      * <p>
  189.      * The position parameter type is meaningful only if {@link
  190.      * #getOrbitType() propagation orbit type}
  191.      * support it. As an example, it is not meaningful for propagation
  192.      * in {@link OrbitType#CARTESIAN Cartesian} parameters.
  193.      * </p>
  194.      * @param positionAngleType angle type to use for propagation
  195.      */
  196.     protected void setPositionAngleType(final PositionAngle positionAngleType) {
  197.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  198.                                    stateMapper.getOrbitType(), positionAngleType,
  199.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  200.     }

  202.     /** Get propagation parameter type.
  203.      * @return angle type to use for propagation
  204.      */
  205.     protected PositionAngle getPositionAngleType() {
  206.         return stateMapper.getPositionAngleType();
  207.     }

  209.     /** Set the central attraction coefficient μ.
  210.      * @param mu central attraction coefficient (m³/s²)
  211.      */
  212.     public void setMu(final T mu) {
  213.         stateMapper = createMapper(stateMapper.getReferenceDate(), mu,
  214.                                    stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  215.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  216.     }

  218.     /** Get the central attraction coefficient μ.
  219.      * @return mu central attraction coefficient (m³/s²)
  220.      * @see #setMu(CalculusFieldElement)
  221.      */
  222.     public T getMu() {
  223.         return stateMapper.getMu();
  224.     }

  226.     /** Get the number of calls to the differential equations computation method.
  227.      * <p>The number of calls is reset each time the {@link #propagate(FieldAbsoluteDate)}
  228.      * method is called.</p>
  229.      * @return number of calls to the differential equations computation method
  230.      */
  231.     public int getCalls() {
  232.         return calls;
  233.     }

  235.     /** {@inheritDoc} */
  236.     @Override
  237.     public boolean isAdditionalStateManaged(final String name) {

  239.         // first look at already integrated states
  240.         if (super.isAdditionalStateManaged(name)) {
  241.             return true;
  242.         }

  244.         // then look at states we integrate ourselves
  245.         for (final FieldAdditionalDerivativesProvider<T> provider : additionalDerivativesProviders) {
  246.             if (provider.getName().equals(name)) {
  247.                 return true;
  248.             }
  249.         }

  251.         return false;
  252.     }

  254.     /** {@inheritDoc} */
  255.     @Override
  256.     public String[] getManagedAdditionalStates() {
  257.         final String[] alreadyIntegrated = super.getManagedAdditionalStates();
  258.         final String[] managed = new String[alreadyIntegrated.length + additionalDerivativesProviders.size()];
  259.         System.arraycopy(alreadyIntegrated, 0, managed, 0, alreadyIntegrated.length);
  260.         for (int i = 0; i < additionalDerivativesProviders.size(); ++i) {
  261.             managed[i + alreadyIntegrated.length] = additionalDerivativesProviders.get(i).getName();
  262.         }
  263.         return managed;
  264.     }

  266.     /** Add a set of user-specified equations to be integrated along with the orbit propagation.
  267.      * @param additional additional equations
  268.      * @deprecated as of 11.1, replaced by {@link #addAdditionalDerivativesProvider(FieldAdditionalDerivativesProvider)}
  269.      */
  270.     @Deprecated
  271.     public void addAdditionalEquations(final FieldAdditionalEquations<T> additional) {
  272.         addAdditionalDerivativesProvider(new FieldAdditionalEquationsAdapter<>(additional, this::getInitialState));
  273.     }

  275.     /** Add a provider for user-specified state derivatives to be integrated along with the orbit propagation.
  276.      * @param provider provider for additional derivatives
  277.      * @see #addAdditionalStateProvider(org.orekit.propagation.FieldAdditionalStateProvider)
  278.      * @since 11.1
  279.      */
  280.     public void addAdditionalDerivativesProvider(final FieldAdditionalDerivativesProvider<T> provider) {
  281.         // check if the name is already used
  282.         if (isAdditionalStateManaged(provider.getName())) {
  283.             // these derivatives are already registered, complain
  284.             throw new OrekitException(OrekitMessages.ADDITIONAL_STATE_NAME_ALREADY_IN_USE,
  285.                                       provider.getName());
  286.         }

  288.         // this is really a new set of derivatives, add it
  289.         additionalDerivativesProviders.add(provider);

  291.         secondaryOffsets.clear();

  293.     }

  295.     /** Get an unmodifiable list of providers for additional derivatives.
  296.      * @return providers for additional derivatives
  297.      * @since 11.1
  298.      */
  299.     public List<FieldAdditionalDerivativesProvider<T>> getAdditionalDerivativesProviders() {
  300.         return Collections.unmodifiableList(additionalDerivativesProviders);
  301.     }

  303.     /** {@inheritDoc} */
  304.     public <D extends FieldEventDetector<T>> void addEventDetector(final D detector) {
  305.         detectors.add(detector);
  306.     }

  308.     /** {@inheritDoc} */
  309.     public Collection<FieldEventDetector<T>> getEventsDetectors() {
  310.         return Collections.unmodifiableCollection(detectors);
  311.     }

  313.     /** {@inheritDoc} */
  314.     public void clearEventsDetectors() {
  315.         detectors.clear();
  316.     }

  318.     /** Set up all user defined event detectors.
  319.      */
  320.     protected void setUpUserEventDetectors() {
  321.         for (final FieldEventDetector<T> detector : detectors) {
  322.             setUpEventDetector(integrator, detector);
  323.         }
  324.     }

  326.     /** Wrap an Orekit event detector and register it to the integrator.
  327.      * @param integ integrator into which event detector should be registered
  328.      * @param detector event detector to wrap
  329.      */
  330.     protected void setUpEventDetector(final FieldODEIntegrator<T> integ, final FieldEventDetector<T> detector) {
  331.         integ.addEventHandler(new FieldAdaptedEventDetector(detector),
  332.                               detector.getMaxCheckInterval().getReal(),
  333.                               detector.getThreshold().getReal(),
  334.                               detector.getMaxIterationCount());
  335.     }

  337.     /** {@inheritDoc} */
  338.     @Override
  339.     public FieldEphemerisGenerator<T> getEphemerisGenerator() {
  340.         final FieldStoringStepHandler storingHandler = new FieldStoringStepHandler();
  341.         ephemerisGenerators.add(storingHandler);
  342.         return storingHandler;
  343.     }

  345.     /** Create a mapper between raw double components and spacecraft state.
  346.     /** Simple constructor.
  347.      * <p>
  348.      * The position parameter type is meaningful only if {@link
  349.      * #getOrbitType() propagation orbit type}
  350.      * support it. As an example, it is not meaningful for propagation
  351.      * in {@link OrbitType#CARTESIAN Cartesian} parameters.
  352.      * </p>
  353.      * @param referenceDate reference date
  354.      * @param mu central attraction coefficient (m³/s²)
  355.      * @param orbitType orbit type to use for mapping
  356.      * @param positionAngleType angle type to use for propagation
  357.      * @param attitudeProvider attitude provider
  358.      * @param frame inertial frame
  359.      * @return new mapper
  360.      */
  361.     protected abstract FieldStateMapper<T> createMapper(FieldAbsoluteDate<T> referenceDate, T mu,
  362.                                                         OrbitType orbitType, PositionAngle positionAngleType,
  363.                                                         AttitudeProvider attitudeProvider, Frame frame);

  365.     /** Get the differential equations to integrate (for main state only).
  366.      * @param integ numerical integrator to use for propagation.
  367.      * @return differential equations for main state
  368.      */
  369.     protected abstract MainStateEquations<T> getMainStateEquations(FieldODEIntegrator<T> integ);

  371.     /** {@inheritDoc} */
  372.     public FieldSpacecraftState<T> propagate(final FieldAbsoluteDate<T> target) {
  373.         if (getStartDate() == null) {
  374.             if (getInitialState() == null) {
  375.                 throw new OrekitException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
  376.             }
  377.             setStartDate(getInitialState().getDate());
  378.         }
  379.         return propagate(getStartDate(), target);
  380.     }

  382.     /** {@inheritDoc} */
  383.     public FieldSpacecraftState<T> propagate(final FieldAbsoluteDate<T> tStart, final FieldAbsoluteDate<T> tEnd) {

  385.         if (getInitialState() == null) {
  386.             throw new OrekitException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
  387.         }

  389.         // make sure the integrator will be reset properly even if we change its events handlers and step handlers
  390.         try (IntegratorResetter<T> resetter = new IntegratorResetter<>(integrator)) {

  392.             if (!tStart.equals(getInitialState().getDate())) {
  393.                 // if propagation start date is not initial date,
  394.                 // propagate from initial to start date without event detection
  395.                 try (IntegratorResetter<T> startResetter = new IntegratorResetter<>(integrator)) {
  396.                     integrateDynamics(tStart);
  397.                 }
  398.             }

  400.             // set up events added by user
  401.             setUpUserEventDetectors();

  403.             // set up step handlers
  404.             for (final FieldOrekitStepHandler<T> handler : getMultiplexer().getHandlers()) {
  405.                 integrator.addStepHandler(new FieldAdaptedStepHandler(handler));
  406.             }
  407.             for (final FieldStoringStepHandler generator : ephemerisGenerators) {
  408.                 generator.setEndDate(tEnd);
  409.                 integrator.addStepHandler(generator);
  410.             }

  412.             // propagate from start date to end date with event detection
  413.             return integrateDynamics(tEnd);

  415.         }

  417.     }

  419.     /** Propagation with or without event detection.
  420.      * @param tEnd target date to which orbit should be propagated
  421.      * @return state at end of propagation
  422.      */
  423.     private FieldSpacecraftState<T> integrateDynamics(final FieldAbsoluteDate<T> tEnd) {
  424.         try {

  426.             initializePropagation();

  428.             if (getInitialState().getDate().equals(tEnd)) {
  429.                 // don't extrapolate
  430.                 return getInitialState();
  431.             }
  432.             // space dynamics view
  433.             stateMapper = createMapper(getInitialState().getDate(), stateMapper.getMu(),
  434.                                        stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  435.                                        stateMapper.getAttitudeProvider(), getInitialState().getFrame());


  438.             // set propagation orbit type
  439.             //final FieldOrbit<T> initialOrbit = stateMapper.getOrbitType().convertType(getInitialState().getOrbit());
  440.             if (Double.isNaN(getMu().getReal())) {
  441.                 setMu(getInitialState().getMu());
  442.             }
  443.             if (getInitialState().getMass().getReal() <= 0.0) {
  444.                 throw new OrekitException(OrekitMessages.SPACECRAFT_MASS_BECOMES_NEGATIVE,
  445.                                                getInitialState().getMass());
  446.             }

  448.             // convert space flight dynamics API to math API
  449.             final FieldSpacecraftState<T> initialIntegrationState = getInitialIntegrationState();
  450.             final FieldODEState<T> mathInitialState = createInitialState(initialIntegrationState);
  451.             final FieldExpandableODE<T> mathODE = createODE(integrator, mathInitialState);

  453.             // mathematical integration
  454.             final FieldODEStateAndDerivative<T> mathFinalState;
  455.             beforeIntegration(initialIntegrationState, tEnd);
  456.             mathFinalState = integrator.integrate(mathODE, mathInitialState,
  457.                                                   tEnd.durationFrom(getInitialState().getDate()));

  459.             afterIntegration();

  461.             // get final state
  462.             FieldSpacecraftState<T> finalState =
  463.                             stateMapper.mapArrayToState(stateMapper.mapDoubleToDate(mathFinalState.getTime(), tEnd),
  464.                                                         mathFinalState.getPrimaryState(),
  465.                                                         mathFinalState.getPrimaryDerivative(),
  466.                                                         propagationType);
  467.             if (!additionalDerivativesProviders.isEmpty()) {
  468.                 final T[] secondary            = mathFinalState.getSecondaryState(1);
  469.                 final T[] secondaryDerivatives = mathFinalState.getSecondaryDerivative(1);
  470.                 for (FieldAdditionalDerivativesProvider<T> provider : additionalDerivativesProviders) {
  471.                     final String   name        = provider.getName();
  472.                     final int      offset      = secondaryOffsets.get(name);
  473.                     final int      dimension   = provider.getDimension();
  474.                     finalState = finalState.
  475.                                  addAdditionalState(name, Arrays.copyOfRange(secondary, offset, offset + dimension)).
  476.                                  addAdditionalStateDerivative(name, Arrays.copyOfRange(secondaryDerivatives, offset, offset + dimension));
  477.                 }
  478.             }
  479.             finalState = updateAdditionalStates(finalState);

  481.             if (resetAtEnd) {
  482.                 resetInitialState(finalState);
  483.                 setStartDate(finalState.getDate());
  484.             }

  486.             return finalState;

  488.         } catch (OrekitException pe) {
  489.             throw pe;
  490.         } catch (MathIllegalArgumentException | MathIllegalStateException me) {
  491.             throw OrekitException.unwrap(me);
  492.         }
  493.     }

  495.     /** Get the initial state for integration.
  496.      * @return initial state for integration
  497.      */
  498.     protected FieldSpacecraftState<T> getInitialIntegrationState() {
  499.         return getInitialState();
  500.     }

  502.     /** Create an initial state.
  503.      * @param initialState initial state in flight dynamics world
  504.      * @return initial state in mathematics world
  505.      */
  506.     private FieldODEState<T> createInitialState(final FieldSpacecraftState<T> initialState) {

  508.         // retrieve initial state
  509.         final T[] primary  = MathArrays.buildArray(initialState.getA().getField(), getBasicDimension());
  510.         stateMapper.mapStateToArray(initialState, primary, null);

  512.         if (secondaryOffsets.isEmpty()) {
  513.             // compute dimension of the secondary state
  514.             int offset = 0;
  515.             for (final FieldAdditionalDerivativesProvider<T> provider : additionalDerivativesProviders) {
  516.                 secondaryOffsets.put(provider.getName(), offset);
  517.                 offset += provider.getDimension();
  518.             }
  519.             secondaryOffsets.put(SECONDARY_DIMENSION, offset);
  520.         }

  522.         return new FieldODEState<>(initialState.getA().getField().getZero(), primary, secondary(initialState));

  524.     }

  526.     /** Create secondary state.
  527.      * @param state spacecraft state
  528.      * @return secondary state
  529.      * @since 11.1
  530.      */
  531.     private T[][] secondary(final FieldSpacecraftState<T> state) {

  533.         if (secondaryOffsets.isEmpty()) {
  534.             return null;
  535.         }

  537.         final T[][] secondary = MathArrays.buildArray(state.getDate().getField(), 1, secondaryOffsets.get(SECONDARY_DIMENSION));
  538.         for (final FieldAdditionalDerivativesProvider<T> provider : additionalDerivativesProviders) {
  539.             final String name       = provider.getName();
  540.             final int    offset     = secondaryOffsets.get(name);
  541.             final T[]    additional = state.getAdditionalState(name);
  542.             System.arraycopy(additional, 0, secondary[0], offset, additional.length);
  543.         }

  545.         return secondary;

  547.     }

  549.     /** Create secondary state derivative.
  550.      * @param state spacecraft state
  551.      * @return secondary state derivative
  552.      * @since 11.1
  553.      */
  554.     private T[][] secondaryDerivative(final FieldSpacecraftState<T> state) {

  556.         if (secondaryOffsets.isEmpty()) {
  557.             return null;
  558.         }

  560.         final T[][] secondaryDerivative = MathArrays.buildArray(state.getDate().getField(), 1, secondaryOffsets.get(SECONDARY_DIMENSION));
  561.         for (final FieldAdditionalDerivativesProvider<T> providcer : additionalDerivativesProviders) {
  562.             final String name       = providcer.getName();
  563.             final int    offset     = secondaryOffsets.get(name);
  564.             final T[]    additionalDerivative = state.getAdditionalStateDerivative(name);
  565.             System.arraycopy(additionalDerivative, 0, secondaryDerivative[0], offset, additionalDerivative.length);
  566.         }

  568.         return secondaryDerivative;

  570.     }

  572.     /** Create an ODE with all equations.
  573.      * @param integ numerical integrator to use for propagation.
  574.      * @param mathInitialState initial state
  575.      * @return a new ode
  576.      */
  577.     private FieldExpandableODE<T> createODE(final FieldODEIntegrator<T> integ,
  578.                                     final FieldODEState<T> mathInitialState) {

  580.         final FieldExpandableODE<T> ode =
  581.                 new FieldExpandableODE<>(new ConvertedMainStateEquations(getMainStateEquations(integ)));

  583.         // secondary part of the ODE
  584.         if (!additionalDerivativesProviders.isEmpty()) {
  585.             ode.addSecondaryEquations(new ConvertedSecondaryStateEquations());
  586.         }

  588.         return ode;

  590.     }

  592.     /** Method called just before integration.
  593.      * <p>
  594.      * The default implementation does nothing, it may be specialized in subclasses.
  595.      * </p>
  596.      * @param initialState initial state
  597.      * @param tEnd target date at which state should be propagated
  598.      */
  599.     protected void beforeIntegration(final FieldSpacecraftState<T> initialState,
  600.                                      final FieldAbsoluteDate<T> tEnd) {
  601.         // do nothing by default
  602.     }

  604.     /** Method called just after integration.
  605.      * <p>
  606.      * The default implementation does nothing, it may be specialized in subclasses.
  607.      * </p>
  608.      */
  609.     protected void afterIntegration() {
  610.         // do nothing by default
  611.     }

  613.     /** Get state vector dimension without additional parameters.
  614.      * @return state vector dimension without additional parameters.
  615.      */
  616.     public int getBasicDimension() {
  617.         return 7;

  619.     }

  621.     /** Get the integrator used by the propagator.
  622.      * @return the integrator.
  623.      */
  624.     protected FieldODEIntegrator<T> getIntegrator() {
  625.         return integrator;
  626.     }

  628.     /** Convert a state from mathematical world to space flight dynamics world.
  629.      * @param os mathematical state
  630.      * @return space flight dynamics state
  631.      */
  632.     private FieldSpacecraftState<T> convert(final FieldODEStateAndDerivative<T> os) {

  634.         FieldSpacecraftState<T> s =
  635.                         stateMapper.mapArrayToState(os.getTime(),
  636.                                                     os.getPrimaryState(),
  637.                                                     os.getPrimaryDerivative(),
  638.                                                     propagationType);
  639.         if (os.getNumberOfSecondaryStates() > 0) {
  640.             final T[] secondary           = os.getSecondaryState(1);
  641.             final T[] secondaryDerivative = os.getSecondaryDerivative(1);
  642.             for (final FieldAdditionalDerivativesProvider<T> equations : additionalDerivativesProviders) {
  643.                 final String name      = equations.getName();
  644.                 final int    offset    = secondaryOffsets.get(name);
  645.                 final int    dimension = equations.getDimension();
  646.                 s = s.addAdditionalState(name, Arrays.copyOfRange(secondary, offset, offset + dimension));
  647.                 s = s.addAdditionalStateDerivative(name, Arrays.copyOfRange(secondaryDerivative, offset, offset + dimension));
  648.             }
  649.         }
  650.         s = updateAdditionalStates(s);

  652.         return s;

  654.     }

  656.     /** Convert a state from space flight dynamics world to mathematical world.
  657.      * @param state space flight dynamics state
  658.      * @return mathematical state
  659.      */
  660.     private FieldODEStateAndDerivative<T> convert(final FieldSpacecraftState<T> state) {

  662.         // retrieve initial state
  663.         final T[] primary    = MathArrays.buildArray(getField(), getBasicDimension());
  664.         final T[] primaryDot = MathArrays.buildArray(getField(), getBasicDimension());
  665.         stateMapper.mapStateToArray(state, primary, primaryDot);

  667.         // secondary part of the ODE
  668.         final T[][] secondary           = secondary(state);
  669.         final T[][] secondaryDerivative = secondaryDerivative(state);

  671.         return new FieldODEStateAndDerivative<>(stateMapper.mapDateToDouble(state.getDate()),
  672.                                                 primary, primaryDot,
  673.                                                 secondary, secondaryDerivative);

  675.     }

  677.     /** Differential equations for the main state (orbit, attitude and mass). */
  678.     public interface MainStateEquations<T extends CalculusFieldElement<T>> {

  680.         /**
  681.          * Initialize the equations at the start of propagation. This method will be
  682.          * called before any calls to {@link #computeDerivatives(FieldSpacecraftState)}.
  683.          *
  684.          * <p> The default implementation of this method does nothing.
  685.          *
  686.          * @param initialState initial state information at the start of propagation.
  687.          * @param target       date of propagation. Not equal to {@code
  688.          *                     initialState.getDate()}.
  689.          */
  690.         void init(FieldSpacecraftState<T> initialState, FieldAbsoluteDate<T> target);

  692.         /** Compute differential equations for main state.
  693.          * @param state current state
  694.          * @return derivatives of main state
  695.          */
  696.         T[] computeDerivatives(FieldSpacecraftState<T> state);

  698.     }

  700.     /** Differential equations for the main state (orbit, attitude and mass), with converted API. */
  701.     private class ConvertedMainStateEquations implements FieldOrdinaryDifferentialEquation<T> {

  703.         /** Main state equations. */
  704.         private final MainStateEquations<T> main;

  706.         /** Simple constructor.
  707.          * @param main main state equations
  708.          */
  709.         ConvertedMainStateEquations(final MainStateEquations<T> main) {
  710.             this.main = main;
  711.             calls = 0;
  712.         }

  714.         /** {@inheritDoc} */
  715.         public int getDimension() {
  716.             return getBasicDimension();
  717.         }

  719.         @Override
  720.         public void init(final T t0, final T[] y0, final T finalTime) {
  721.             // update space dynamics view
  722.             FieldSpacecraftState<T> initialState = stateMapper.mapArrayToState(t0, y0, null, PropagationType.MEAN);
  723.             initialState = updateAdditionalStates(initialState);
  724.             final FieldAbsoluteDate<T> target = stateMapper.mapDoubleToDate(finalTime);
  725.             main.init(initialState, target);
  726.         }
  727.         /** {@inheritDoc} */
  728.         public T[] computeDerivatives(final T t, final T[] y) {

  730.             // increment calls counter
  731.             ++calls;

  733.             // update space dynamics view
  734.             FieldSpacecraftState<T> currentState = stateMapper.mapArrayToState(t, y, null, PropagationType.MEAN);
  735.             currentState = updateAdditionalStates(currentState);

  737.             // compute main state differentials
  738.             return main.computeDerivatives(currentState);

  740.         }

  742.     }

  744.     /** Differential equations for the secondary state (Jacobians, user variables ...), with converted API. */
  745.     private class ConvertedSecondaryStateEquations implements FieldSecondaryODE<T> {

  747.         /** Dimension of the combined additional states. */
  748.         private final int combinedDimension;

  750.         /** Simple constructor.
  751.          */
  752.         ConvertedSecondaryStateEquations() {
  753.             this.combinedDimension = secondaryOffsets.get(SECONDARY_DIMENSION);
  754.         }

  756.         /** {@inheritDoc} */
  757.         @Override
  758.         public int getDimension() {
  759.             return combinedDimension;
  760.         }

  762.         /** {@inheritDoc} */
  763.         @Override
  764.         public void init(final T t0, final T[] primary0,
  765.                          final T[] secondary0, final T finalTime) {
  766.             // update space dynamics view
  767.             final FieldSpacecraftState<T> initialState = convert(t0, primary0, null, secondary0);

  769.             final FieldAbsoluteDate<T> target = stateMapper.mapDoubleToDate(finalTime);
  770.             for (final FieldAdditionalDerivativesProvider<T> provider : additionalDerivativesProviders) {
  771.                 provider.init(initialState, target);
  772.             }

  774.         }

  776.         /** {@inheritDoc} */
  777.         @Override
  778.         public T[] computeDerivatives(final T t, final T[] primary,
  779.                                       final T[] primaryDot, final T[] secondary) {

  781.             // update space dynamics view
  782.             // the integrable generators generate method will be called here,
  783.             // according to the generators yield order
  784.             FieldSpacecraftState<T> updated = convert(t, primary, primaryDot, secondary);

  786.             // set up queue for equations
  787.             final Queue<FieldAdditionalDerivativesProvider<T>> pending = new LinkedList<>(additionalDerivativesProviders);

  789.             // gather the derivatives from all additional equations, taking care of dependencies
  790.             final T[] secondaryDot = MathArrays.buildArray(t.getField(), combinedDimension);
  791.             int yieldCount = 0;
  792.             while (!pending.isEmpty()) {
  793.                 final FieldAdditionalDerivativesProvider<T> equations = pending.remove();
  794.                 if (equations.yield(updated)) {
  795.                     // these equations have to wait for another set,
  796.                     // we put them again in the pending queue
  797.                     pending.add(equations);
  798.                     if (++yieldCount >= pending.size()) {
  799.                         // all pending equations yielded!, they probably need data not yet initialized
  800.                         // we let the propagation proceed, if these data are really needed right now
  801.                         // an appropriate exception will be triggered when caller tries to access them
  802.                         break;
  803.                     }
  804.                 } else {
  805.                     // we can use these equations right now
  806.                     final String name        = equations.getName();
  807.                     final int    offset      = secondaryOffsets.get(name);
  808.                     final int    dimension   = equations.getDimension();
  809.                     final T[]    derivatives = equations.derivatives(updated);
  810.                     System.arraycopy(derivatives, 0, secondaryDot, offset, dimension);
  811.                     updated = updated.addAdditionalStateDerivative(name, derivatives);
  812.                     yieldCount = 0;
  813.                 }
  814.             }

  816.             return secondaryDot;

  818.         }

  820.         /** Convert mathematical view to space view.
  821.          * @param t current value of the independent <I>time</I> variable
  822.          * @param primary array containing the current value of the primary state vector
  823.          * @param primaryDot array containing the derivative of the primary state vector
  824.          * @param secondary array containing the current value of the secondary state vector
  825.          * @return space view of the state
  826.          */
  827.         private FieldSpacecraftState<T> convert(final T t, final T[] primary,
  828.                                                 final T[] primaryDot, final T[] secondary) {

  830.             FieldSpacecraftState<T> initialState = stateMapper.mapArrayToState(t, primary, primaryDot, PropagationType.MEAN);

  832.             for (final FieldAdditionalDerivativesProvider<T> provider : additionalDerivativesProviders) {
  833.                 final String name      = provider.getName();
  834.                 final int    offset    = secondaryOffsets.get(name);
  835.                 final int    dimension = provider.getDimension();
  836.                 initialState = initialState.addAdditionalState(name, Arrays.copyOfRange(secondary, offset, offset + dimension));
  837.             }

  839.             return updateAdditionalStates(initialState);

  841.         }

  843.     }

  845.     /** Adapt an {@link org.orekit.propagation.events.FieldEventDetector<T>}
  846.      * to Hipparchus {@link org.hipparchus.ode.events.FieldODEEventHandler<T>} interface.
  847.      * @param <T> class type for the generic version
  848.      * @author Fabien Maussion
  849.      */
  850.     private class FieldAdaptedEventDetector implements FieldODEEventHandler<T> {

  852.         /** Underlying event detector. */
  853.         private final FieldEventDetector<T> detector;

  855.         /** Time of the previous call to g. */
  856.         private T lastT;

  858.         /** Value from the previous call to g. */
  859.         private T lastG;

  861.         /** Build a wrapped event detector.
  862.          * @param detector event detector to wrap
  863.         */
  864.         FieldAdaptedEventDetector(final FieldEventDetector<T> detector) {
  865.             this.detector = detector;
  866.             this.lastT    = getField().getZero().add(Double.NaN);
  867.             this.lastG    = getField().getZero().add(Double.NaN);
  868.         }

  870.         /** {@inheritDoc} */
  871.         public void init(final FieldODEStateAndDerivative<T> s0, final T t) {
  872.             detector.init(convert(s0), stateMapper.mapDoubleToDate(t));
  873.             this.lastT = getField().getZero().add(Double.NaN);
  874.             this.lastG = getField().getZero().add(Double.NaN);
  875.         }

  877.         /** {@inheritDoc} */
  878.         public T g(final FieldODEStateAndDerivative<T> s) {
  879.             if (!Precision.equals(lastT.getReal(), s.getTime().getReal(), 0)) {
  880.                 lastT = s.getTime();
  881.                 lastG = detector.g(convert(s));
  882.             }
  883.             return lastG;
  884.         }

  886.         /** {@inheritDoc} */
  887.         public Action eventOccurred(final FieldODEStateAndDerivative<T> s, final boolean increasing) {
  888.             return detector.eventOccurred(convert(s), increasing);
  889.         }

  891.         /** {@inheritDoc} */
  892.         public FieldODEState<T> resetState(final FieldODEStateAndDerivative<T> s) {

  894.             final FieldSpacecraftState<T> oldState = convert(s);
  895.             final FieldSpacecraftState<T> newState = detector.resetState(oldState);
  896.             stateChanged(newState);

  898.             // main part
  899.             final T[] primary    = MathArrays.buildArray(getField(), s.getPrimaryStateDimension());
  900.             stateMapper.mapStateToArray(newState, primary, null);

  902.             // secondary part
  903.             final T[][] secondary = MathArrays.buildArray(getField(), 1, additionalDerivativesProviders.size());
  904.             for (final FieldAdditionalDerivativesProvider<T> provider : additionalDerivativesProviders) {
  905.                 final String name      = provider.getName();
  906.                 final int    offset    = secondaryOffsets.get(name);
  907.                 final int    dimension = provider.getDimension();
  908.                 System.arraycopy(newState.getAdditionalState(name), 0, secondary[0], offset, dimension);
  909.             }

  911.             return new FieldODEState<>(newState.getDate().durationFrom(getStartDate()),
  912.                                        primary, secondary);
  913.         }

  915.     }

  917.     /** Adapt an {@link org.orekit.propagation.sampling.FieldOrekitStepHandler<T>}
  918.      * to Hipparchus {@link FieldODEStepHandler<T>} interface.
  919.      * @author Luc Maisonobe
  920.      */
  921.     private class FieldAdaptedStepHandler implements FieldODEStepHandler<T> {

  923.         /** Underlying handler. */
  924.         private final FieldOrekitStepHandler<T> handler;

  926.         /** Build an instance.
  927.          * @param handler underlying handler to wrap
  928.          */
  929.         FieldAdaptedStepHandler(final FieldOrekitStepHandler<T> handler) {
  930.             this.handler = handler;
  931.         }

  933.         /** {@inheritDoc} */
  934.         public void init(final FieldODEStateAndDerivative<T> s0, final T t) {
  935.             handler.init(convert(s0), stateMapper.mapDoubleToDate(t));
  936.         }

  938.         /** {@inheritDoc} */
  939.         public void handleStep(final FieldODEStateInterpolator<T> interpolator) {
  940.             handler.handleStep(new FieldAdaptedStepInterpolator(interpolator));
  941.         }

  943.         /** {@inheritDoc} */
  944.         @Override
  945.         public void finish(final FieldODEStateAndDerivative<T> finalState) {
  946.             handler.finish(convert(finalState));
  947.         }

  949.     }

  951.     /** Adapt an {@link org.orekit.propagation.sampling.FieldOrekitStepInterpolator<T>}
  952.      * to Hipparchus {@link FieldODEStepInterpolator<T>} interface.
  953.      * @author Luc Maisonobe
  954.      */
  955.     private class FieldAdaptedStepInterpolator implements FieldOrekitStepInterpolator<T> {

  957.         /** Underlying raw rawInterpolator. */
  958.         private final FieldODEStateInterpolator<T> mathInterpolator;

  960.         /** Build an instance.
  961.          * @param mathInterpolator underlying raw interpolator
  962.          */
  963.         FieldAdaptedStepInterpolator(final FieldODEStateInterpolator<T> mathInterpolator) {
  964.             this.mathInterpolator = mathInterpolator;
  965.         }

  967.         /** {@inheritDoc}} */
  968.         @Override
  969.         public FieldSpacecraftState<T> getPreviousState() {
  970.             return convert(mathInterpolator.getPreviousState());
  971.         }

  973.         /** {@inheritDoc}} */
  974.         @Override
  975.         public FieldSpacecraftState<T> getCurrentState() {
  976.             return convert(mathInterpolator.getCurrentState());
  977.         }

  979.         /** {@inheritDoc}} */
  980.         @Override
  981.         public FieldSpacecraftState<T> getInterpolatedState(final FieldAbsoluteDate<T> date) {
  982.             return convert(mathInterpolator.getInterpolatedState(date.durationFrom(getStartDate())));
  983.         }

  985.         /** Check is integration direction is forward in date.
  986.          * @return true if integration is forward in date
  987.          */
  988.         public boolean isForward() {
  989.             return mathInterpolator.isForward();
  990.         }

  992.         /** {@inheritDoc}} */
  993.         @Override
  994.         public FieldAdaptedStepInterpolator restrictStep(final FieldSpacecraftState<T> newPreviousState,
  995.                                                          final FieldSpacecraftState<T> newCurrentState) {
  996.             try {
  997.                 final AbstractFieldODEStateInterpolator<T> aosi = (AbstractFieldODEStateInterpolator<T>) mathInterpolator;
  998.                 return new FieldAdaptedStepInterpolator(aosi.restrictStep(convert(newPreviousState),
  999.                                                                           convert(newCurrentState)));
  1000.             } catch (ClassCastException cce) {
  1001.                 // this should never happen
  1002.                 throw new OrekitInternalError(cce);
  1003.             }
  1004.         }

  1006.     }

  1008.     /** Specialized step handler storing interpolators for ephemeris generation.
  1009.      * @since 11.0
  1010.      */
  1011.     private class FieldStoringStepHandler implements FieldODEStepHandler<T>, FieldEphemerisGenerator<T> {

  1013.         /** Underlying raw mathematical model. */
  1014.         private FieldDenseOutputModel<T> model;

  1016.         /** the user supplied end date. Propagation may not end on this date. */
  1017.         private FieldAbsoluteDate<T> endDate;

  1019.         /** Generated ephemeris. */
  1020.         private FieldBoundedPropagator<T> ephemeris;

  1022.         /** Set the end date.
  1023.          * @param endDate end date
  1024.          */
  1025.         public void setEndDate(final FieldAbsoluteDate<T> endDate) {
  1026.             this.endDate = endDate;
  1027.         }

  1029.         /** {@inheritDoc} */
  1030.         @Override
  1031.         public void init(final FieldODEStateAndDerivative<T> s0, final T t) {
  1032.             this.model = new FieldDenseOutputModel<>();
  1033.             model.init(s0, t);

  1035.             // ephemeris will be generated when last step is processed
  1036.             this.ephemeris = null;

  1038.         }

  1040.         /** {@inheritDoc} */
  1041.         @Override
  1042.         public FieldBoundedPropagator<T> getGeneratedEphemeris() {
  1043.             return ephemeris;
  1044.         }

  1046.         /** {@inheritDoc} */
  1047.         @Override
  1048.         public void handleStep(final FieldODEStateInterpolator<T> interpolator) {
  1049.             model.handleStep(interpolator);
  1050.         }

  1052.         /** {@inheritDoc} */
  1053.         @Override
  1054.         public void finish(final FieldODEStateAndDerivative<T> finalState) {

  1056.             // set up the boundary dates
  1057.             final T tI = model.getInitialTime();
  1058.             final T tF = model.getFinalTime();
  1059.             // tI is almost? always zero
  1060.             final FieldAbsoluteDate<T> startDate =
  1061.                             stateMapper.mapDoubleToDate(tI);
  1062.             final FieldAbsoluteDate<T> finalDate =
  1063.                             stateMapper.mapDoubleToDate(tF, this.endDate);
  1064.             final FieldAbsoluteDate<T> minDate;
  1065.             final FieldAbsoluteDate<T> maxDate;
  1066.             if (tF.getReal() < tI.getReal()) {
  1067.                 minDate = finalDate;
  1068.                 maxDate = startDate;
  1069.             } else {
  1070.                 minDate = startDate;
  1071.                 maxDate = finalDate;
  1072.             }

  1074.             // get the initial additional states that are not managed
  1075.             final FieldArrayDictionary<T> unmanaged = new FieldArrayDictionary<>(startDate.getField());
  1076.             for (final FieldArrayDictionary<T>.Entry initial : getInitialState().getAdditionalStatesValues().getData()) {
  1077.                 if (!isAdditionalStateManaged(initial.getKey())) {
  1078.                     // this additional state was in the initial state, but is unknown to the propagator
  1079.                     // we simply copy its initial value as is
  1080.                     unmanaged.put(initial.getKey(), initial.getValue());
  1081.                 }
  1082.             }

  1084.             // get the names of additional states managed by differential equations
  1085.             final String[] names = new String[additionalDerivativesProviders.size()];
  1086.             for (int i = 0; i < names.length; ++i) {
  1087.                 names[i] = additionalDerivativesProviders.get(i).getName();
  1088.             }

  1090.             // create the ephemeris
  1091.             ephemeris = new FieldIntegratedEphemeris<>(startDate, minDate, maxDate,
  1092.                                                        stateMapper, propagationType, model,
  1093.                                                        unmanaged, getAdditionalStateProviders(), names);

  1095.         }

  1097.     }

  1099.     /** Wrapper for resetting an integrator handlers.
  1100.      * <p>
  1101.      * This class is intended to be used in a try-with-resource statement.
  1102.      * If propagator-specific event handlers and step handlers are added to
  1103.      * the integrator in the try block, they will be removed automatically
  1104.      * when leaving the block, so the integrator only keep its own handlers
  1105.      * between calls to {@link AbstractIntegratedPropagator#propagate(AbsoluteDate, AbsoluteDate).
  1106.      * </p>
  1107.      * @param <T> the type of the field elements
  1108.      * @since 11.0
  1109.      */
  1110.     private static class IntegratorResetter<T extends CalculusFieldElement<T>> implements AutoCloseable {

  1112.         /** Wrapped integrator. */
  1113.         private final FieldODEIntegrator<T> integrator;

  1115.         /** Initial event handlers list. */
  1116.         private final List<FieldEventHandlerConfiguration<T>> eventHandlersConfigurations;

  1118.         /** Initial step handlers list. */
  1119.         private final List<FieldODEStepHandler<T>> stepHandlers;

  1121.         /** Simple constructor.
  1122.          * @param integrator wrapped integrator
  1123.          */
  1124.         IntegratorResetter(final FieldODEIntegrator<T> integrator) {
  1125.             this.integrator                  = integrator;
  1126.             this.eventHandlersConfigurations = new ArrayList<>(integrator.getEventHandlersConfigurations());
  1127.             this.stepHandlers                = new ArrayList<>(integrator.getStepHandlers());
  1128.         }

  1130.         /** {@inheritDoc}
  1131.          * <p>
  1132.          * Reset event handlers and step handlers back to the initial list
  1133.          * </p>
  1134.          */
  1135.         @Override
  1136.         public void close() {

  1138.             // reset event handlers
  1139.             integrator.clearEventHandlers();
  1140.             eventHandlersConfigurations.forEach(c -> integrator.addEventHandler(c.getEventHandler(),
  1141.                                                                                 c.getMaxCheckInterval(),
  1142.                                                                                 c.getConvergence().getReal(),
  1143.                                                                                 c.getMaxIterationCount(),
  1144.                                                                                 c.getSolver()));

  1146.             // reset step handlers
  1147.             integrator.clearStepHandlers();
  1148.             stepHandlers.forEach(stepHandler -> integrator.addStepHandler(stepHandler));

  1150.         }

  1152.     }

  1154. }
Created with JaCoCo 0.8.7.202105040129