AbstractIntegratedPropagator.java

  1. /* Copyright 2002-2024 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.analysis.UnivariateFunction;
  30. import org.hipparchus.analysis.solvers.BracketedUnivariateSolver;
  31. import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
  32. import org.hipparchus.exception.MathRuntimeException;
  33. import org.hipparchus.ode.DenseOutputModel;
  34. import org.hipparchus.ode.ExpandableODE;
  35. import org.hipparchus.ode.ODEIntegrator;
  36. import org.hipparchus.ode.ODEState;
  37. import org.hipparchus.ode.ODEStateAndDerivative;
  38. import org.hipparchus.ode.OrdinaryDifferentialEquation;
  39. import org.hipparchus.ode.SecondaryODE;
  40. import org.hipparchus.ode.events.Action;
  41. import org.hipparchus.ode.events.AdaptableInterval;
  42. import org.hipparchus.ode.events.ODEEventDetector;
  43. import org.hipparchus.ode.events.ODEEventHandler;
  44. import org.hipparchus.ode.sampling.AbstractODEStateInterpolator;
  45. import org.hipparchus.ode.sampling.ODEStateInterpolator;
  46. import org.hipparchus.ode.sampling.ODEStepHandler;
  47. import org.hipparchus.util.Precision;
  48. import org.orekit.attitudes.AttitudeProvider;
  49. import org.orekit.errors.OrekitException;
  50. import org.orekit.errors.OrekitInternalError;
  51. import org.orekit.errors.OrekitMessages;
  52. import org.orekit.frames.Frame;
  53. import org.orekit.orbits.OrbitType;
  54. import org.orekit.orbits.PositionAngleType;
  55. import org.orekit.propagation.AbstractPropagator;
  56. import org.orekit.propagation.BoundedPropagator;
  57. import org.orekit.propagation.EphemerisGenerator;
  58. import org.orekit.propagation.PropagationType;
  59. import org.orekit.propagation.SpacecraftState;
  60. import org.orekit.propagation.events.EventDetector;
  61. import org.orekit.propagation.events.handlers.EventHandler;
  62. import org.orekit.propagation.sampling.OrekitStepHandler;
  63. import org.orekit.propagation.sampling.OrekitStepInterpolator;
  64. import org.orekit.time.AbsoluteDate;
  65. import org.orekit.utils.DoubleArrayDictionary;


  68. /** Common handling of {@link org.orekit.propagation.Propagator Propagator}
  69.  *  methods for both numerical and semi-analytical propagators.
  70.  *  @author Luc Maisonobe
  71.  */
  72. public abstract class AbstractIntegratedPropagator extends AbstractPropagator {

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

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

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

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

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

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

  98.     /** Map of secondary equation offset in main
  99.     /** Counter for differential equations calls. */
  100.     private int calls;

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

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

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

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

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

  146.     /** Getter for the resetting flag regarding initial state.
  147.      * @return resetting flag
  148.      * @since 12.0
  149.      */
  150.     public boolean getResetAtEnd() {
  151.         return this.resetAtEnd;
  152.     }

  154.     /** Initialize the mapper. */
  155.     protected void initMapper() {
  156.         stateMapper = createMapper(null, Double.NaN, null, null, null, null);
  157.     }

  159.     /** Get the integrator's name.
  160.      * @return name of underlying integrator
  161.      * @since 12.0
  162.      */
  163.     public String getIntegratorName() {
  164.         return integrator.getName();
  165.     }

  167.     /**  {@inheritDoc} */
  168.     public void setAttitudeProvider(final AttitudeProvider attitudeProvider) {
  169.         super.setAttitudeProvider(attitudeProvider);
  170.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  171.                                    stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  172.                                    attitudeProvider, stateMapper.getFrame());
  173.     }

  175.     /** Set propagation orbit type.
  176.      * @param orbitType orbit type to use for propagation, null for
  177.      * propagating using {@link org.orekit.utils.AbsolutePVCoordinates AbsolutePVCoordinates}
  178.      * rather than {@link org.orekit.orbits Orbit}
  179.      */
  180.     protected void setOrbitType(final OrbitType orbitType) {
  181.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  182.                                    orbitType, stateMapper.getPositionAngleType(),
  183.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  184.     }

  186.     /** Get propagation parameter type.
  187.      * @return orbit type used for propagation, null for
  188.      * propagating using {@link org.orekit.utils.AbsolutePVCoordinates AbsolutePVCoordinates}
  189.      * rather than {@link org.orekit.orbits Orbit}
  190.      */
  191.     protected OrbitType getOrbitType() {
  192.         return stateMapper.getOrbitType();
  193.     }

  195.     /** Get the propagation type.
  196.      * @return propagation type.
  197.      * @since 11.1
  198.      */
  199.     public PropagationType getPropagationType() {
  200.         return propagationType;
  201.     }

  203.     /** Set position angle type.
  204.      * <p>
  205.      * The position parameter type is meaningful only if {@link
  206.      * #getOrbitType() propagation orbit type}
  207.      * support it. As an example, it is not meaningful for propagation
  208.      * in {@link OrbitType#CARTESIAN Cartesian} parameters.
  209.      * </p>
  210.      * @param positionAngleType angle type to use for propagation
  211.      */
  212.     protected void setPositionAngleType(final PositionAngleType positionAngleType) {
  213.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  214.                                    stateMapper.getOrbitType(), positionAngleType,
  215.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  216.     }

  218.     /** Get propagation parameter type.
  219.      * @return angle type to use for propagation
  220.      */
  221.     protected PositionAngleType getPositionAngleType() {
  222.         return stateMapper.getPositionAngleType();
  223.     }

  225.     /** Set the central attraction coefficient μ.
  226.      * @param mu central attraction coefficient (m³/s²)
  227.      */
  228.     public void setMu(final double mu) {
  229.         stateMapper = createMapper(stateMapper.getReferenceDate(), mu,
  230.                                    stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  231.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  232.     }

  234.     /** Get the central attraction coefficient μ.
  235.      * @return mu central attraction coefficient (m³/s²)
  236.      * @see #setMu(double)
  237.      */
  238.     public double getMu() {
  239.         return stateMapper.getMu();
  240.     }

  242.     /** Get the number of calls to the differential equations computation method.
  243.      * <p>The number of calls is reset each time the {@link #propagate(AbsoluteDate)}
  244.      * method is called.</p>
  245.      * @return number of calls to the differential equations computation method
  246.      */
  247.     public int getCalls() {
  248.         return calls;
  249.     }

  251.     /** {@inheritDoc} */
  252.     @Override
  253.     public boolean isAdditionalStateManaged(final String name) {

  255.         // first look at already integrated states
  256.         if (super.isAdditionalStateManaged(name)) {
  257.             return true;
  258.         }

  260.         // then look at states we integrate ourselves
  261.         for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
  262.             if (provider.getName().equals(name)) {
  263.                 return true;
  264.             }
  265.         }

  267.         return false;
  268.     }

  270.     /** {@inheritDoc} */
  271.     @Override
  272.     public String[] getManagedAdditionalStates() {
  273.         final String[] alreadyIntegrated = super.getManagedAdditionalStates();
  274.         final String[] managed = new String[alreadyIntegrated.length + additionalDerivativesProviders.size()];
  275.         System.arraycopy(alreadyIntegrated, 0, managed, 0, alreadyIntegrated.length);
  276.         for (int i = 0; i < additionalDerivativesProviders.size(); ++i) {
  277.             managed[i + alreadyIntegrated.length] = additionalDerivativesProviders.get(i).getName();
  278.         }
  279.         return managed;
  280.     }

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

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

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

  299.         secondaryOffsets.clear();

  301.     }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  426.             return finalState;

  428.         }

  430.     }

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

  439.     /** Propagation with or without event detection.
  440.      * @param tEnd target date to which orbit should be propagated
  441.      * @param forceResetAtEnd flag to force resetting state and date after integration
  442.      * @return state at end of propagation
  443.      */
  444.     private SpacecraftState integrateDynamics(final AbsoluteDate tEnd, final boolean forceResetAtEnd) {
  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.NOT_POSITIVE_SPACECRAFT_MASS,
  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);

  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 || forceResetAtEnd) {
  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.      * @return a new ode
  595.      */
  596.     private ExpandableODE createODE(final ODEIntegrator integ) {

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

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

  606.         return ode;

  608.     }

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

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

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

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

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

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

  668.         return s;

  670.     }

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

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

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

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

  691.     }

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

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

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

  715.     }

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

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

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

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

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

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

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

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

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

  758.         }

  760.     }

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

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

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

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

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

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

  792.         }

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

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

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

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

  842.             return secondaryDot;

  844.         }

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

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

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

  865.             return updateAdditionalStates(initialState);

  867.         }

  869.     }

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

  877.         /** Underlying event detector. */
  878.         private final EventDetector detector;

  880.         /** Underlying event handler.
  881.          * @since 12.0
  882.          */
  883.         private final EventHandler handler;

  885.         /** Time of the previous call to g. */
  886.         private double lastT;

  888.         /** Value from the previous call to g. */
  889.         private double lastG;

  891.         /** Build a wrapped event detector.
  892.          * @param detector event detector to wrap
  893.         */
  894.         AdaptedEventDetector(final EventDetector detector) {
  895.             this.detector = detector;
  896.             this.handler  = detector.getHandler();
  897.             this.lastT    = Double.NaN;
  898.             this.lastG    = Double.NaN;
  899.         }

  901.         /** {@inheritDoc} */
  902.         @Override
  903.         public AdaptableInterval getMaxCheckInterval() {
  904.             return s -> detector.getMaxCheckInterval().currentInterval(convert(s));
  905.         }

  907.         /** {@inheritDoc} */
  908.         @Override
  909.         public int getMaxIterationCount() {
  910.             return detector.getMaxIterationCount();
  911.         }

  913.         /** {@inheritDoc} */
  914.         @Override
  915.         public BracketedUnivariateSolver<UnivariateFunction> getSolver() {
  916.             return new BracketingNthOrderBrentSolver(0, detector.getThreshold(), 0, 5);
  917.         }

  919.         /** {@inheritDoc} */
  920.         public void init(final ODEStateAndDerivative s0, final double t) {
  921.             detector.init(convert(s0), stateMapper.mapDoubleToDate(t));
  922.             this.lastT = Double.NaN;
  923.             this.lastG = Double.NaN;
  924.         }

  926.         /** {@inheritDoc} */
  927.         public double g(final ODEStateAndDerivative s) {
  928.             if (!Precision.equals(lastT, s.getTime(), 0)) {
  929.                 lastT = s.getTime();
  930.                 lastG = detector.g(convert(s));
  931.             }
  932.             return lastG;
  933.         }

  935.         /** {@inheritDoc} */
  936.         public ODEEventHandler getHandler() {

  938.             return new ODEEventHandler() {

  940.                 /** {@inheritDoc} */
  941.                 public Action eventOccurred(final ODEStateAndDerivative s, final ODEEventDetector d, final boolean increasing) {
  942.                     return handler.eventOccurred(convert(s), detector, increasing);
  943.                 }

  945.                 /** {@inheritDoc} */
  946.                 public ODEState resetState(final ODEEventDetector d, final ODEStateAndDerivative s) {

  948.                     final SpacecraftState oldState = convert(s);
  949.                     final SpacecraftState newState = handler.resetState(detector, oldState);
  950.                     stateChanged(newState);

  952.                     // main part
  953.                     final double[] primary    = new double[s.getPrimaryStateDimension()];
  954.                     stateMapper.mapStateToArray(newState, primary, null);

  956.                     // secondary part
  957.                     final double[][] secondary = new double[1][secondaryOffsets.get(SECONDARY_DIMENSION)];
  958.                     for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
  959.                         final String name      = provider.getName();
  960.                         final int    offset    = secondaryOffsets.get(name);
  961.                         final int    dimension = provider.getDimension();
  962.                         System.arraycopy(newState.getAdditionalState(name), 0, secondary[0], offset, dimension);
  963.                     }

  965.                     return new ODEState(newState.getDate().durationFrom(getStartDate()),
  966.                                         primary, secondary);

  968.                 }

  970.             };
  971.         }

  973.     }

  975.     /** Adapt an {@link org.orekit.propagation.sampling.OrekitStepHandler}
  976.      * to Hipparchus {@link ODEStepHandler} interface.
  977.      * @author Luc Maisonobe
  978.      */
  979.     private class AdaptedStepHandler implements ODEStepHandler {

  981.         /** Underlying handler. */
  982.         private final OrekitStepHandler handler;

  984.         /** Build an instance.
  985.          * @param handler underlying handler to wrap
  986.          */
  987.         AdaptedStepHandler(final OrekitStepHandler handler) {
  988.             this.handler = handler;
  989.         }

  991.         /** {@inheritDoc} */
  992.         public void init(final ODEStateAndDerivative s0, final double t) {
  993.             handler.init(convert(s0), stateMapper.mapDoubleToDate(t));
  994.         }

  996.         /** {@inheritDoc} */
  997.         @Override
  998.         public void handleStep(final ODEStateInterpolator interpolator) {
  999.             handler.handleStep(new AdaptedStepInterpolator(interpolator));
  1000.         }

  1002.         /** {@inheritDoc} */
  1003.         @Override
  1004.         public void finish(final ODEStateAndDerivative finalState) {
  1005.             handler.finish(convert(finalState));
  1006.         }

  1008.     }

  1010.     /** Adapt an Hipparchus {@link ODEStateInterpolator}
  1011.      * to an orekit {@link OrekitStepInterpolator} interface.
  1012.      * @author Luc Maisonobe
  1013.      */
  1014.     private class AdaptedStepInterpolator implements OrekitStepInterpolator {

  1016.         /** Underlying raw rawInterpolator. */
  1017.         private final ODEStateInterpolator mathInterpolator;

  1019.         /** Simple constructor.
  1020.          * @param mathInterpolator underlying raw interpolator
  1021.          */
  1022.         AdaptedStepInterpolator(final ODEStateInterpolator mathInterpolator) {
  1023.             this.mathInterpolator = mathInterpolator;
  1024.         }

  1026.         /** {@inheritDoc}} */
  1027.         @Override
  1028.         public SpacecraftState getPreviousState() {
  1029.             return convert(mathInterpolator.getPreviousState());
  1030.         }

  1032.         /** {@inheritDoc}} */
  1033.         @Override
  1034.         public boolean isPreviousStateInterpolated() {
  1035.             return mathInterpolator.isPreviousStateInterpolated();
  1036.         }

  1038.         /** {@inheritDoc}} */
  1039.         @Override
  1040.         public SpacecraftState getCurrentState() {
  1041.             return convert(mathInterpolator.getCurrentState());
  1042.         }

  1044.         /** {@inheritDoc}} */
  1045.         @Override
  1046.         public boolean isCurrentStateInterpolated() {
  1047.             return mathInterpolator.isCurrentStateInterpolated();
  1048.         }

  1050.         /** {@inheritDoc}} */
  1051.         @Override
  1052.         public SpacecraftState getInterpolatedState(final AbsoluteDate date) {
  1053.             return convert(mathInterpolator.getInterpolatedState(date.durationFrom(stateMapper.getReferenceDate())));
  1054.         }

  1056.         /** {@inheritDoc}} */
  1057.         @Override
  1058.         public boolean isForward() {
  1059.             return mathInterpolator.isForward();
  1060.         }

  1062.         /** {@inheritDoc}} */
  1063.         @Override
  1064.         public AdaptedStepInterpolator restrictStep(final SpacecraftState newPreviousState,
  1065.                                                     final SpacecraftState newCurrentState) {
  1066.             try {
  1067.                 final AbstractODEStateInterpolator aosi = (AbstractODEStateInterpolator) mathInterpolator;
  1068.                 return new AdaptedStepInterpolator(aosi.restrictStep(convert(newPreviousState),
  1069.                                                                      convert(newCurrentState)));
  1070.             } catch (ClassCastException cce) {
  1071.                 // this should never happen
  1072.                 throw new OrekitInternalError(cce);
  1073.             }
  1074.         }

  1076.     }

  1078.     /** Specialized step handler storing interpolators for ephemeris generation.
  1079.      * @since 11.0
  1080.      */
  1081.     private class StoringStepHandler implements ODEStepHandler, EphemerisGenerator {

  1083.         /** Underlying raw mathematical model. */
  1084.         private DenseOutputModel model;

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

  1089.         /** Generated ephemeris. */
  1090.         private BoundedPropagator ephemeris;

  1092.         /** Last interpolator handled by the object.*/
  1093.         private  ODEStateInterpolator lastInterpolator;

  1095.         /** Set the end date.
  1096.          * @param endDate end date
  1097.          */
  1098.         public void setEndDate(final AbsoluteDate endDate) {
  1099.             this.endDate = endDate;
  1100.         }

  1102.         /** {@inheritDoc} */
  1103.         @Override
  1104.         public void init(final ODEStateAndDerivative s0, final double t) {

  1106.             this.model = new DenseOutputModel();
  1107.             model.init(s0, t);

  1109.             // ephemeris will be generated when last step is processed
  1110.             this.ephemeris = null;

  1112.             this.lastInterpolator = null;

  1114.         }

  1116.         /** {@inheritDoc} */
  1117.         @Override
  1118.         public BoundedPropagator getGeneratedEphemeris() {
  1119.             // Each time we try to get the ephemeris, rebuild it using the last data.
  1120.             buildEphemeris();
  1121.             return ephemeris;
  1122.         }

  1124.         /** {@inheritDoc} */
  1125.         @Override
  1126.         public void handleStep(final ODEStateInterpolator interpolator) {
  1127.             model.handleStep(interpolator);
  1128.             lastInterpolator = interpolator;
  1129.         }

  1131.         /** {@inheritDoc} */
  1132.         @Override
  1133.         public void finish(final ODEStateAndDerivative finalState) {
  1134.             buildEphemeris();
  1135.         }

  1137.         /** Method used to produce ephemeris at a given time.
  1138.          * Can be used at multiple times, updating the ephemeris to
  1139.          * its last state.
  1140.          */
  1141.         private void buildEphemeris() {
  1142.             // buildEphemeris was built in order to allow access to what was previously the finish method.
  1143.             // This now allows to call it through getGeneratedEphemeris, therefore through an external call,
  1144.             // which was not previously the case.

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

  1149.             // set up the boundary dates
  1150.             final double tI = model.getInitialTime();
  1151.             final double tF = model.getFinalTime();
  1152.             // tI is almost? always zero
  1153.             final AbsoluteDate startDate =
  1154.                             stateMapper.mapDoubleToDate(tI);
  1155.             final AbsoluteDate finalDate =
  1156.                             stateMapper.mapDoubleToDate(tF, this.endDate);
  1157.             final AbsoluteDate minDate;
  1158.             final AbsoluteDate maxDate;
  1159.             if (tF < tI) {
  1160.                 minDate = finalDate;
  1161.                 maxDate = startDate;
  1162.             } else {
  1163.                 minDate = startDate;
  1164.                 maxDate = finalDate;
  1165.             }

  1167.             // get the initial additional states that are not managed
  1168.             final DoubleArrayDictionary unmanaged = new DoubleArrayDictionary();
  1169.             for (final DoubleArrayDictionary.Entry initial : getInitialState().getAdditionalStatesValues().getData()) {
  1170.                 if (!isAdditionalStateManaged(initial.getKey())) {
  1171.                     // this additional state was in the initial state, but is unknown to the propagator
  1172.                     // we simply copy its initial value as is
  1173.                     unmanaged.put(initial.getKey(), initial.getValue());
  1174.                 }
  1175.             }

  1177.             // get the names of additional states managed by differential equations
  1178.             final String[] names      = new String[additionalDerivativesProviders.size()];
  1179.             final int[]    dimensions = new int[additionalDerivativesProviders.size()];
  1180.             for (int i = 0; i < names.length; ++i) {
  1181.                 names[i] = additionalDerivativesProviders.get(i).getName();
  1182.                 dimensions[i] = additionalDerivativesProviders.get(i).getDimension();
  1183.             }

  1185.             // create the ephemeris
  1186.             ephemeris = new IntegratedEphemeris(startDate, minDate, maxDate,
  1187.                                                 stateMapper, propagationType, model,
  1188.                                                 unmanaged, getAdditionalStateProviders(),
  1189.                                                 names, dimensions);

  1191.         }

  1193.     }

  1195.     /** Wrapper for resetting an integrator handlers.
  1196.      * <p>
  1197.      * This class is intended to be used in a try-with-resource statement.
  1198.      * If propagator-specific event handlers and step handlers are added to
  1199.      * the integrator in the try block, they will be removed automatically
  1200.      * when leaving the block, so the integrator only keeps its own handlers
  1201.      * between calls to {@link AbstractIntegratedPropagator#propagate(AbsoluteDate, AbsoluteDate).
  1202.      * </p>
  1203.      * @since 11.0
  1204.      */
  1205.     private static class IntegratorResetter implements AutoCloseable {

  1207.         /** Wrapped integrator. */
  1208.         private final ODEIntegrator integrator;

  1210.         /** Initial event detectors list. */
  1211.         private final List<ODEEventDetector> detectors;

  1213.         /** Initial step handlers list. */
  1214.         private final List<ODEStepHandler> stepHandlers;

  1216.         /** Simple constructor.
  1217.          * @param integrator wrapped integrator
  1218.          */
  1219.         IntegratorResetter(final ODEIntegrator integrator) {
  1220.             this.integrator   = integrator;
  1221.             this.detectors    = new ArrayList<>(integrator.getEventDetectors());
  1222.             this.stepHandlers = new ArrayList<>(integrator.getStepHandlers());
  1223.         }

  1225.         /** {@inheritDoc}
  1226.          * <p>
  1227.          * Reset event handlers and step handlers back to the initial list
  1228.          * </p>
  1229.          */
  1230.         @Override
  1231.         public void close() {

  1233.             // reset event handlers
  1234.             integrator.clearEventDetectors();
  1235.             detectors.forEach(integrator::addEventDetector);

  1237.             // reset step handlers
  1238.             integrator.clearStepHandlers();
  1239.             stepHandlers.forEach(integrator::addStepHandler);

  1241.         }

  1243.     }

  1245. }
Created with JaCoCo 0.8.11.202310140853