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.             if (!tStart.equals(getInitialState().getDate())) {
  402.                 // if propagation start date is not initial date,
  403.                 // propagate from initial to start date without event detection
  404.                 try (IntegratorResetter startResetter = new IntegratorResetter(integrator)) {
  405.                     integrateDynamics(tStart);
  406.                 }
  407.             }

  409.             // set up events added by user
  410.             setUpUserEventDetectors();

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

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

  424.             return finalState;

  426.         }

  428.     }

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

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

  444.             initializePropagation();

  446.             if (getInitialState().getDate().equals(tEnd)) {
  447.                 // don't extrapolate
  448.                 return getInitialState();
  449.             }

  451.             // space dynamics view
  452.             stateMapper = createMapper(getInitialState().getDate(), stateMapper.getMu(),
  453.                                        stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  454.                                        stateMapper.getAttitudeProvider(), getInitialState().getFrame());


  457.             if (Double.isNaN(getMu())) {
  458.                 setMu(getInitialState().getMu());
  459.             }

  461.             if (getInitialState().getMass() <= 0.0) {
  462.                 throw new OrekitException(OrekitMessages.SPACECRAFT_MASS_BECOMES_NEGATIVE,
  463.                                           getInitialState().getMass());
  464.             }

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

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

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

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

  500.             if (resetAtEnd) {
  501.                 resetInitialState(finalState);
  502.                 setStartDate(finalState.getDate());
  503.             }

  505.             return finalState;

  507.         } catch (MathRuntimeException mre) {
  508.             throw OrekitException.unwrap(mre);
  509.         }
  510.     }

  512.     /** Get the initial state for integration.
  513.      * @return initial state for integration
  514.      */
  515.     protected SpacecraftState getInitialIntegrationState() {
  516.         return getInitialState();
  517.     }

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

  525.         // retrieve initial state
  526.         final double[] primary = new double[getBasicDimension()];
  527.         stateMapper.mapStateToArray(initialState, primary, null);

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

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

  541.     }

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

  550.         if (secondaryOffsets.isEmpty()) {
  551.             return null;
  552.         }

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

  562.         return secondary;

  564.     }

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

  573.         if (secondaryOffsets.isEmpty()) {
  574.             return null;
  575.         }

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

  585.         return secondaryDerivative;

  587.     }

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

  597.         final ExpandableODE ode =
  598.                 new ExpandableODE(new ConvertedMainStateEquations(getMainStateEquations(integ)));

  600.         // secondary part of the ODE
  601.         if (!additionalDerivativesProviders.isEmpty()) {
  602.             ode.addSecondaryEquations(new ConvertedSecondaryStateEquations());
  603.         }

  605.         return ode;

  607.     }

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

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

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

  637.     /** Get the integrator used by the propagator.
  638.      * @return the integrator.
  639.      */
  640.     protected ODEIntegrator getIntegrator() {
  641.         return integrator;
  642.     }

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

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

  667.         return s;

  669.     }

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

  677.         // retrieve initial state
  678.         final double[] primary    = new double[getBasicDimension()];
  679.         final double[] primaryDot = new double[getBasicDimension()];
  680.         stateMapper.mapStateToArray(state, primary, primaryDot);

  682.         // secondary part of the ODE
  683.         final double[][] secondary           = secondary(state);
  684.         final double[][] secondaryDerivative = secondaryDerivative(state);

  686.         return new ODEStateAndDerivative(stateMapper.mapDateToDouble(state.getDate()),
  687.                                          primary, primaryDot,
  688.                                          secondary, secondaryDerivative);

  690.     }

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

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

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

  714.     }

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

  719.         /** Main state equations. */
  720.         private final MainStateEquations main;

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

  730.         /** {@inheritDoc} */
  731.         public int getDimension() {
  732.             return getBasicDimension();
  733.         }

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

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

  747.             // increment calls counter
  748.             ++calls;

  750.             // update space dynamics view
  751.             SpacecraftState currentState = stateMapper.mapArrayToState(t, y, null, PropagationType.MEAN);
  752.             currentState = updateAdditionalStates(currentState);

  754.             // compute main state differentials
  755.             return main.computeDerivatives(currentState);

  757.         }

  759.     }

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

  764.         /** Dimension of the combined additional states. */
  765.         private final int combinedDimension;

  767.         /** Simple constructor.
  768.           */
  769.         ConvertedSecondaryStateEquations() {
  770.             this.combinedDimension = secondaryOffsets.get(SECONDARY_DIMENSION);
  771.         }

  773.         /** {@inheritDoc} */
  774.         @Override
  775.         public int getDimension() {
  776.             return combinedDimension;
  777.         }

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

  786.             final AbsoluteDate target = stateMapper.mapDoubleToDate(finalTime);
  787.             for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
  788.                 provider.init(initialState, target);
  789.             }

  791.         }

  793.         /** {@inheritDoc} */
  794.         @Override
  795.         public double[] computeDerivatives(final double t, final double[] primary,
  796.                                            final double[] primaryDot, final double[] secondary) {

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

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

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

  833.             return secondaryDot;

  835.         }

  837.         /** Convert mathematical view to space view.
  838.          * @param t current value of the independent <I>time</I> variable
  839.          * @param primary array containing the current value of the primary state vector
  840.          * @param primaryDot array containing the derivative of the primary state vector
  841.          * @param secondary array containing the current value of the secondary state vector
  842.          * @return space view of the state
  843.          */
  844.         private SpacecraftState convert(final double t, final double[] primary,
  845.                                         final double[] primaryDot, final double[] secondary) {

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

  849.             for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
  850.                 final String name      = provider.getName();
  851.                 final int    offset    = secondaryOffsets.get(name);
  852.                 final int    dimension = provider.getDimension();
  853.                 initialState = initialState.addAdditionalState(name, Arrays.copyOfRange(secondary, offset, offset + dimension));
  854.             }

  856.             return updateAdditionalStates(initialState);

  858.         }

  860.     }

  862.     /** Adapt an {@link org.orekit.propagation.events.EventDetector}
  863.      * to Hipparchus {@link org.hipparchus.ode.events.ODEEventHandler} interface.
  864.      * @author Fabien Maussion
  865.      */
  866.     private class AdaptedEventDetector implements ODEEventHandler {

  868.         /** Underlying event detector. */
  869.         private final EventDetector detector;

  871.         /** Time of the previous call to g. */
  872.         private double lastT;

  874.         /** Value from the previous call to g. */
  875.         private double lastG;

  877.         /** Build a wrapped event detector.
  878.          * @param detector event detector to wrap
  879.         */
  880.         AdaptedEventDetector(final EventDetector detector) {
  881.             this.detector = detector;
  882.             this.lastT    = Double.NaN;
  883.             this.lastG    = Double.NaN;
  884.         }

  886.         /** {@inheritDoc} */
  887.         public void init(final ODEStateAndDerivative s0, final double t) {
  888.             detector.init(convert(s0), stateMapper.mapDoubleToDate(t));
  889.             this.lastT = Double.NaN;
  890.             this.lastG = Double.NaN;
  891.         }

  893.         /** {@inheritDoc} */
  894.         public double g(final ODEStateAndDerivative s) {
  895.             if (!Precision.equals(lastT, s.getTime(), 0)) {
  896.                 lastT = s.getTime();
  897.                 lastG = detector.g(convert(s));
  898.             }
  899.             return lastG;
  900.         }

  902.         /** {@inheritDoc} */
  903.         public Action eventOccurred(final ODEStateAndDerivative s, final boolean increasing) {
  904.             return detector.eventOccurred(convert(s), increasing);
  905.         }

  907.         /** {@inheritDoc} */
  908.         public ODEState resetState(final ODEStateAndDerivative s) {

  910.             final SpacecraftState oldState = convert(s);
  911.             final SpacecraftState newState = detector.resetState(oldState);
  912.             stateChanged(newState);

  914.             // main part
  915.             final double[] primary    = new double[s.getPrimaryStateDimension()];
  916.             stateMapper.mapStateToArray(newState, primary, null);

  918.             // secondary part
  919.             final double[][] secondary = new double[1][secondaryOffsets.get(SECONDARY_DIMENSION)];
  920.             for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
  921.                 final String name      = provider.getName();
  922.                 final int    offset    = secondaryOffsets.get(name);
  923.                 final int    dimension = provider.getDimension();
  924.                 System.arraycopy(newState.getAdditionalState(name), 0, secondary[0], offset, dimension);
  925.             }

  927.             return new ODEState(newState.getDate().durationFrom(getStartDate()),
  928.                                 primary, secondary);

  930.         }

  932.     }

  934.     /** Adapt an {@link org.orekit.propagation.sampling.OrekitStepHandler}
  935.      * to Hipparchus {@link ODEStepHandler} interface.
  936.      * @author Luc Maisonobe
  937.      */
  938.     private class AdaptedStepHandler implements ODEStepHandler {

  940.         /** Underlying handler. */
  941.         private final OrekitStepHandler handler;

  943.         /** Build an instance.
  944.          * @param handler underlying handler to wrap
  945.          */
  946.         AdaptedStepHandler(final OrekitStepHandler handler) {
  947.             this.handler = handler;
  948.         }

  950.         /** {@inheritDoc} */
  951.         public void init(final ODEStateAndDerivative s0, final double t) {
  952.             handler.init(convert(s0), stateMapper.mapDoubleToDate(t));
  953.         }

  955.         /** {@inheritDoc} */
  956.         @Override
  957.         public void handleStep(final ODEStateInterpolator interpolator) {
  958.             handler.handleStep(new AdaptedStepInterpolator(interpolator));
  959.         }

  961.         /** {@inheritDoc} */
  962.         @Override
  963.         public void finish(final ODEStateAndDerivative finalState) {
  964.             handler.finish(convert(finalState));
  965.         }

  967.     }

  969.     /** Adapt an Hipparchus {@link ODEStateInterpolator}
  970.      * to an orekit {@link OrekitStepInterpolator} interface.
  971.      * @author Luc Maisonobe
  972.      */
  973.     private class AdaptedStepInterpolator implements OrekitStepInterpolator {

  975.         /** Underlying raw rawInterpolator. */
  976.         private final ODEStateInterpolator mathInterpolator;

  978.         /** Simple constructor.
  979.          * @param mathInterpolator underlying raw interpolator
  980.          */
  981.         AdaptedStepInterpolator(final ODEStateInterpolator mathInterpolator) {
  982.             this.mathInterpolator = mathInterpolator;
  983.         }

  985.         /** {@inheritDoc}} */
  986.         @Override
  987.         public SpacecraftState getPreviousState() {
  988.             return convert(mathInterpolator.getPreviousState());
  989.         }

  991.         /** {@inheritDoc}} */
  992.         @Override
  993.         public boolean isPreviousStateInterpolated() {
  994.             return mathInterpolator.isPreviousStateInterpolated();
  995.         }

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

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

  1009.         /** {@inheritDoc}} */
  1010.         @Override
  1011.         public SpacecraftState getInterpolatedState(final AbsoluteDate date) {
  1012.             return convert(mathInterpolator.getInterpolatedState(date.durationFrom(stateMapper.getReferenceDate())));
  1013.         }

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

  1021.         /** {@inheritDoc}} */
  1022.         @Override
  1023.         public AdaptedStepInterpolator restrictStep(final SpacecraftState newPreviousState,
  1024.                                                     final SpacecraftState newCurrentState) {
  1025.             try {
  1026.                 final AbstractODEStateInterpolator aosi = (AbstractODEStateInterpolator) mathInterpolator;
  1027.                 return new AdaptedStepInterpolator(aosi.restrictStep(convert(newPreviousState),
  1028.                                                                      convert(newCurrentState)));
  1029.             } catch (ClassCastException cce) {
  1030.                 // this should never happen
  1031.                 throw new OrekitInternalError(cce);
  1032.             }
  1033.         }

  1035.     }

  1037.     /** Specialized step handler storing interpolators for ephemeris generation.
  1038.      * @since 11.0
  1039.      */
  1040.     private class StoringStepHandler implements ODEStepHandler, EphemerisGenerator {

  1042.         /** Underlying raw mathematical model. */
  1043.         private DenseOutputModel model;

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

  1048.         /** Generated ephemeris. */
  1049.         private BoundedPropagator ephemeris;

  1051.         /** Set the end date.
  1052.          * @param endDate end date
  1053.          */
  1054.         public void setEndDate(final AbsoluteDate endDate) {
  1055.             this.endDate = endDate;
  1056.         }

  1058.         /** {@inheritDoc} */
  1059.         @Override
  1060.         public void init(final ODEStateAndDerivative s0, final double t) {

  1062.             this.model = new DenseOutputModel();
  1063.             model.init(s0, t);

  1065.             // ephemeris will be generated when last step is processed
  1066.             this.ephemeris = null;

  1068.         }

  1070.         /** {@inheritDoc} */
  1071.         @Override
  1072.         public BoundedPropagator getGeneratedEphemeris() {
  1073.             return ephemeris;
  1074.         }

  1076.         /** {@inheritDoc} */
  1077.         @Override
  1078.         public void handleStep(final ODEStateInterpolator interpolator) {
  1079.             model.handleStep(interpolator);
  1080.         }

  1082.         /** {@inheritDoc} */
  1083.         @Override
  1084.         public void finish(final ODEStateAndDerivative finalState) {

  1086.             // set up the boundary dates
  1087.             final double tI = model.getInitialTime();
  1088.             final double tF = model.getFinalTime();
  1089.             // tI is almost? always zero
  1090.             final AbsoluteDate startDate =
  1091.                             stateMapper.mapDoubleToDate(tI);
  1092.             final AbsoluteDate finalDate =
  1093.                             stateMapper.mapDoubleToDate(tF, this.endDate);
  1094.             final AbsoluteDate minDate;
  1095.             final AbsoluteDate maxDate;
  1096.             if (tF < tI) {
  1097.                 minDate = finalDate;
  1098.                 maxDate = startDate;
  1099.             } else {
  1100.                 minDate = startDate;
  1101.                 maxDate = finalDate;
  1102.             }

  1104.             // get the initial additional states that are not managed
  1105.             final DoubleArrayDictionary unmanaged = new DoubleArrayDictionary();
  1106.             for (final DoubleArrayDictionary.Entry initial : getInitialState().getAdditionalStatesValues().getData()) {
  1107.                 if (!isAdditionalStateManaged(initial.getKey())) {
  1108.                     // this additional state was in the initial state, but is unknown to the propagator
  1109.                     // we simply copy its initial value as is
  1110.                     unmanaged.put(initial.getKey(), initial.getValue());
  1111.                 }
  1112.             }

  1114.             // get the names of additional states managed by differential equations
  1115.             final String[] names = new String[additionalDerivativesProviders.size()];
  1116.             for (int i = 0; i < names.length; ++i) {
  1117.                 names[i] = additionalDerivativesProviders.get(i).getName();
  1118.             }

  1120.             // create the ephemeris
  1121.             ephemeris = new IntegratedEphemeris(startDate, minDate, maxDate,
  1122.                                                 stateMapper, propagationType, model,
  1123.                                                 unmanaged, getAdditionalStateProviders(), names);

  1125.         }

  1127.     }

  1129.     /** Wrapper for resetting an integrator handlers.
  1130.      * <p>
  1131.      * This class is intended to be used in a try-with-resource statement.
  1132.      * If propagator-specific event handlers and step handlers are added to
  1133.      * the integrator in the try block, they will be removed automatically
  1134.      * when leaving the block, so the integrator only keeps its own handlers
  1135.      * between calls to {@link AbstractIntegratedPropagator#propagate(AbsoluteDate, AbsoluteDate).
  1136.      * </p>
  1137.      * @since 11.0
  1138.      */
  1139.     private static class IntegratorResetter implements AutoCloseable {

  1141.         /** Wrapped integrator. */
  1142.         private final ODEIntegrator integrator;

  1144.         /** Initial event handlers list. */
  1145.         private final List<EventHandlerConfiguration> eventHandlersConfigurations;

  1147.         /** Initial step handlers list. */
  1148.         private final List<ODEStepHandler> stepHandlers;

  1150.         /** Simple constructor.
  1151.          * @param integrator wrapped integrator
  1152.          */
  1153.         IntegratorResetter(final ODEIntegrator integrator) {
  1154.             this.integrator                  = integrator;
  1155.             this.eventHandlersConfigurations = new ArrayList<>(integrator.getEventHandlersConfigurations());
  1156.             this.stepHandlers                = new ArrayList<>(integrator.getStepHandlers());
  1157.         }

  1159.         /** {@inheritDoc}
  1160.          * <p>
  1161.          * Reset event handlers and step handlers back to the initial list
  1162.          * </p>
  1163.          */
  1164.         @Override
  1165.         public void close() {

  1167.             // reset event handlers
  1168.             integrator.clearEventHandlers();
  1169.             eventHandlersConfigurations.forEach(c -> integrator.addEventHandler(c.getEventHandler(),
  1170.                                                                                 c.getMaxCheckInterval(),
  1171.                                                                                 c.getConvergence(),
  1172.                                                                                 c.getMaxIterationCount(),
  1173.                                                                                 c.getSolver()));

  1175.             // reset step handlers
  1176.             integrator.clearStepHandlers();
  1177.             stepHandlers.forEach(stepHandler -> integrator.addStepHandler(stepHandler));

  1179.         }

  1181.     }

  1183. }
Created with JaCoCo 0.8.7.202105040129