AbstractIntegratedPropagator.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.exception.MathRuntimeException;
  30. import org.hipparchus.ode.DenseOutputModel;
  31. import org.hipparchus.ode.ExpandableODE;
  32. import org.hipparchus.ode.ODEIntegrator;
  33. import org.hipparchus.ode.ODEState;
  34. import org.hipparchus.ode.ODEStateAndDerivative;
  35. import org.hipparchus.ode.OrdinaryDifferentialEquation;
  36. import org.hipparchus.ode.SecondaryODE;
  37. import org.hipparchus.ode.events.Action;
  38. import org.hipparchus.ode.events.EventHandlerConfiguration;
  39. import org.hipparchus.ode.events.ODEEventHandler;
  40. import org.hipparchus.ode.sampling.AbstractODEStateInterpolator;
  41. import org.hipparchus.ode.sampling.ODEStateInterpolator;
  42. import org.hipparchus.ode.sampling.ODEStepHandler;
  43. import org.hipparchus.util.Precision;
  44. import org.orekit.attitudes.AttitudeProvider;
  45. import org.orekit.errors.OrekitException;
  46. import org.orekit.errors.OrekitInternalError;
  47. import org.orekit.errors.OrekitMessages;
  48. import org.orekit.frames.Frame;
  49. import org.orekit.orbits.OrbitType;
  50. import org.orekit.orbits.PositionAngle;
  51. import org.orekit.propagation.AbstractPropagator;
  52. import org.orekit.propagation.BoundedPropagator;
  53. import org.orekit.propagation.EphemerisGenerator;
  54. import org.orekit.propagation.PropagationType;
  55. import org.orekit.propagation.SpacecraftState;
  56. import org.orekit.propagation.events.EventDetector;
  57. import org.orekit.propagation.sampling.OrekitStepHandler;
  58. import org.orekit.propagation.sampling.OrekitStepInterpolator;
  59. import org.orekit.time.AbsoluteDate;
  60. import org.orekit.utils.DoubleArrayDictionary;


  63. /** Common handling of {@link org.orekit.propagation.Propagator Propagator}
  64.  *  methods for both numerical and semi-analytical propagators.
  65.  *  @author Luc Maisonobe
  66.  */
  67. public abstract class AbstractIntegratedPropagator extends AbstractPropagator {

  69.     /** Internal name used for complete secondary state dimension.
  70.      * @since 11.1
  71.      */
  72.     private static final String SECONDARY_DIMENSION = "Orekit-secondary-dimension";

  74.     /** Event detectors not related to force models. */
  75.     private final List<EventDetector> detectors;

  77.     /** Step handlers dedicated to ephemeris generation. */
  78.     private final List<StoringStepHandler> ephemerisGenerators;

  80.     /** Integrator selected by the user for the orbital extrapolation process. */
  81.     private final ODEIntegrator integrator;

  83.     /** Offsets of secondary states managed by {@link AdditionalEquations}.
  84.      * @since 11.1
  85.      */
  86.     private final Map<String, Integer> secondaryOffsets;

  88.     /** Additional derivatives providers.
  89.      * @since 11.1
  90.      */
  91.     private List<AdditionalDerivativesProvider> additionalDerivativesProviders;

  93.     /** Map of secondary equation offset in main
  94.     /** Counter for differential equations calls. */
  95.     private int calls;

  97.     /** Mapper between raw double components and space flight dynamics objects. */
  98.     private StateMapper stateMapper;

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

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

  111.     /** Build a new instance.
  112.      * @param integrator numerical integrator to use for propagation.
  113.      * @param propagationType type of orbit to output (mean or osculating).
  114.      */
  115.     protected AbstractIntegratedPropagator(final ODEIntegrator integrator, final PropagationType propagationType) {
  116.         detectors                      = new ArrayList<>();
  117.         ephemerisGenerators            = new ArrayList<>();
  118.         additionalDerivativesProviders = new ArrayList<>();
  119.         this.secondaryOffsets          = new HashMap<>();
  120.         this.integrator                = integrator;
  121.         this.propagationType           = propagationType;
  122.         this.resetAtEnd                = true;
  123.     }

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

  141.     /** Initialize the mapper. */
  142.     protected void initMapper() {
  143.         stateMapper = createMapper(null, Double.NaN, null, null, null, null);
  144.     }

  146.     /**  {@inheritDoc} */
  147.     public void setAttitudeProvider(final AttitudeProvider attitudeProvider) {
  148.         super.setAttitudeProvider(attitudeProvider);
  149.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  150.                                    stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  151.                                    attitudeProvider, stateMapper.getFrame());
  152.     }

  154.     /** Set propagation orbit type.
  155.      * @param orbitType orbit type to use for propagation, null for
  156.      * propagating using {@link org.orekit.utils.AbsolutePVCoordinates AbsolutePVCoordinates}
  157.      * rather than {@link org.orekit.orbits Orbit}
  158.      */
  159.     protected void setOrbitType(final OrbitType orbitType) {
  160.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  161.                                    orbitType, stateMapper.getPositionAngleType(),
  162.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  163.     }

  165.     /** Get propagation parameter type.
  166.      * @return orbit type used for propagation, null for
  167.      * propagating using {@link org.orekit.utils.AbsolutePVCoordinates AbsolutePVCoordinates}
  168.      * rather than {@link org.orekit.orbits Orbit}
  169.      */
  170.     protected OrbitType getOrbitType() {
  171.         return stateMapper.getOrbitType();
  172.     }

  174.     /** Check if only the mean elements should be used in a semianalitical propagation.
  175.      * @return {@link PropagationType MEAN} if only mean elements have to be used or
  176.      *         {@link PropagationType OSCULATING} if osculating elements have to be also used.
  177.      * @deprecated as of 11.1, replaced by {@link #getPropagationType()}
  178.      */
  179.     @Deprecated
  180.     protected PropagationType isMeanOrbit() {
  181.         return getPropagationType();
  182.     }

  184.     /** Get the propagation type.
  185.      * @return propagation type.
  186.      * @since 11.1
  187.      */
  188.     public PropagationType getPropagationType() {
  189.         return propagationType;
  190.     }

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

  207.     /** Get propagation parameter type.
  208.      * @return angle type to use for propagation
  209.      */
  210.     protected PositionAngle getPositionAngleType() {
  211.         return stateMapper.getPositionAngleType();
  212.     }

  214.     /** Set the central attraction coefficient μ.
  215.      * @param mu central attraction coefficient (m³/s²)
  216.      */
  217.     public void setMu(final double mu) {
  218.         stateMapper = createMapper(stateMapper.getReferenceDate(), mu,
  219.                                    stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  220.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  221.     }

  223.     /** Get the central attraction coefficient μ.
  224.      * @return mu central attraction coefficient (m³/s²)
  225.      * @see #setMu(double)
  226.      */
  227.     public double getMu() {
  228.         return stateMapper.getMu();
  229.     }

  231.     /** Get the number of calls to the differential equations computation method.
  232.      * <p>The number of calls is reset each time the {@link #propagate(AbsoluteDate)}
  233.      * method is called.</p>
  234.      * @return number of calls to the differential equations computation method
  235.      */
  236.     public int getCalls() {
  237.         return calls;
  238.     }

  240.     /** {@inheritDoc} */
  241.     @Override
  242.     public boolean isAdditionalStateManaged(final String name) {

  244.         // first look at already integrated states
  245.         if (super.isAdditionalStateManaged(name)) {
  246.             return true;
  247.         }

  249.         // then look at states we integrate ourselves
  250.         for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
  251.             if (provider.getName().equals(name)) {
  252.                 return true;
  253.             }
  254.         }

  256.         return false;
  257.     }

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

  271.     /** Add a set of user-specified equations to be integrated along with the orbit propagation.
  272.      * @param additional additional equations
  273.      * @deprecated as of 11.1, replaced by {@link #addAdditionalDerivativesProvider(AdditionalDerivativesProvider)}
  274.      */
  275.     @Deprecated
  276.     public void addAdditionalEquations(final AdditionalEquations additional) {
  277.         addAdditionalDerivativesProvider(new AdditionalEquationsAdapter(additional, this::getInitialState));
  278.     }

  280.     /** Add a provider for user-specified state derivatives to be integrated along with the orbit propagation.
  281.      * @param provider provider for additional derivatives
  282.      * @see #addAdditionalStateProvider(org.orekit.propagation.AdditionalStateProvider)
  283.      * @since 11.1
  284.      */
  285.     public void addAdditionalDerivativesProvider(final AdditionalDerivativesProvider provider) {

  287.         // check if the name is already used
  288.         if (isAdditionalStateManaged(provider.getName())) {
  289.             // these derivatives are already registered, complain
  290.             throw new OrekitException(OrekitMessages.ADDITIONAL_STATE_NAME_ALREADY_IN_USE,
  291.                                       provider.getName());
  292.         }

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

  297.         secondaryOffsets.clear();

  299.     }

  301.     /** Get an unmodifiable list of providers for additional derivatives.
  302.      * @return providers for the additional derivatives
  303.      * @since 11.1
  304.      */
  305.     public List<AdditionalDerivativesProvider> getAdditionalDerivativesProviders() {
  306.         return Collections.unmodifiableList(additionalDerivativesProviders);
  307.     }

  309.     /** {@inheritDoc} */
  310.     public void addEventDetector(final EventDetector detector) {
  311.         detectors.add(detector);
  312.     }

  314.     /** {@inheritDoc} */
  315.     public Collection<EventDetector> getEventsDetectors() {
  316.         return Collections.unmodifiableCollection(detectors);
  317.     }

  319.     /** {@inheritDoc} */
  320.     public void clearEventsDetectors() {
  321.         detectors.clear();
  322.     }

  324.     /** Set up all user defined event detectors.
  325.      */
  326.     protected void setUpUserEventDetectors() {
  327.         for (final EventDetector detector : detectors) {
  328.             setUpEventDetector(integrator, detector);
  329.         }
  330.     }

  332.     /** Wrap an Orekit event detector and register it to the integrator.
  333.      * @param integ integrator into which event detector should be registered
  334.      * @param detector event detector to wrap
  335.      */
  336.     protected void setUpEventDetector(final ODEIntegrator integ, final EventDetector detector) {
  337.         integ.addEventHandler(new AdaptedEventDetector(detector),
  338.                               detector.getMaxCheckInterval(),
  339.                               detector.getThreshold(),
  340.                               detector.getMaxIterationCount());
  341.     }

  343.     /** {@inheritDoc} */
  344.     @Override
  345.     public EphemerisGenerator getEphemerisGenerator() {
  346.         final StoringStepHandler storingHandler = new StoringStepHandler();
  347.         ephemerisGenerators.add(storingHandler);
  348.         return storingHandler;
  349.     }

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

  371.     /** Get the differential equations to integrate (for main state only).
  372.      * @param integ numerical integrator to use for propagation.
  373.      * @return differential equations for main state
  374.      */
  375.     protected abstract MainStateEquations getMainStateEquations(ODEIntegrator integ);

  377.     /** {@inheritDoc} */
  378.     public SpacecraftState propagate(final AbsoluteDate target) {
  379.         if (getStartDate() == null) {
  380.             if (getInitialState() == null) {
  381.                 throw new OrekitException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
  382.             }
  383.             setStartDate(getInitialState().getDate());
  384.         }
  385.         return propagate(getStartDate(), target);
  386.     }

  388.     /** {@inheritDoc} */
  389.     public SpacecraftState propagate(final AbsoluteDate tStart, final AbsoluteDate tEnd) {

  391.         if (getInitialState() == null) {
  392.             throw new OrekitException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
  393.         }

  395.         // make sure the integrator will be reset properly even if we change its events handlers and step handlers
  396.         try (IntegratorResetter resetter = new IntegratorResetter(integrator)) {

  398.             // prepare handling of STM and Jacobian matrices
  399.             setUpStmAndJacobianGenerators();

  401.             // Initialize additional states
  402.             initializeAdditionalStates(tEnd);

  404.             if (!tStart.equals(getInitialState().getDate())) {
  405.                 // if propagation start date is not initial date,
  406.                 // propagate from initial to start date without event detection
  407.                 try (IntegratorResetter startResetter = new IntegratorResetter(integrator)) {
  408.                     integrateDynamics(tStart);
  409.                 }
  410.             }

  412.             // set up events added by user
  413.             setUpUserEventDetectors();

  415.             // set up step handlers
  416.             for (final OrekitStepHandler handler : getMultiplexer().getHandlers()) {
  417.                 integrator.addStepHandler(new AdaptedStepHandler(handler));
  418.             }
  419.             for (final StoringStepHandler generator : ephemerisGenerators) {
  420.                 generator.setEndDate(tEnd);
  421.                 integrator.addStepHandler(generator);
  422.             }

  424.             // propagate from start date to end date with event detection
  425.             final SpacecraftState finalState = integrateDynamics(tEnd);

  427.             return finalState;

  429.         }

  431.     }

  433.     /** Set up State Transition Matrix and Jacobian matrix handling.
  434.      * @since 11.1
  435.      */
  436.     protected void setUpStmAndJacobianGenerators() {
  437.         // nothing to do by default
  438.     }

  440.     /** Propagation with or without event detection.
  441.      * @param tEnd target date to which orbit should be propagated
  442.      * @return state at end of propagation
  443.      */
  444.     private SpacecraftState integrateDynamics(final AbsoluteDate tEnd) {
  445.         try {

  447.             initializePropagation();

  449.             if (getInitialState().getDate().equals(tEnd)) {
  450.                 // don't extrapolate
  451.                 return getInitialState();
  452.             }

  454.             // space dynamics view
  455.             stateMapper = createMapper(getInitialState().getDate(), stateMapper.getMu(),
  456.                                        stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  457.                                        stateMapper.getAttitudeProvider(), getInitialState().getFrame());


  460.             if (Double.isNaN(getMu())) {
  461.                 setMu(getInitialState().getMu());
  462.             }

  464.             if (getInitialState().getMass() <= 0.0) {
  465.                 throw new OrekitException(OrekitMessages.SPACECRAFT_MASS_BECOMES_NEGATIVE,
  466.                                           getInitialState().getMass());
  467.             }

  469.             // convert space flight dynamics API to math API
  470.             final SpacecraftState initialIntegrationState = getInitialIntegrationState();
  471.             final ODEState mathInitialState = createInitialState(initialIntegrationState);
  472.             final ExpandableODE mathODE = createODE(integrator, mathInitialState);

  474.             // mathematical integration
  475.             final ODEStateAndDerivative mathFinalState;
  476.             beforeIntegration(initialIntegrationState, tEnd);
  477.             mathFinalState = integrator.integrate(mathODE, mathInitialState,
  478.                                                   tEnd.durationFrom(getInitialState().getDate()));
  479.             afterIntegration();

  481.             // get final state
  482.             SpacecraftState finalState =
  483.                             stateMapper.mapArrayToState(stateMapper.mapDoubleToDate(mathFinalState.getTime(),
  484.                                                                                     tEnd),
  485.                                                         mathFinalState.getPrimaryState(),
  486.                                                         mathFinalState.getPrimaryDerivative(),
  487.                                                         propagationType);

  489.             if (!additionalDerivativesProviders.isEmpty()) {
  490.                 final double[] secondary            = mathFinalState.getSecondaryState(1);
  491.                 final double[] secondaryDerivatives = mathFinalState.getSecondaryDerivative(1);
  492.                 for (AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
  493.                     final String   name        = provider.getName();
  494.                     final int      offset      = secondaryOffsets.get(name);
  495.                     final int      dimension   = provider.getDimension();
  496.                     finalState = finalState.
  497.                                  addAdditionalState(name, Arrays.copyOfRange(secondary, offset, offset + dimension)).
  498.                                  addAdditionalStateDerivative(name, Arrays.copyOfRange(secondaryDerivatives, offset, offset + dimension));
  499.                 }
  500.             }
  501.             finalState = updateAdditionalStates(finalState);

  503.             if (resetAtEnd) {
  504.                 resetInitialState(finalState);
  505.                 setStartDate(finalState.getDate());
  506.             }

  508.             return finalState;

  510.         } catch (MathRuntimeException mre) {
  511.             throw OrekitException.unwrap(mre);
  512.         }
  513.     }

  515.     /** Get the initial state for integration.
  516.      * @return initial state for integration
  517.      */
  518.     protected SpacecraftState getInitialIntegrationState() {
  519.         return getInitialState();
  520.     }

  522.     /** Create an initial state.
  523.      * @param initialState initial state in flight dynamics world
  524.      * @return initial state in mathematics world
  525.      */
  526.     private ODEState createInitialState(final SpacecraftState initialState) {

  528.         // retrieve initial state
  529.         final double[] primary = new double[getBasicDimension()];
  530.         stateMapper.mapStateToArray(initialState, primary, null);

  532.         if (secondaryOffsets.isEmpty()) {
  533.             // compute dimension of the secondary state
  534.             int offset = 0;
  535.             for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
  536.                 secondaryOffsets.put(provider.getName(), offset);
  537.                 offset += provider.getDimension();
  538.             }
  539.             secondaryOffsets.put(SECONDARY_DIMENSION, offset);
  540.         }

  542.         return new ODEState(0.0, primary, secondary(initialState));

  544.     }

  546.     /** Create secondary state.
  547.      * @param state spacecraft state
  548.      * @return secondary state
  549.      * @since 11.1
  550.      */
  551.     private double[][] secondary(final SpacecraftState state) {

  553.         if (secondaryOffsets.isEmpty()) {
  554.             return null;
  555.         }

  557.         final double[][] secondary = new double[1][secondaryOffsets.get(SECONDARY_DIMENSION)];
  558.         for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
  559.             final String   name       = provider.getName();
  560.             final int      offset     = secondaryOffsets.get(name);
  561.             final double[] additional = state.getAdditionalState(name);
  562.             System.arraycopy(additional, 0, secondary[0], offset, additional.length);
  563.         }

  565.         return secondary;

  567.     }

  569.     /** Create secondary state derivative.
  570.      * @param state spacecraft state
  571.      * @return secondary state derivative
  572.      * @since 11.1
  573.      */
  574.     private double[][] secondaryDerivative(final SpacecraftState state) {

  576.         if (secondaryOffsets.isEmpty()) {
  577.             return null;
  578.         }

  580.         final double[][] secondaryDerivative = new double[1][secondaryOffsets.get(SECONDARY_DIMENSION)];
  581.         for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
  582.             final String   name       = provider.getName();
  583.             final int      offset     = secondaryOffsets.get(name);
  584.             final double[] additionalDerivative = state.getAdditionalStateDerivative(name);
  585.             System.arraycopy(additionalDerivative, 0, secondaryDerivative[0], offset, additionalDerivative.length);
  586.         }

  588.         return secondaryDerivative;

  590.     }

  592.     /** Create an ODE with all equations.
  593.      * @param integ numerical integrator to use for propagation.
  594.      * @param mathInitialState initial state
  595.      * @return a new ode
  596.      */
  597.     private ExpandableODE createODE(final ODEIntegrator integ,
  598.                                     final ODEState mathInitialState) {

  600.         final ExpandableODE ode =
  601.                 new ExpandableODE(new ConvertedMainStateEquations(getMainStateEquations(integ)));

  603.         // secondary part of the ODE
  604.         if (!additionalDerivativesProviders.isEmpty()) {
  605.             ode.addSecondaryEquations(new ConvertedSecondaryStateEquations());
  606.         }

  608.         return ode;

  610.     }

  612.     /** Method called just before integration.
  613.      * <p>
  614.      * The default implementation does nothing, it may be specialized in subclasses.
  615.      * </p>
  616.      * @param initialState initial state
  617.      * @param tEnd target date at which state should be propagated
  618.      */
  619.     protected void beforeIntegration(final SpacecraftState initialState,
  620.                                      final AbsoluteDate tEnd) {
  621.         // do nothing by default
  622.     }

  624.     /** Method called just after integration.
  625.      * <p>
  626.      * The default implementation does nothing, it may be specialized in subclasses.
  627.      * </p>
  628.      */
  629.     protected void afterIntegration() {
  630.         // do nothing by default
  631.     }

  633.     /** Get state vector dimension without additional parameters.
  634.      * @return state vector dimension without additional parameters.
  635.      */
  636.     public int getBasicDimension() {
  637.         return 7;
  638.     }

  640.     /** Get the integrator used by the propagator.
  641.      * @return the integrator.
  642.      */
  643.     protected ODEIntegrator getIntegrator() {
  644.         return integrator;
  645.     }

  647.     /** Convert a state from mathematical world to space flight dynamics world.
  648.      * @param os mathematical state
  649.      * @return space flight dynamics state
  650.      */
  651.     private SpacecraftState convert(final ODEStateAndDerivative os) {

  653.         SpacecraftState s = stateMapper.mapArrayToState(os.getTime(),
  654.                                                         os.getPrimaryState(),
  655.                                                         os.getPrimaryDerivative(),
  656.                                                         propagationType);
  657.         if (os.getNumberOfSecondaryStates() > 0) {
  658.             final double[] secondary           = os.getSecondaryState(1);
  659.             final double[] secondaryDerivative = os.getSecondaryDerivative(1);
  660.             for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
  661.                 final String name      = provider.getName();
  662.                 final int    offset    = secondaryOffsets.get(name);
  663.                 final int    dimension = provider.getDimension();
  664.                 s = s.addAdditionalState(name, Arrays.copyOfRange(secondary, offset, offset + dimension));
  665.                 s = s.addAdditionalStateDerivative(name, Arrays.copyOfRange(secondaryDerivative, offset, offset + dimension));
  666.             }
  667.         }
  668.         s = updateAdditionalStates(s);

  670.         return s;

  672.     }

  674.     /** Convert a state from space flight dynamics world to mathematical world.
  675.      * @param state space flight dynamics state
  676.      * @return mathematical state
  677.      */
  678.     private ODEStateAndDerivative convert(final SpacecraftState state) {

  680.         // retrieve initial state
  681.         final double[] primary    = new double[getBasicDimension()];
  682.         final double[] primaryDot = new double[getBasicDimension()];
  683.         stateMapper.mapStateToArray(state, primary, primaryDot);

  685.         // secondary part of the ODE
  686.         final double[][] secondary           = secondary(state);
  687.         final double[][] secondaryDerivative = secondaryDerivative(state);

  689.         return new ODEStateAndDerivative(stateMapper.mapDateToDouble(state.getDate()),
  690.                                          primary, primaryDot,
  691.                                          secondary, secondaryDerivative);

  693.     }

  695.     /** Differential equations for the main state (orbit, attitude and mass). */
  696.     public interface MainStateEquations {

  698.         /**
  699.          * Initialize the equations at the start of propagation. This method will be
  700.          * called before any calls to {@link #computeDerivatives(SpacecraftState)}.
  701.          *
  702.          * <p> The default implementation of this method does nothing.
  703.          *
  704.          * @param initialState initial state information at the start of propagation.
  705.          * @param target       date of propagation. Not equal to {@code
  706.          *                     initialState.getDate()}.
  707.          */
  708.         default void init(final SpacecraftState initialState, final AbsoluteDate target) {
  709.         }

  711.         /** Compute differential equations for main state.
  712.          * @param state current state
  713.          * @return derivatives of main state
  714.          */
  715.         double[] computeDerivatives(SpacecraftState state);

  717.     }

  719.     /** Differential equations for the main state (orbit, attitude and mass), with converted API. */
  720.     private class ConvertedMainStateEquations implements OrdinaryDifferentialEquation {

  722.         /** Main state equations. */
  723.         private final MainStateEquations main;

  725.         /** Simple constructor.
  726.          * @param main main state equations
  727.          */
  728.         ConvertedMainStateEquations(final MainStateEquations main) {
  729.             this.main = main;
  730.             calls = 0;
  731.         }

  733.         /** {@inheritDoc} */
  734.         public int getDimension() {
  735.             return getBasicDimension();
  736.         }

  738.         @Override
  739.         public void init(final double t0, final double[] y0, final double finalTime) {
  740.             // update space dynamics view
  741.             SpacecraftState initialState = stateMapper.mapArrayToState(t0, y0, null, PropagationType.MEAN);
  742.             initialState = updateAdditionalStates(initialState);
  743.             final AbsoluteDate target = stateMapper.mapDoubleToDate(finalTime);
  744.             main.init(initialState, target);
  745.         }

  747.         /** {@inheritDoc} */
  748.         public double[] computeDerivatives(final double t, final double[] y) {

  750.             // increment calls counter
  751.             ++calls;

  753.             // update space dynamics view
  754.             SpacecraftState currentState = stateMapper.mapArrayToState(t, y, null, PropagationType.MEAN);
  755.             currentState = updateAdditionalStates(currentState);

  757.             // compute main state differentials
  758.             return main.computeDerivatives(currentState);

  760.         }

  762.     }

  764.     /** Differential equations for the secondary state (Jacobians, user variables ...), with converted API. */
  765.     private class ConvertedSecondaryStateEquations implements SecondaryODE {

  767.         /** Dimension of the combined additional states. */
  768.         private final int combinedDimension;

  770.         /** Simple constructor.
  771.           */
  772.         ConvertedSecondaryStateEquations() {
  773.             this.combinedDimension = secondaryOffsets.get(SECONDARY_DIMENSION);
  774.         }

  776.         /** {@inheritDoc} */
  777.         @Override
  778.         public int getDimension() {
  779.             return combinedDimension;
  780.         }

  782.         /** {@inheritDoc} */
  783.         @Override
  784.         public void init(final double t0, final double[] primary0,
  785.                          final double[] secondary0, final double finalTime) {
  786.             // update space dynamics view
  787.             final SpacecraftState initialState = convert(t0, primary0, null, secondary0);

  789.             final AbsoluteDate target = stateMapper.mapDoubleToDate(finalTime);
  790.             for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
  791.                 provider.init(initialState, target);
  792.             }

  794.         }

  796.         /** {@inheritDoc} */
  797.         @Override
  798.         public double[] computeDerivatives(final double t, final double[] primary,
  799.                                            final double[] primaryDot, final double[] secondary) {

  801.             // update space dynamics view
  802.             // the integrable generators generate method will be called here,
  803.             // according to the generators yield order
  804.             SpacecraftState updated = convert(t, primary, primaryDot, secondary);

  806.             // set up queue for equations
  807.             final Queue<AdditionalDerivativesProvider> pending = new LinkedList<>(additionalDerivativesProviders);

  809.             // gather the derivatives from all additional equations, taking care of dependencies
  810.             final double[] secondaryDot = new double[combinedDimension];
  811.             int yieldCount = 0;
  812.             while (!pending.isEmpty()) {
  813.                 final AdditionalDerivativesProvider provider = pending.remove();
  814.                 if (provider.yield(updated)) {
  815.                     // this provider has to wait for another one,
  816.                     // we put it again in the pending queue
  817.                     pending.add(provider);
  818.                     if (++yieldCount >= pending.size()) {
  819.                         // all pending providers yielded!, they probably need data not yet initialized
  820.                         // we let the propagation proceed, if these data are really needed right now
  821.                         // an appropriate exception will be triggered when caller tries to access them
  822.                         break;
  823.                     }
  824.                 } else {
  825.                     // we can use these equations right now
  826.                     final String              name           = provider.getName();
  827.                     final int                 offset         = secondaryOffsets.get(name);
  828.                     final int                 dimension      = provider.getDimension();
  829.                     final CombinedDerivatives derivatives    = provider.combinedDerivatives(updated);
  830.                     final double[]            additionalPart = derivatives.getAdditionalDerivatives();
  831.                     final double[]            mainPart       = derivatives.getMainStateDerivativesIncrements();
  832.                     System.arraycopy(additionalPart, 0, secondaryDot, offset, dimension);
  833.                     updated = updated.addAdditionalStateDerivative(name, additionalPart);
  834.                     if (mainPart != null) {
  835.                         // this equation does change the main state derivatives
  836.                         for (int i = 0; i < mainPart.length; ++i) {
  837.                             primaryDot[i] += mainPart[i];
  838.                         }
  839.                     }
  840.                     yieldCount = 0;
  841.                 }
  842.             }

  844.             return secondaryDot;

  846.         }

  848.         /** Convert mathematical view to space view.
  849.          * @param t current value of the independent <I>time</I> variable
  850.          * @param primary array containing the current value of the primary state vector
  851.          * @param primaryDot array containing the derivative of the primary state vector
  852.          * @param secondary array containing the current value of the secondary state vector
  853.          * @return space view of the state
  854.          */
  855.         private SpacecraftState convert(final double t, final double[] primary,
  856.                                         final double[] primaryDot, final double[] secondary) {

  858.             SpacecraftState initialState = stateMapper.mapArrayToState(t, primary, primaryDot, PropagationType.MEAN);

  860.             for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
  861.                 final String name      = provider.getName();
  862.                 final int    offset    = secondaryOffsets.get(name);
  863.                 final int    dimension = provider.getDimension();
  864.                 initialState = initialState.addAdditionalState(name, Arrays.copyOfRange(secondary, offset, offset + dimension));
  865.             }

  867.             return updateAdditionalStates(initialState);

  869.         }

  871.     }

  873.     /** Adapt an {@link org.orekit.propagation.events.EventDetector}
  874.      * to Hipparchus {@link org.hipparchus.ode.events.ODEEventHandler} interface.
  875.      * @author Fabien Maussion
  876.      */
  877.     private class AdaptedEventDetector implements ODEEventHandler {

  879.         /** Underlying event detector. */
  880.         private final EventDetector detector;

  882.         /** Time of the previous call to g. */
  883.         private double lastT;

  885.         /** Value from the previous call to g. */
  886.         private double lastG;

  888.         /** Build a wrapped event detector.
  889.          * @param detector event detector to wrap
  890.         */
  891.         AdaptedEventDetector(final EventDetector detector) {
  892.             this.detector = detector;
  893.             this.lastT    = Double.NaN;
  894.             this.lastG    = Double.NaN;
  895.         }

  897.         /** {@inheritDoc} */
  898.         public void init(final ODEStateAndDerivative s0, final double t) {
  899.             detector.init(convert(s0), stateMapper.mapDoubleToDate(t));
  900.             this.lastT = Double.NaN;
  901.             this.lastG = Double.NaN;
  902.         }

  904.         /** {@inheritDoc} */
  905.         public double g(final ODEStateAndDerivative s) {
  906.             if (!Precision.equals(lastT, s.getTime(), 0)) {
  907.                 lastT = s.getTime();
  908.                 lastG = detector.g(convert(s));
  909.             }
  910.             return lastG;
  911.         }

  913.         /** {@inheritDoc} */
  914.         public Action eventOccurred(final ODEStateAndDerivative s, final boolean increasing) {
  915.             return detector.eventOccurred(convert(s), increasing);
  916.         }

  918.         /** {@inheritDoc} */
  919.         public ODEState resetState(final ODEStateAndDerivative s) {

  921.             final SpacecraftState oldState = convert(s);
  922.             final SpacecraftState newState = detector.resetState(oldState);
  923.             stateChanged(newState);

  925.             // main part
  926.             final double[] primary    = new double[s.getPrimaryStateDimension()];
  927.             stateMapper.mapStateToArray(newState, primary, null);

  929.             // secondary part
  930.             final double[][] secondary = new double[1][secondaryOffsets.get(SECONDARY_DIMENSION)];
  931.             for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
  932.                 final String name      = provider.getName();
  933.                 final int    offset    = secondaryOffsets.get(name);
  934.                 final int    dimension = provider.getDimension();
  935.                 System.arraycopy(newState.getAdditionalState(name), 0, secondary[0], offset, dimension);
  936.             }

  938.             return new ODEState(newState.getDate().durationFrom(getStartDate()),
  939.                                 primary, secondary);

  941.         }

  943.     }

  945.     /** Adapt an {@link org.orekit.propagation.sampling.OrekitStepHandler}
  946.      * to Hipparchus {@link ODEStepHandler} interface.
  947.      * @author Luc Maisonobe
  948.      */
  949.     private class AdaptedStepHandler implements ODEStepHandler {

  951.         /** Underlying handler. */
  952.         private final OrekitStepHandler handler;

  954.         /** Build an instance.
  955.          * @param handler underlying handler to wrap
  956.          */
  957.         AdaptedStepHandler(final OrekitStepHandler handler) {
  958.             this.handler = handler;
  959.         }

  961.         /** {@inheritDoc} */
  962.         public void init(final ODEStateAndDerivative s0, final double t) {
  963.             handler.init(convert(s0), stateMapper.mapDoubleToDate(t));
  964.         }

  966.         /** {@inheritDoc} */
  967.         @Override
  968.         public void handleStep(final ODEStateInterpolator interpolator) {
  969.             handler.handleStep(new AdaptedStepInterpolator(interpolator));
  970.         }

  972.         /** {@inheritDoc} */
  973.         @Override
  974.         public void finish(final ODEStateAndDerivative finalState) {
  975.             handler.finish(convert(finalState));
  976.         }

  978.     }

  980.     /** Adapt an Hipparchus {@link ODEStateInterpolator}
  981.      * to an orekit {@link OrekitStepInterpolator} interface.
  982.      * @author Luc Maisonobe
  983.      */
  984.     private class AdaptedStepInterpolator implements OrekitStepInterpolator {

  986.         /** Underlying raw rawInterpolator. */
  987.         private final ODEStateInterpolator mathInterpolator;

  989.         /** Simple constructor.
  990.          * @param mathInterpolator underlying raw interpolator
  991.          */
  992.         AdaptedStepInterpolator(final ODEStateInterpolator mathInterpolator) {
  993.             this.mathInterpolator = mathInterpolator;
  994.         }

  996.         /** {@inheritDoc}} */
  997.         @Override
  998.         public SpacecraftState getPreviousState() {
  999.             return convert(mathInterpolator.getPreviousState());
  1000.         }

  1002.         /** {@inheritDoc}} */
  1003.         @Override
  1004.         public boolean isPreviousStateInterpolated() {
  1005.             return mathInterpolator.isPreviousStateInterpolated();
  1006.         }

  1008.         /** {@inheritDoc}} */
  1009.         @Override
  1010.         public SpacecraftState getCurrentState() {
  1011.             return convert(mathInterpolator.getCurrentState());
  1012.         }

  1014.         /** {@inheritDoc}} */
  1015.         @Override
  1016.         public boolean isCurrentStateInterpolated() {
  1017.             return mathInterpolator.isCurrentStateInterpolated();
  1018.         }

  1020.         /** {@inheritDoc}} */
  1021.         @Override
  1022.         public SpacecraftState getInterpolatedState(final AbsoluteDate date) {
  1023.             return convert(mathInterpolator.getInterpolatedState(date.durationFrom(stateMapper.getReferenceDate())));
  1024.         }

  1026.         /** {@inheritDoc}} */
  1027.         @Override
  1028.         public boolean isForward() {
  1029.             return mathInterpolator.isForward();
  1030.         }

  1032.         /** {@inheritDoc}} */
  1033.         @Override
  1034.         public AdaptedStepInterpolator restrictStep(final SpacecraftState newPreviousState,
  1035.                                                     final SpacecraftState newCurrentState) {
  1036.             try {
  1037.                 final AbstractODEStateInterpolator aosi = (AbstractODEStateInterpolator) mathInterpolator;
  1038.                 return new AdaptedStepInterpolator(aosi.restrictStep(convert(newPreviousState),
  1039.                                                                      convert(newCurrentState)));
  1040.             } catch (ClassCastException cce) {
  1041.                 // this should never happen
  1042.                 throw new OrekitInternalError(cce);
  1043.             }
  1044.         }

  1046.     }

  1048.     /** Specialized step handler storing interpolators for ephemeris generation.
  1049.      * @since 11.0
  1050.      */
  1051.     private class StoringStepHandler implements ODEStepHandler, EphemerisGenerator {

  1053.         /** Underlying raw mathematical model. */
  1054.         private DenseOutputModel model;

  1056.         /** the user supplied end date. Propagation may not end on this date. */
  1057.         private AbsoluteDate endDate;

  1059.         /** Generated ephemeris. */
  1060.         private BoundedPropagator ephemeris;

  1062.         /** Last interpolator handled by the object.*/
  1063.         private  ODEStateInterpolator lastInterpolator;

  1065.         /** Set the end date.
  1066.          * @param endDate end date
  1067.          */
  1068.         public void setEndDate(final AbsoluteDate endDate) {
  1069.             this.endDate = endDate;
  1070.         }

  1072.         /** {@inheritDoc} */
  1073.         @Override
  1074.         public void init(final ODEStateAndDerivative s0, final double t) {

  1076.             this.model = new DenseOutputModel();
  1077.             model.init(s0, t);

  1079.             // ephemeris will be generated when last step is processed
  1080.             this.ephemeris = null;

  1082.             this.lastInterpolator = null;

  1084.         }

  1086.         /** {@inheritDoc} */
  1087.         @Override
  1088.         public BoundedPropagator getGeneratedEphemeris() {
  1089.             // Each time we try to get the ephemeris, rebuild it using the last data.
  1090.             buildEphemeris();
  1091.             return ephemeris;
  1092.         }

  1094.         /** {@inheritDoc} */
  1095.         @Override
  1096.         public void handleStep(final ODEStateInterpolator interpolator) {
  1097.             model.handleStep(interpolator);
  1098.             lastInterpolator = interpolator;
  1099.         }

  1101.         /** {@inheritDoc} */
  1102.         @Override
  1103.         public void finish(final ODEStateAndDerivative finalState) {
  1104.             buildEphemeris();
  1105.         }

  1107.         /** Method used to produce ephemeris at a given time.
  1108.          * Can be used at multiple times, updating the ephemeris to
  1109.          * its last state.
  1110.          */
  1111.         private void buildEphemeris() {
  1112.             // buildEphemeris was built in order to allow access to what was previously the finish method.
  1113.             // This now allows to call it through getGeneratedEphemeris, therefore through an external call,
  1114.             // which was not previously the case.

  1116.             // Update the model's finalTime with the last interpolator.
  1117.             model.finish(lastInterpolator.getCurrentState());

  1119.             // set up the boundary dates
  1120.             final double tI = model.getInitialTime();
  1121.             final double tF = model.getFinalTime();
  1122.             // tI is almost? always zero
  1123.             final AbsoluteDate startDate =
  1124.                             stateMapper.mapDoubleToDate(tI);
  1125.             final AbsoluteDate finalDate =
  1126.                             stateMapper.mapDoubleToDate(tF, this.endDate);
  1127.             final AbsoluteDate minDate;
  1128.             final AbsoluteDate maxDate;
  1129.             if (tF < tI) {
  1130.                 minDate = finalDate;
  1131.                 maxDate = startDate;
  1132.             } else {
  1133.                 minDate = startDate;
  1134.                 maxDate = finalDate;
  1135.             }

  1137.             // get the initial additional states that are not managed
  1138.             final DoubleArrayDictionary unmanaged = new DoubleArrayDictionary();
  1139.             for (final DoubleArrayDictionary.Entry initial : getInitialState().getAdditionalStatesValues().getData()) {
  1140.                 if (!isAdditionalStateManaged(initial.getKey())) {
  1141.                     // this additional state was in the initial state, but is unknown to the propagator
  1142.                     // we simply copy its initial value as is
  1143.                     unmanaged.put(initial.getKey(), initial.getValue());
  1144.                 }
  1145.             }

  1147.             // get the names of additional states managed by differential equations
  1148.             final String[] names      = new String[additionalDerivativesProviders.size()];
  1149.             final int[]    dimensions = new int[additionalDerivativesProviders.size()];
  1150.             for (int i = 0; i < names.length; ++i) {
  1151.                 names[i] = additionalDerivativesProviders.get(i).getName();
  1152.                 dimensions[i] = additionalDerivativesProviders.get(i).getDimension();
  1153.             }

  1155.             // create the ephemeris
  1156.             ephemeris = new IntegratedEphemeris(startDate, minDate, maxDate,
  1157.                                                 stateMapper, propagationType, model,
  1158.                                                 unmanaged, getAdditionalStateProviders(),
  1159.                                                 names, dimensions);

  1161.         }

  1163.     }

  1165.     /** Wrapper for resetting an integrator handlers.
  1166.      * <p>
  1167.      * This class is intended to be used in a try-with-resource statement.
  1168.      * If propagator-specific event handlers and step handlers are added to
  1169.      * the integrator in the try block, they will be removed automatically
  1170.      * when leaving the block, so the integrator only keeps its own handlers
  1171.      * between calls to {@link AbstractIntegratedPropagator#propagate(AbsoluteDate, AbsoluteDate).
  1172.      * </p>
  1173.      * @since 11.0
  1174.      */
  1175.     private static class IntegratorResetter implements AutoCloseable {

  1177.         /** Wrapped integrator. */
  1178.         private final ODEIntegrator integrator;

  1180.         /** Initial event handlers list. */
  1181.         private final List<EventHandlerConfiguration> eventHandlersConfigurations;

  1183.         /** Initial step handlers list. */
  1184.         private final List<ODEStepHandler> stepHandlers;

  1186.         /** Simple constructor.
  1187.          * @param integrator wrapped integrator
  1188.          */
  1189.         IntegratorResetter(final ODEIntegrator integrator) {
  1190.             this.integrator                  = integrator;
  1191.             this.eventHandlersConfigurations = new ArrayList<>(integrator.getEventHandlersConfigurations());
  1192.             this.stepHandlers                = new ArrayList<>(integrator.getStepHandlers());
  1193.         }

  1195.         /** {@inheritDoc}
  1196.          * <p>
  1197.          * Reset event handlers and step handlers back to the initial list
  1198.          * </p>
  1199.          */
  1200.         @Override
  1201.         public void close() {

  1203.             // reset event handlers
  1204.             integrator.clearEventHandlers();
  1205.             eventHandlersConfigurations.forEach(c -> integrator.addEventHandler(c.getEventHandler(),
  1206.                                                                                 c.getMaxCheckInterval(),
  1207.                                                                                 c.getConvergence(),
  1208.                                                                                 c.getMaxIterationCount(),
  1209.                                                                                 c.getSolver()));

  1211.             // reset step handlers
  1212.             integrator.clearStepHandlers();
  1213.             stepHandlers.forEach(stepHandler -> integrator.addStepHandler(stepHandler));

  1215.         }

  1217.     }

  1219. }
Created with JaCoCo 0.8.8.202204050719