AbstractIntegratedPropagator.java

  1. /* Copyright 2002-2020 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.Collection;
  21. import java.util.Collections;
  22. import java.util.HashMap;
  23. import java.util.List;
  24. import java.util.Map;

  26. import org.hipparchus.exception.MathRuntimeException;
  27. import org.hipparchus.ode.DenseOutputModel;
  28. import org.hipparchus.ode.EquationsMapper;
  29. import org.hipparchus.ode.ExpandableODE;
  30. import org.hipparchus.ode.ODEIntegrator;
  31. import org.hipparchus.ode.ODEState;
  32. import org.hipparchus.ode.ODEStateAndDerivative;
  33. import org.hipparchus.ode.OrdinaryDifferentialEquation;
  34. import org.hipparchus.ode.SecondaryODE;
  35. import org.hipparchus.ode.events.Action;
  36. import org.hipparchus.ode.events.ODEEventHandler;
  37. import org.hipparchus.ode.sampling.AbstractODEStateInterpolator;
  38. import org.hipparchus.ode.sampling.ODEStateInterpolator;
  39. import org.hipparchus.ode.sampling.ODEStepHandler;
  40. import org.hipparchus.util.Precision;
  41. import org.orekit.attitudes.AttitudeProvider;
  42. import org.orekit.errors.OrekitException;
  43. import org.orekit.errors.OrekitIllegalStateException;
  44. import org.orekit.errors.OrekitInternalError;
  45. import org.orekit.errors.OrekitMessages;
  46. import org.orekit.frames.Frame;
  47. import org.orekit.orbits.OrbitType;
  48. import org.orekit.orbits.PositionAngle;
  49. import org.orekit.propagation.AbstractPropagator;
  50. import org.orekit.propagation.BoundedPropagator;
  51. import org.orekit.propagation.PropagationType;
  52. import org.orekit.propagation.SpacecraftState;
  53. import org.orekit.propagation.events.EventDetector;
  54. import org.orekit.propagation.sampling.OrekitStepHandler;
  55. import org.orekit.propagation.sampling.OrekitStepInterpolator;
  56. import org.orekit.time.AbsoluteDate;


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

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

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

  71.     /** Mode handler. */
  72.     private ModeHandler modeHandler;

  74.     /** Additional equations. */
  75.     private List<AdditionalEquations> additionalEquations;

  77.     /** Counter for differential equations calls. */
  78.     private int calls;

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

  83.     /** Equations mapper. */
  84.     private EquationsMapper equationsMapper;

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

  89.     /** Type of orbit to output (mean or osculating) <br/>
  90.      * <p>
  91.      * This is used only in the case of semianalitical propagators where there is a clear separation between
  92.      * mean and short periodic elements. It is ignored by the Numerical propagator.
  93.      * </p>
  94.      */
  95.     private PropagationType propagationType;

  97.     /** Build a new instance.
  98.      * @param integrator numerical integrator to use for propagation.
  99.      * @param propagationType type of orbit to output (mean or osculating).
  100.      */
  101.     protected AbstractIntegratedPropagator(final ODEIntegrator integrator, final PropagationType propagationType) {
  102.         detectors            = new ArrayList<EventDetector>();
  103.         additionalEquations  = new ArrayList<AdditionalEquations>();
  104.         this.integrator      = integrator;
  105.         this.propagationType = propagationType;
  106.         this.resetAtEnd      = true;
  107.     }

  109.     /** Allow/disallow resetting the initial state at end of propagation.
  110.      * <p>
  111.      * By default, at the end of the propagation, the propagator resets the initial state
  112.      * to the final state, thus allowing a new propagation to be started from there without
  113.      * recomputing the part already performed. Calling this method with {@code resetAtEnd} set
  114.      * to false changes prevents such reset.
  115.      * </p>
  116.      * @param resetAtEnd if true, at end of each propagation, the {@link
  117.      * #getInitialState() initial state} will be reset to the final state of
  118.      * the propagation, otherwise the initial state will be preserved
  119.      * @since 9.0
  120.      */
  121.     public void setResetAtEnd(final boolean resetAtEnd) {
  122.         this.resetAtEnd = resetAtEnd;
  123.     }

  125.     /** Initialize the mapper. */
  126.     protected void initMapper() {
  127.         stateMapper = createMapper(null, Double.NaN, null, null, null, null);
  128.     }

  130.     /**  {@inheritDoc} */
  131.     public void setAttitudeProvider(final AttitudeProvider attitudeProvider) {
  132.         super.setAttitudeProvider(attitudeProvider);
  133.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  134.                                    stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  135.                                    attitudeProvider, stateMapper.getFrame());
  136.     }

  138.     /** Set propagation orbit type.
  139.      * @param orbitType orbit type to use for propagation, null for
  140.      * propagating using {@link org.orekit.utils.AbsolutePVCoordinates AbsolutePVCoordinates}
  141.      * rather than {@link org.orekit.orbits Orbit}
  142.      */
  143.     protected void setOrbitType(final OrbitType orbitType) {
  144.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  145.                                    orbitType, stateMapper.getPositionAngleType(),
  146.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  147.     }

  149.     /** Get propagation parameter type.
  150.      * @return orbit type used for propagation, null for
  151.      * propagating using {@link org.orekit.utils.AbsolutePVCoordinates AbsolutePVCoordinates}
  152.      * rather than {@link org.orekit.orbits Orbit}
  153.      */
  154.     protected OrbitType getOrbitType() {
  155.         return stateMapper.getOrbitType();
  156.     }

  158.     /** Check if only the mean elements should be used in a semianalitical propagation.
  159.      * @return {@link PropagationType MEAN} if only mean elements have to be used or
  160.      *         {@link PropagationType OSCULATING} if osculating elements have to be also used.
  161.      */
  162.     protected PropagationType isMeanOrbit() {
  163.         return propagationType;
  164.     }

  166.     /** Set position angle type.
  167.      * <p>
  168.      * The position parameter type is meaningful only if {@link
  169.      * #getOrbitType() propagation orbit type}
  170.      * support it. As an example, it is not meaningful for propagation
  171.      * in {@link OrbitType#CARTESIAN Cartesian} parameters.
  172.      * </p>
  173.      * @param positionAngleType angle type to use for propagation
  174.      */
  175.     protected void setPositionAngleType(final PositionAngle positionAngleType) {
  176.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  177.                                    stateMapper.getOrbitType(), positionAngleType,
  178.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  179.     }

  181.     /** Get propagation parameter type.
  182.      * @return angle type to use for propagation
  183.      */
  184.     protected PositionAngle getPositionAngleType() {
  185.         return stateMapper.getPositionAngleType();
  186.     }

  188.     /** Set the central attraction coefficient μ.
  189.      * @param mu central attraction coefficient (m³/s²)
  190.      */
  191.     public void setMu(final double mu) {
  192.         stateMapper = createMapper(stateMapper.getReferenceDate(), mu,
  193.                                    stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  194.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  195.     }

  197.     /** Get the central attraction coefficient μ.
  198.      * @return mu central attraction coefficient (m³/s²)
  199.      * @see #setMu(double)
  200.      */
  201.     public double getMu() {
  202.         return stateMapper.getMu();
  203.     }

  205.     /** Get the number of calls to the differential equations computation method.
  206.      * <p>The number of calls is reset each time the {@link #propagate(AbsoluteDate)}
  207.      * method is called.</p>
  208.      * @return number of calls to the differential equations computation method
  209.      */
  210.     public int getCalls() {
  211.         return calls;
  212.     }

  214.     /** {@inheritDoc} */
  215.     @Override
  216.     public boolean isAdditionalStateManaged(final String name) {

  218.         // first look at already integrated states
  219.         if (super.isAdditionalStateManaged(name)) {
  220.             return true;
  221.         }

  223.         // then look at states we integrate ourselves
  224.         for (final AdditionalEquations equation : additionalEquations) {
  225.             if (equation.getName().equals(name)) {
  226.                 return true;
  227.             }
  228.         }

  230.         return false;
  231.     }

  233.     /** {@inheritDoc} */
  234.     @Override
  235.     public String[] getManagedAdditionalStates() {
  236.         final String[] alreadyIntegrated = super.getManagedAdditionalStates();
  237.         final String[] managed = new String[alreadyIntegrated.length + additionalEquations.size()];
  238.         System.arraycopy(alreadyIntegrated, 0, managed, 0, alreadyIntegrated.length);
  239.         for (int i = 0; i < additionalEquations.size(); ++i) {
  240.             managed[i + alreadyIntegrated.length] = additionalEquations.get(i).getName();
  241.         }
  242.         return managed;
  243.     }

  245.     /** Add a set of user-specified equations to be integrated along with the orbit propagation.
  246.      * @param additional additional equations
  247.      */
  248.     public void addAdditionalEquations(final AdditionalEquations additional) {

  250.         // check if the name is already used
  251.         if (isAdditionalStateManaged(additional.getName())) {
  252.             // this set of equations is already registered, complain
  253.             throw new OrekitException(OrekitMessages.ADDITIONAL_STATE_NAME_ALREADY_IN_USE,
  254.                                       additional.getName());
  255.         }

  257.         // this is really a new set of equations, add it
  258.         additionalEquations.add(additional);

  260.     }

  262.     /** {@inheritDoc} */
  263.     public void addEventDetector(final EventDetector detector) {
  264.         detectors.add(detector);
  265.     }

  267.     /** {@inheritDoc} */
  268.     public Collection<EventDetector> getEventsDetectors() {
  269.         return Collections.unmodifiableCollection(detectors);
  270.     }

  272.     /** {@inheritDoc} */
  273.     public void clearEventsDetectors() {
  274.         detectors.clear();
  275.     }

  277.     /** Set up all user defined event detectors.
  278.      */
  279.     protected void setUpUserEventDetectors() {
  280.         for (final EventDetector detector : detectors) {
  281.             setUpEventDetector(integrator, detector);
  282.         }
  283.     }

  285.     /** Wrap an Orekit event detector and register it to the integrator.
  286.      * @param integ integrator into which event detector should be registered
  287.      * @param detector event detector to wrap
  288.      */
  289.     protected void setUpEventDetector(final ODEIntegrator integ, final EventDetector detector) {
  290.         integ.addEventHandler(new AdaptedEventDetector(detector),
  291.                               detector.getMaxCheckInterval(),
  292.                               detector.getThreshold(),
  293.                               detector.getMaxIterationCount());
  294.     }

  296.     /** {@inheritDoc}
  297.      * <p>Note that this method has the side effect of replacing the step handlers
  298.      * of the underlying integrator set up in the {@link
  299.      * #AbstractIntegratedPropagator(ODEIntegrator, PropagationType) constructor}. So if a specific
  300.      * step handler is needed, it should be added after this method has been callled.</p>
  301.      */
  302.     public void setSlaveMode() {
  303.         super.setSlaveMode();
  304.         if (integrator != null) {
  305.             integrator.clearStepHandlers();
  306.         }
  307.         modeHandler = null;
  308.     }

  310.     /** {@inheritDoc}
  311.      * <p>Note that this method has the side effect of replacing the step handlers
  312.      * of the underlying integrator set up in the {@link
  313.      * #AbstractIntegratedPropagator(ODEIntegrator, PropagationType) constructor}. So if a specific
  314.      * step handler is needed, it should be added after this method has been callled.</p>
  315.      */
  316.     public void setMasterMode(final OrekitStepHandler handler) {
  317.         super.setMasterMode(handler);
  318.         integrator.clearStepHandlers();
  319.         final AdaptedStepHandler wrapped = new AdaptedStepHandler(handler);
  320.         integrator.addStepHandler(wrapped);
  321.         modeHandler = wrapped;
  322.     }

  324.     /** {@inheritDoc}
  325.      * <p>Note that this method has the side effect of replacing the step handlers
  326.      * of the underlying integrator set up in the {@link
  327.      * #AbstractIntegratedPropagator(ODEIntegrator, PropagationType) constructor}. So if a specific
  328.      * step handler is needed, it should be added after this method has been called.</p>
  329.      */
  330.     public void setEphemerisMode() {
  331.         super.setEphemerisMode();
  332.         integrator.clearStepHandlers();
  333.         final EphemerisModeHandler ephemeris = new EphemerisModeHandler();
  334.         modeHandler = ephemeris;
  335.         integrator.addStepHandler(ephemeris);
  336.     }

  338.     /**
  339.      * {@inheritDoc}
  340.      *
  341.      * <p>Note that this method has the side effect of replacing the step handlers of the
  342.      * underlying integrator set up in the {@link #AbstractIntegratedPropagator(ODEIntegrator,
  343.      * PropagationType) constructor}.</p>
  344.      */
  345.     @Override
  346.     public void setEphemerisMode(final OrekitStepHandler handler) {
  347.         super.setEphemerisMode();
  348.         integrator.clearStepHandlers();
  349.         final EphemerisModeHandler ephemeris = new EphemerisModeHandler(handler);
  350.         modeHandler = ephemeris;
  351.         integrator.addStepHandler(ephemeris);
  352.     }

  354.     /** {@inheritDoc} */
  355.     public BoundedPropagator getGeneratedEphemeris()
  356.         throws IllegalStateException {
  357.         if (getMode() != EPHEMERIS_GENERATION_MODE) {
  358.             throw new OrekitIllegalStateException(OrekitMessages.PROPAGATOR_NOT_IN_EPHEMERIS_GENERATION_MODE);
  359.         }
  360.         return ((EphemerisModeHandler) modeHandler).getEphemeris();
  361.     }

  363.     /** Create a mapper between raw double components and spacecraft state.
  364.     /** Simple constructor.
  365.      * <p>
  366.      * The position parameter type is meaningful only if {@link
  367.      * #getOrbitType() propagation orbit type}
  368.      * support it. As an example, it is not meaningful for propagation
  369.      * in {@link OrbitType#CARTESIAN Cartesian} parameters.
  370.      * </p>
  371.      * @param referenceDate reference date
  372.      * @param mu central attraction coefficient (m³/s²)
  373.      * @param orbitType orbit type to use for mapping
  374.      * @param positionAngleType angle type to use for propagation
  375.      * @param attitudeProvider attitude provider
  376.      * @param frame inertial frame
  377.      * @return new mapper
  378.      */
  379.     protected abstract StateMapper createMapper(AbsoluteDate referenceDate, double mu,
  380.                                                 OrbitType orbitType, PositionAngle positionAngleType,
  381.                                                 AttitudeProvider attitudeProvider, Frame frame);

  383.     /** Get the differential equations to integrate (for main state only).
  384.      * @param integ numerical integrator to use for propagation.
  385.      * @return differential equations for main state
  386.      */
  387.     protected abstract MainStateEquations getMainStateEquations(ODEIntegrator integ);

  389.     /** {@inheritDoc} */
  390.     public SpacecraftState propagate(final AbsoluteDate target) {
  391.         if (getStartDate() == null) {
  392.             if (getInitialState() == null) {
  393.                 throw new OrekitException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
  394.             }
  395.             setStartDate(getInitialState().getDate());
  396.         }
  397.         return propagate(getStartDate(), target);
  398.     }

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

  403.         if (getInitialState() == null) {
  404.             throw new OrekitException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
  405.         }

  407.         if (!tStart.equals(getInitialState().getDate())) {
  408.             // if propagation start date is not initial date,
  409.             // propagate from initial to start date without event detection
  410.             propagate(tStart, false);
  411.         }

  413.         // propagate from start date to end date with event detection
  414.         return propagate(tEnd, true);

  416.     }

  418.     /** Propagation with or without event detection.
  419.      * @param tEnd target date to which orbit should be propagated
  420.      * @param activateHandlers if true, step and event handlers should be activated
  421.      * @return state at end of propagation
  422.      */
  423.     protected SpacecraftState propagate(final AbsoluteDate tEnd, final boolean activateHandlers) {
  424.         try {

  426.             initializePropagation();

  428.             if (getInitialState().getDate().equals(tEnd)) {
  429.                 // don't extrapolate
  430.                 return getInitialState();
  431.             }

  433.             // space dynamics view
  434.             stateMapper = createMapper(getInitialState().getDate(), stateMapper.getMu(),
  435.                                        stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  436.                                        stateMapper.getAttitudeProvider(), getInitialState().getFrame());


  439.             if (Double.isNaN(getMu())) {
  440.                 setMu(getInitialState().getMu());
  441.             }

  443.             if (getInitialState().getMass() <= 0.0) {
  444.                 throw new OrekitException(OrekitMessages.SPACECRAFT_MASS_BECOMES_NEGATIVE,
  445.                                           getInitialState().getMass());
  446.             }

  448.             integrator.clearEventHandlers();

  450.             // set up events added by user, only if handlers are activated
  451.             if (activateHandlers) {
  452.                 setUpUserEventDetectors();
  453.             }

  455.             // convert space flight dynamics API to math API
  456.             final SpacecraftState initialIntegrationState = getInitialIntegrationState();
  457.             final ODEState mathInitialState = createInitialState(initialIntegrationState);
  458.             final ExpandableODE mathODE = createODE(integrator, mathInitialState);
  459.             equationsMapper = mathODE.getMapper();

  461.             // initialize mode handler
  462.             if (modeHandler != null) {
  463.                 modeHandler.initialize(activateHandlers, tEnd);
  464.             }

  466.             // mathematical integration
  467.             final ODEStateAndDerivative mathFinalState;
  468.             beforeIntegration(initialIntegrationState, tEnd);
  469.             mathFinalState = integrator.integrate(mathODE, mathInitialState,
  470.                                                   tEnd.durationFrom(getInitialState().getDate()));
  471.             afterIntegration();

  473.             // get final state
  474.             SpacecraftState finalState =
  475.                             stateMapper.mapArrayToState(stateMapper.mapDoubleToDate(mathFinalState.getTime(),
  476.                                                                                     tEnd),
  477.                                                         mathFinalState.getPrimaryState(),
  478.                                                         mathFinalState.getPrimaryDerivative(),
  479.                                                         propagationType);

  481.             finalState = updateAdditionalStates(finalState);
  482.             for (int i = 0; i < additionalEquations.size(); ++i) {
  483.                 final double[] secondary = mathFinalState.getSecondaryState(i + 1);
  484.                 finalState = finalState.addAdditionalState(additionalEquations.get(i).getName(),
  485.                                                            secondary);
  486.             }
  487.             if (resetAtEnd) {
  488.                 resetInitialState(finalState);
  489.                 setStartDate(finalState.getDate());
  490.             }

  492.             return finalState;

  494.         } catch (MathRuntimeException mre) {
  495.             throw OrekitException.unwrap(mre);
  496.         }
  497.     }

  499.     /** Get the initial state for integration.
  500.      * @return initial state for integration
  501.      */
  502.     protected SpacecraftState getInitialIntegrationState() {
  503.         return getInitialState();
  504.     }

  506.     /** Create an initial state.
  507.      * @param initialState initial state in flight dynamics world
  508.      * @return initial state in mathematics world
  509.      */
  510.     private ODEState createInitialState(final SpacecraftState initialState) {

  512.         // retrieve initial state
  513.         final double[] primary  = new double[getBasicDimension()];
  514.         stateMapper.mapStateToArray(initialState, primary, null);

  516.         // secondary part of the ODE
  517.         final double[][] secondary = new double[additionalEquations.size()][];
  518.         for (int i = 0; i < additionalEquations.size(); ++i) {
  519.             final AdditionalEquations additional = additionalEquations.get(i);
  520.             secondary[i] = initialState.getAdditionalState(additional.getName());
  521.         }

  523.         return new ODEState(0.0, primary, secondary);

  525.     }

  527.     /** Create an ODE with all equations.
  528.      * @param integ numerical integrator to use for propagation.
  529.      * @param mathInitialState initial state
  530.      * @return a new ode
  531.      */
  532.     private ExpandableODE createODE(final ODEIntegrator integ,
  533.                                     final ODEState mathInitialState) {

  535.         final ExpandableODE ode =
  536.                 new ExpandableODE(new ConvertedMainStateEquations(getMainStateEquations(integ)));

  538.         // secondary part of the ODE
  539.         for (int i = 0; i < additionalEquations.size(); ++i) {
  540.             final AdditionalEquations additional = additionalEquations.get(i);
  541.             final SecondaryODE secondary =
  542.                     new ConvertedSecondaryStateEquations(additional,
  543.                                                          mathInitialState.getSecondaryStateDimension(i + 1));
  544.             ode.addSecondaryEquations(secondary);
  545.         }

  547.         return ode;

  549.     }

  551.     /** Method called just before integration.
  552.      * <p>
  553.      * The default implementation does nothing, it may be specialized in subclasses.
  554.      * </p>
  555.      * @param initialState initial state
  556.      * @param tEnd target date at which state should be propagated
  557.      */
  558.     protected void beforeIntegration(final SpacecraftState initialState,
  559.                                      final AbsoluteDate tEnd) {
  560.         // do nothing by default
  561.     }

  563.     /** Method called just after integration.
  564.      * <p>
  565.      * The default implementation does nothing, it may be specialized in subclasses.
  566.      * </p>
  567.      */
  568.     protected void afterIntegration() {
  569.         // do nothing by default
  570.     }

  572.     /** Get state vector dimension without additional parameters.
  573.      * @return state vector dimension without additional parameters.
  574.      */
  575.     public int getBasicDimension() {
  576.         return 7;

  578.     }

  580.     /** Get the integrator used by the propagator.
  581.      * @return the integrator.
  582.      */
  583.     protected ODEIntegrator getIntegrator() {
  584.         return integrator;
  585.     }

  587.     /** Get a complete state with all additional equations.
  588.      * @param t current value of the independent <I>time</I> variable
  589.      * @param y array containing the current value of the state vector
  590.      * @param yDot array containing the current value of the state vector derivative
  591.      * @return complete state
  592.      */
  593.     private SpacecraftState getCompleteState(final double t, final double[] y, final double[] yDot) {

  595.         // main state
  596.         SpacecraftState state = stateMapper.mapArrayToState(t, y, yDot, propagationType);

  598.         // pre-integrated additional states
  599.         state = updateAdditionalStates(state);

  601.         // additional states integrated here
  602.         if (!additionalEquations.isEmpty()) {

  604.             for (int i = 0; i < additionalEquations.size(); ++i) {
  605.                 state = state.addAdditionalState(additionalEquations.get(i).getName(),
  606.                                                  equationsMapper.extractEquationData(i + 1, y));
  607.             }

  609.         }

  611.         return state;

  613.     }

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

  618.         /**
  619.          * Initialize the equations at the start of propagation. This method will be
  620.          * called before any calls to {@link #computeDerivatives(SpacecraftState)}.
  621.          *
  622.          * <p> The default implementation of this method does nothing.
  623.          *
  624.          * @param initialState initial state information at the start of propagation.
  625.          * @param target       date of propagation. Not equal to {@code
  626.          *                     initialState.getDate()}.
  627.          */
  628.         default void init(final SpacecraftState initialState, final AbsoluteDate target) {
  629.         }

  631.         /** Compute differential equations for main state.
  632.          * @param state current state
  633.          * @return derivatives of main state
  634.          */
  635.         double[] computeDerivatives(SpacecraftState state);

  637.     }

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

  642.         /** Main state equations. */
  643.         private final MainStateEquations main;

  645.         /** Simple constructor.
  646.          * @param main main state equations
  647.          */
  648.         ConvertedMainStateEquations(final MainStateEquations main) {
  649.             this.main = main;
  650.             calls = 0;
  651.         }

  653.         /** {@inheritDoc} */
  654.         public int getDimension() {
  655.             return getBasicDimension();
  656.         }

  658.         @Override
  659.         public void init(final double t0, final double[] y0, final double finalTime) {
  660.             // update space dynamics view
  661.             SpacecraftState initialState = stateMapper.mapArrayToState(t0, y0, null, PropagationType.MEAN);
  662.             initialState = updateAdditionalStates(initialState);
  663.             final AbsoluteDate target = stateMapper.mapDoubleToDate(finalTime);
  664.             main.init(initialState, target);
  665.         }

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

  670.             // increment calls counter
  671.             ++calls;

  673.             // update space dynamics view
  674.             SpacecraftState currentState = stateMapper.mapArrayToState(t, y, null, PropagationType.MEAN);
  675.             currentState = updateAdditionalStates(currentState);

  677.             // compute main state differentials
  678.             return main.computeDerivatives(currentState);

  680.         }

  682.     }

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

  687.         /** Additional equations. */
  688.         private final AdditionalEquations equations;

  690.         /** Dimension of the additional state. */
  691.         private final int dimension;

  693.         /** Simple constructor.
  694.          * @param equations additional equations
  695.          * @param dimension dimension of the additional state
  696.          */
  697.         ConvertedSecondaryStateEquations(final AdditionalEquations equations,
  698.                                          final int dimension) {
  699.             this.equations = equations;
  700.             this.dimension = dimension;
  701.         }

  703.         /** {@inheritDoc} */
  704.         @Override
  705.         public int getDimension() {
  706.             return dimension;
  707.         }

  709.         /** {@inheritDoc} */
  710.         @Override
  711.         public void init(final double t0, final double[] primary0,
  712.                          final double[] secondary0, final double finalTime) {
  713.             // update space dynamics view
  714.             SpacecraftState initialState = stateMapper.mapArrayToState(t0, primary0, null, PropagationType.MEAN);
  715.             initialState = updateAdditionalStates(initialState);
  716.             initialState = initialState.addAdditionalState(equations.getName(), secondary0);
  717.             final AbsoluteDate target = stateMapper.mapDoubleToDate(finalTime);
  718.             equations.init(initialState, target);

  720.         }

  722.         /** {@inheritDoc} */
  723.         @Override
  724.         public double[] computeDerivatives(final double t, final double[] primary,
  725.                                            final double[] primaryDot, final double[] secondary) {

  727.             // update space dynamics view
  728.             SpacecraftState currentState = stateMapper.mapArrayToState(t, primary, primaryDot, PropagationType.MEAN);
  729.             currentState = updateAdditionalStates(currentState);
  730.             currentState = currentState.addAdditionalState(equations.getName(), secondary);

  732.             // compute additional derivatives
  733.             final double[] secondaryDot = new double[secondary.length];
  734.             final double[] additionalMainDot =
  735.                             equations.computeDerivatives(currentState, secondaryDot);
  736.             if (additionalMainDot != null) {
  737.                 // the additional equations have an effect on main equations
  738.                 for (int i = 0; i < additionalMainDot.length; ++i) {
  739.                     primaryDot[i] += additionalMainDot[i];
  740.                 }
  741.             }

  743.             return secondaryDot;

  745.         }

  747.     }

  749.     /** Adapt an {@link org.orekit.propagation.events.EventDetector}
  750.      * to Hipparchus {@link org.hipparchus.ode.events.ODEEventHandler} interface.
  751.      * @author Fabien Maussion
  752.      */
  753.     private class AdaptedEventDetector implements ODEEventHandler {

  755.         /** Underlying event detector. */
  756.         private final EventDetector detector;

  758.         /** Time of the previous call to g. */
  759.         private double lastT;

  761.         /** Value from the previous call to g. */
  762.         private double lastG;

  764.         /** Build a wrapped event detector.
  765.          * @param detector event detector to wrap
  766.         */
  767.         AdaptedEventDetector(final EventDetector detector) {
  768.             this.detector = detector;
  769.             this.lastT    = Double.NaN;
  770.             this.lastG    = Double.NaN;
  771.         }

  773.         /** {@inheritDoc} */
  774.         public void init(final ODEStateAndDerivative s0, final double t) {
  775.             detector.init(getCompleteState(s0.getTime(), s0.getCompleteState(), s0.getCompleteDerivative()),
  776.                           stateMapper.mapDoubleToDate(t));
  777.             this.lastT = Double.NaN;
  778.             this.lastG = Double.NaN;
  779.         }

  781.         /** {@inheritDoc} */
  782.         public double g(final ODEStateAndDerivative s) {
  783.             if (!Precision.equals(lastT, s.getTime(), 0)) {
  784.                 lastT = s.getTime();
  785.                 lastG = detector.g(getCompleteState(s.getTime(), s.getCompleteState(), s.getCompleteDerivative()));
  786.             }
  787.             return lastG;
  788.         }

  790.         /** {@inheritDoc} */
  791.         public Action eventOccurred(final ODEStateAndDerivative s, final boolean increasing) {
  792.             return detector.eventOccurred(
  793.                     getCompleteState(
  794.                             s.getTime(),
  795.                             s.getCompleteState(),
  796.                             s.getCompleteDerivative()),
  797.                     increasing);
  798.         }

  800.         /** {@inheritDoc} */
  801.         public ODEState resetState(final ODEStateAndDerivative s) {

  803.             final SpacecraftState oldState = getCompleteState(s.getTime(), s.getCompleteState(), s.getCompleteDerivative());
  804.             final SpacecraftState newState = detector.resetState(oldState);
  805.             stateChanged(newState);

  807.             // main part
  808.             final double[] primary    = new double[s.getPrimaryStateDimension()];
  809.             stateMapper.mapStateToArray(newState, primary, null);

  811.             // secondary part
  812.             final double[][] secondary    = new double[additionalEquations.size()][];
  813.             for (int i = 0; i < additionalEquations.size(); ++i) {
  814.                 secondary[i] = newState.getAdditionalState(additionalEquations.get(i).getName());
  815.             }

  817.             return new ODEState(newState.getDate().durationFrom(getStartDate()),
  818.                                 primary, secondary);

  820.         }

  822.     }

  824.     /** Adapt an {@link org.orekit.propagation.sampling.OrekitStepHandler}
  825.      * to Hipparchus {@link ODEStepHandler} interface.
  826.      * @author Luc Maisonobe
  827.      */
  828.     private class AdaptedStepHandler implements ODEStepHandler, ModeHandler {

  830.         /** Underlying handler. */
  831.         private final OrekitStepHandler handler;

  833.         /** Flag for handler . */
  834.         private boolean activate;

  836.         /** Build an instance.
  837.          * @param handler underlying handler to wrap
  838.          */
  839.         AdaptedStepHandler(final OrekitStepHandler handler) {
  840.             this.handler = handler;
  841.         }

  843.         /** {@inheritDoc} */
  844.         public void initialize(final boolean activateHandlers,
  845.                                final AbsoluteDate targetDate) {
  846.             this.activate = activateHandlers;
  847.         }

  849.         /** {@inheritDoc} */
  850.         public void init(final ODEStateAndDerivative s0, final double t) {
  851.             if (activate) {
  852.                 handler.init(getCompleteState(s0.getTime(), s0.getCompleteState(), s0.getCompleteDerivative()),
  853.                              stateMapper.mapDoubleToDate(t));
  854.             }
  855.         }

  857.         /** {@inheritDoc} */
  858.         public void handleStep(final ODEStateInterpolator interpolator, final boolean isLast) {
  859.             if (activate) {
  860.                 handler.handleStep(new AdaptedStepInterpolator(interpolator), isLast);
  861.             }
  862.         }

  864.     }

  866.     /** Adapt an Hipparchus {@link ODEStateInterpolator}
  867.      * to an orekit {@link OrekitStepInterpolator} interface.
  868.      * @author Luc Maisonobe
  869.      */
  870.     private class AdaptedStepInterpolator implements OrekitStepInterpolator {

  872.         /** Underlying raw rawInterpolator. */
  873.         private final ODEStateInterpolator mathInterpolator;

  875.         /** Simple constructor.
  876.          * @param mathInterpolator underlying raw interpolator
  877.          */
  878.         AdaptedStepInterpolator(final ODEStateInterpolator mathInterpolator) {
  879.             this.mathInterpolator = mathInterpolator;
  880.         }

  882.         /** {@inheritDoc}} */
  883.         @Override
  884.         public SpacecraftState getPreviousState() {
  885.             return convert(mathInterpolator.getPreviousState());
  886.         }

  888.         /** {@inheritDoc}} */
  889.         @Override
  890.         public boolean isPreviousStateInterpolated() {
  891.             return mathInterpolator.isPreviousStateInterpolated();
  892.         }

  894.         /** {@inheritDoc}} */
  895.         @Override
  896.         public SpacecraftState getCurrentState() {
  897.             return convert(mathInterpolator.getCurrentState());
  898.         }

  900.         /** {@inheritDoc}} */
  901.         @Override
  902.         public boolean isCurrentStateInterpolated() {
  903.             return mathInterpolator.isCurrentStateInterpolated();
  904.         }

  906.         /** {@inheritDoc}} */
  907.         @Override
  908.         public SpacecraftState getInterpolatedState(final AbsoluteDate date) {
  909.             return convert(mathInterpolator.getInterpolatedState(date.durationFrom(stateMapper.getReferenceDate())));
  910.         }

  912.         /** Convert a state from mathematical world to space flight dynamics world.
  913.          * @param os mathematical state
  914.          * @return space flight dynamics state
  915.          */
  916.         private SpacecraftState convert(final ODEStateAndDerivative os) {

  918.             SpacecraftState s =
  919.                             stateMapper.mapArrayToState(os.getTime(),
  920.                                                         os.getPrimaryState(),
  921.                                                         os.getPrimaryDerivative(),
  922.                                                         propagationType);
  923.             s = updateAdditionalStates(s);
  924.             for (int i = 0; i < additionalEquations.size(); ++i) {
  925.                 final double[] secondary = os.getSecondaryState(i + 1);
  926.                 s = s.addAdditionalState(additionalEquations.get(i).getName(), secondary);
  927.             }

  929.             return s;

  931.         }

  933.         /** Convert a state from space flight dynamics world to mathematical world.
  934.          * @param state space flight dynamics state
  935.          * @return mathematical state
  936.          */
  937.         private ODEStateAndDerivative convert(final SpacecraftState state) {

  939.             // retrieve initial state
  940.             final double[] primary    = new double[getBasicDimension()];
  941.             final double[] primaryDot = new double[getBasicDimension()];
  942.             stateMapper.mapStateToArray(state, primary, primaryDot);

  944.             // secondary part of the ODE
  945.             final double[][] secondary    = new double[additionalEquations.size()][];
  946.             for (int i = 0; i < additionalEquations.size(); ++i) {
  947.                 final AdditionalEquations additional = additionalEquations.get(i);
  948.                 secondary[i] = state.getAdditionalState(additional.getName());
  949.             }

  951.             return new ODEStateAndDerivative(stateMapper.mapDateToDouble(state.getDate()),
  952.                                              primary, primaryDot,
  953.                                              secondary, null);

  955.         }

  957.         /** {@inheritDoc}} */
  958.         @Override
  959.         public boolean isForward() {
  960.             return mathInterpolator.isForward();
  961.         }

  963.         /** {@inheritDoc}} */
  964.         @Override
  965.         public AdaptedStepInterpolator restrictStep(final SpacecraftState newPreviousState,
  966.                                                     final SpacecraftState newCurrentState) {
  967.             try {
  968.                 final AbstractODEStateInterpolator aosi = (AbstractODEStateInterpolator) mathInterpolator;
  969.                 return new AdaptedStepInterpolator(aosi.restrictStep(convert(newPreviousState),
  970.                                                                      convert(newCurrentState)));
  971.             } catch (ClassCastException cce) {
  972.                 // this should never happen
  973.                 throw new OrekitInternalError(cce);
  974.             }
  975.         }

  977.     }

  979.     private class EphemerisModeHandler implements ModeHandler, ODEStepHandler {

  981.         /** Underlying raw mathematical model. */
  982.         private DenseOutputModel model;

  984.         /** Generated ephemeris. */
  985.         private BoundedPropagator ephemeris;

  987.         /** Flag for handler . */
  988.         private boolean activate;

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

  993.         /** User's integration step handler. May be null. */
  994.         private final AdaptedStepHandler handler;

  996.         /** Creates a new instance of EphemerisModeHandler which must be
  997.          *  filled by the propagator.
  998.          */
  999.         EphemerisModeHandler() {
  1000.             this.handler = null;
  1001.         }

  1003.         /** Creates a new instance of EphemerisModeHandler which must be
  1004.          *  filled by the propagator.
  1005.          *  @param handler the handler to notify of every integrator step.
  1006.          */
  1007.         EphemerisModeHandler(final OrekitStepHandler handler) {
  1008.             this.handler = new AdaptedStepHandler(handler);
  1009.         }

  1011.         /** {@inheritDoc} */
  1012.         public void initialize(final boolean activateHandlers,
  1013.                                final AbsoluteDate targetDate) {
  1014.             this.activate = activateHandlers;
  1015.             this.model    = new DenseOutputModel();
  1016.             this.endDate  = targetDate;

  1018.             // ephemeris will be generated when last step is processed
  1019.             this.ephemeris = null;
  1020.             if (this.handler != null) {
  1021.                 this.handler.initialize(activateHandlers, targetDate);
  1022.             }
  1023.         }

  1025.         /** Get the generated ephemeris.
  1026.          * @return a new instance of the generated ephemeris
  1027.          */
  1028.         public BoundedPropagator getEphemeris() {
  1029.             return ephemeris;
  1030.         }

  1032.         /** {@inheritDoc} */
  1033.         public void handleStep(final ODEStateInterpolator interpolator, final boolean isLast) {
  1034.             if (activate) {
  1035.                 if (this.handler != null) {
  1036.                     this.handler.handleStep(interpolator, isLast);
  1037.                 }

  1039.                 model.handleStep(interpolator, isLast);
  1040.                 if (isLast) {

  1042.                     // set up the boundary dates
  1043.                     final double tI = model.getInitialTime();
  1044.                     final double tF = model.getFinalTime();
  1045.                     // tI is almost? always zero
  1046.                     final AbsoluteDate startDate =
  1047.                                     stateMapper.mapDoubleToDate(tI);
  1048.                     final AbsoluteDate finalDate =
  1049.                                     stateMapper.mapDoubleToDate(tF, this.endDate);
  1050.                     final AbsoluteDate minDate;
  1051.                     final AbsoluteDate maxDate;
  1052.                     if (tF < tI) {
  1053.                         minDate = finalDate;
  1054.                         maxDate = startDate;
  1055.                     } else {
  1056.                         minDate = startDate;
  1057.                         maxDate = finalDate;
  1058.                     }

  1060.                     // get the initial additional states that are not managed
  1061.                     final Map<String, double[]> unmanaged = new HashMap<String, double[]>();
  1062.                     for (final Map.Entry<String, double[]> initial : getInitialState().getAdditionalStates().entrySet()) {
  1063.                         if (!isAdditionalStateManaged(initial.getKey())) {
  1064.                             // this additional state was in the initial state, but is unknown to the propagator
  1065.                             // we simply copy its initial value as is
  1066.                             unmanaged.put(initial.getKey(), initial.getValue());
  1067.                         }
  1068.                     }

  1070.                     // get the names of additional states managed by differential equations
  1071.                     final String[] names = new String[additionalEquations.size()];
  1072.                     for (int i = 0; i < names.length; ++i) {
  1073.                         names[i] = additionalEquations.get(i).getName();
  1074.                     }

  1076.                     // create the ephemeris
  1077.                     ephemeris = new IntegratedEphemeris(startDate, minDate, maxDate,
  1078.                                                         stateMapper, propagationType, model, unmanaged,
  1079.                                                         getAdditionalStateProviders(), names);

  1081.                 }
  1082.             }

  1084.         }

  1086.         /** {@inheritDoc} */
  1087.         public void init(final ODEStateAndDerivative s0, final double t) {
  1088.             model.init(s0, t);
  1089.             if (this.handler != null) {
  1090.                 this.handler.init(s0, t);
  1091.             }
  1092.         }

  1094.     }

  1096. }
Created with JaCoCo 0.8.6.202009150832