AbstractIntegratedPropagator.java

  1. /* Copyright 2002-2019 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (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.Orbit;
  48. import org.orekit.orbits.OrbitType;
  49. import org.orekit.orbits.PositionAngle;
  50. import org.orekit.propagation.AbstractPropagator;
  51. import org.orekit.propagation.BoundedPropagator;
  52. import org.orekit.propagation.SpacecraftState;
  53. import org.orekit.propagation.events.EventDetector;
  54. import org.orekit.propagation.events.handlers.EventHandler;
  55. import org.orekit.propagation.sampling.OrekitStepHandler;
  56. import org.orekit.propagation.sampling.OrekitStepInterpolator;
  57. import org.orekit.time.AbsoluteDate;


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

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

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

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

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

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

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

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

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

  90.     /** Output only the mean orbit. <br/>
  91.      * <p>
  92.      * This is used only in the case of semianalitical propagators where there is a clear separation between
  93.      * mean and short periodic elements. It is ignored by the Numerical propagator.
  94.      * </p>
  95.      */
  96.     private boolean meanOrbit;

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

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

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

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

  139.     /** Set propagation orbit type.
  140.      * @param orbitType orbit type to use for propagation
  141.      */
  142.     protected void setOrbitType(final OrbitType orbitType) {
  143.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  144.                                    orbitType, stateMapper.getPositionAngleType(),
  145.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  146.     }

  148.     /** Get propagation parameter type.
  149.      * @return orbit type used for propagation
  150.      */
  151.     protected OrbitType getOrbitType() {
  152.         return stateMapper.getOrbitType();
  153.     }

  155.     /** Check if only the mean elements should be used in a semianalitical propagation.
  156.      * @return true if only mean elements have to be used
  157.      */
  158.     protected boolean isMeanOrbit() {
  159.         return meanOrbit;
  160.     }

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

  177.     /** Get propagation parameter type.
  178.      * @return angle type to use for propagation
  179.      */
  180.     protected PositionAngle getPositionAngleType() {
  181.         return stateMapper.getPositionAngleType();
  182.     }

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

  193.     /** Get the central attraction coefficient μ.
  194.      * @return mu central attraction coefficient (m³/s²)
  195.      * @see #setMu(double)
  196.      */
  197.     public double getMu() {
  198.         return stateMapper.getMu();
  199.     }

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

  210.     /** {@inheritDoc} */
  211.     @Override
  212.     public boolean isAdditionalStateManaged(final String name) {

  214.         // first look at already integrated states
  215.         if (super.isAdditionalStateManaged(name)) {
  216.             return true;
  217.         }

  219.         // then look at states we integrate ourselves
  220.         for (final AdditionalEquations equation : additionalEquations) {
  221.             if (equation.getName().equals(name)) {
  222.                 return true;
  223.             }
  224.         }

  226.         return false;
  227.     }

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

  241.     /** Add a set of user-specified equations to be integrated along with the orbit propagation.
  242.      * @param additional additional equations
  243.      */
  244.     public void addAdditionalEquations(final AdditionalEquations additional) {

  246.         // check if the name is already used
  247.         if (isAdditionalStateManaged(additional.getName())) {
  248.             // this set of equations is already registered, complain
  249.             throw new OrekitException(OrekitMessages.ADDITIONAL_STATE_NAME_ALREADY_IN_USE,
  250.                                       additional.getName());
  251.         }

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

  256.     }

  258.     /** {@inheritDoc} */
  259.     public void addEventDetector(final EventDetector detector) {
  260.         detectors.add(detector);
  261.     }

  263.     /** {@inheritDoc} */
  264.     public Collection<EventDetector> getEventsDetectors() {
  265.         return Collections.unmodifiableCollection(detectors);
  266.     }

  268.     /** {@inheritDoc} */
  269.     public void clearEventsDetectors() {
  270.         detectors.clear();
  271.     }

  273.     /** Set up all user defined event detectors.
  274.      */
  275.     protected void setUpUserEventDetectors() {
  276.         for (final EventDetector detector : detectors) {
  277.             setUpEventDetector(integrator, detector);
  278.         }
  279.     }

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

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

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

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

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

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

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

  379.     /** Get the differential equations to integrate (for main state only).
  380.      * @param integ numerical integrator to use for propagation.
  381.      * @return differential equations for main state
  382.      */
  383.     protected abstract MainStateEquations getMainStateEquations(ODEIntegrator integ);

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

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

  399.         if (getInitialState() == null) {
  400.             throw new OrekitException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
  401.         }

  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.             propagate(tStart, false);
  407.         }

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

  412.     }

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

  422.             if (getInitialState().getDate().equals(tEnd)) {
  423.                 // don't extrapolate
  424.                 return getInitialState();
  425.             }

  427.             // space dynamics view
  428.             stateMapper = createMapper(getInitialState().getDate(), stateMapper.getMu(),
  429.                                        stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  430.                                        stateMapper.getAttitudeProvider(), getInitialState().getFrame());


  433.             // set propagation orbit type
  434.             final Orbit initialOrbit = stateMapper.getOrbitType().convertType(getInitialState().getOrbit());
  435.             if (Double.isNaN(getMu())) {
  436.                 setMu(initialOrbit.getMu());
  437.             }

  439.             if (getInitialState().getMass() <= 0.0) {
  440.                 throw new OrekitException(OrekitMessages.SPACECRAFT_MASS_BECOMES_NEGATIVE,
  441.                                           getInitialState().getMass());
  442.             }

  444.             integrator.clearEventHandlers();

  446.             // set up events added by user, only if handlers are activated
  447.             if (activateHandlers) {
  448.                 setUpUserEventDetectors();
  449.             }

  451.             // convert space flight dynamics API to math API
  452.             final ODEState mathInitialState = createInitialState(getInitialIntegrationState());
  453.             final ExpandableODE mathODE = createODE(integrator, mathInitialState);
  454.             equationsMapper = mathODE.getMapper();

  456.             // initialize mode handler
  457.             if (modeHandler != null) {
  458.                 modeHandler.initialize(activateHandlers, tEnd);
  459.             }

  461.             // mathematical integration
  462.             final ODEStateAndDerivative mathFinalState;
  463.             beforeIntegration(getInitialState(), tEnd);
  464.             mathFinalState = integrator.integrate(mathODE, mathInitialState,
  465.                                                   tEnd.durationFrom(getInitialState().getDate()));
  466.             afterIntegration();

  468.             // get final state
  469.             SpacecraftState finalState =
  470.                             stateMapper.mapArrayToState(stateMapper.mapDoubleToDate(mathFinalState.getTime(),
  471.                                                                                     tEnd),
  472.                                                         mathFinalState.getPrimaryState(),
  473.                                                         mathFinalState.getPrimaryDerivative(),
  474.                                                         meanOrbit);
  475.             finalState = updateAdditionalStates(finalState);
  476.             for (int i = 0; i < additionalEquations.size(); ++i) {
  477.                 final double[] secondary = mathFinalState.getSecondaryState(i + 1);
  478.                 finalState = finalState.addAdditionalState(additionalEquations.get(i).getName(),
  479.                                                            secondary);
  480.             }
  481.             if (resetAtEnd) {
  482.                 resetInitialState(finalState);
  483.                 setStartDate(finalState.getDate());
  484.             }

  486.             return finalState;

  488.         } catch (MathRuntimeException mre) {
  489.             throw OrekitException.unwrap(mre);
  490.         }
  491.     }

  493.     /** Get the initial state for integration.
  494.      * @return initial state for integration
  495.      */
  496.     protected SpacecraftState getInitialIntegrationState() {
  497.         return getInitialState();
  498.     }

  500.     /** Create an initial state.
  501.      * @param initialState initial state in flight dynamics world
  502.      * @return initial state in mathematics world
  503.      */
  504.     private ODEState createInitialState(final SpacecraftState initialState) {

  506.         // retrieve initial state
  507.         final double[] primary  = new double[getBasicDimension()];
  508.         stateMapper.mapStateToArray(initialState, primary, null);

  510.         // secondary part of the ODE
  511.         final double[][] secondary = new double[additionalEquations.size()][];
  512.         for (int i = 0; i < additionalEquations.size(); ++i) {
  513.             final AdditionalEquations additional = additionalEquations.get(i);
  514.             secondary[i] = initialState.getAdditionalState(additional.getName());
  515.         }

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

  519.     }

  521.     /** Create an ODE with all equations.
  522.      * @param integ numerical integrator to use for propagation.
  523.      * @param mathInitialState initial state
  524.      * @return a new ode
  525.      */
  526.     private ExpandableODE createODE(final ODEIntegrator integ,
  527.                                     final ODEState mathInitialState) {

  529.         final ExpandableODE ode =
  530.                 new ExpandableODE(new ConvertedMainStateEquations(getMainStateEquations(integ)));

  532.         // secondary part of the ODE
  533.         for (int i = 0; i < additionalEquations.size(); ++i) {
  534.             final AdditionalEquations additional = additionalEquations.get(i);
  535.             final SecondaryODE secondary =
  536.                     new ConvertedSecondaryStateEquations(additional,
  537.                                                          mathInitialState.getSecondaryStateDimension(i + 1));
  538.             ode.addSecondaryEquations(secondary);
  539.         }

  541.         return ode;

  543.     }

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

  557.     /** Method called just after integration.
  558.      * <p>
  559.      * The default implementation does nothing, it may be specialized in subclasses.
  560.      * </p>
  561.      */
  562.     protected void afterIntegration() {
  563.         // do nothing by default
  564.     }

  566.     /** Get state vector dimension without additional parameters.
  567.      * @return state vector dimension without additional parameters.
  568.      */
  569.     public int getBasicDimension() {
  570.         return 7;

  572.     }

  574.     /** Get the integrator used by the propagator.
  575.      * @return the integrator.
  576.      */
  577.     protected ODEIntegrator getIntegrator() {
  578.         return integrator;
  579.     }

  581.     /** Get a complete state with all additional equations.
  582.      * @param t current value of the independent <I>time</I> variable
  583.      * @param y array containing the current value of the state vector
  584.      * @param yDot array containing the current value of the state vector derivative
  585.      * @return complete state
  586.      */
  587.     private SpacecraftState getCompleteState(final double t, final double[] y, final double[] yDot) {

  589.         // main state
  590.         SpacecraftState state = stateMapper.mapArrayToState(t, y, yDot, meanOrbit);

  592.         // pre-integrated additional states
  593.         state = updateAdditionalStates(state);

  595.         // additional states integrated here
  596.         if (!additionalEquations.isEmpty()) {

  598.             for (int i = 0; i < additionalEquations.size(); ++i) {
  599.                 state = state.addAdditionalState(additionalEquations.get(i).getName(),
  600.                                                  equationsMapper.extractEquationData(i + 1, y));
  601.             }

  603.         }

  605.         return state;

  607.     }

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

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

  625.         /** Compute differential equations for main state.
  626.          * @param state current state
  627.          * @return derivatives of main state
  628.          */
  629.         double[] computeDerivatives(SpacecraftState state);

  631.     }

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

  636.         /** Main state equations. */
  637.         private final MainStateEquations main;

  639.         /** Simple constructor.
  640.          * @param main main state equations
  641.          */
  642.         ConvertedMainStateEquations(final MainStateEquations main) {
  643.             this.main = main;
  644.             calls = 0;
  645.         }

  647.         /** {@inheritDoc} */
  648.         public int getDimension() {
  649.             return getBasicDimension();
  650.         }

  652.         @Override
  653.         public void init(final double t0, final double[] y0, final double finalTime) {
  654.             // update space dynamics view
  655.             SpacecraftState initialState = stateMapper.mapArrayToState(t0, y0, null, true);
  656.             initialState = updateAdditionalStates(initialState);
  657.             final AbsoluteDate target = stateMapper.mapDoubleToDate(finalTime);
  658.             main.init(initialState, target);
  659.         }

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

  664.             // increment calls counter
  665.             ++calls;

  667.             // update space dynamics view
  668.             SpacecraftState currentState = stateMapper.mapArrayToState(t, y, null, true);
  669.             currentState = updateAdditionalStates(currentState);

  671.             // compute main state differentials
  672.             return main.computeDerivatives(currentState);

  674.         }

  676.     }

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

  681.         /** Additional equations. */
  682.         private final AdditionalEquations equations;

  684.         /** Dimension of the additional state. */
  685.         private final int dimension;

  687.         /** Simple constructor.
  688.          * @param equations additional equations
  689.          * @param dimension dimension of the additional state
  690.          */
  691.         ConvertedSecondaryStateEquations(final AdditionalEquations equations,
  692.                                          final int dimension) {
  693.             this.equations = equations;
  694.             this.dimension = dimension;
  695.         }

  697.         /** {@inheritDoc} */
  698.         @Override
  699.         public int getDimension() {
  700.             return dimension;
  701.         }

  703.         /** {@inheritDoc} */
  704.         @Override
  705.         public void init(final double t0, final double[] primary0,
  706.                          final double[] secondary0, final double finalTime) {
  707.             // update space dynamics view
  708.             SpacecraftState initialState = stateMapper.mapArrayToState(t0, primary0, null, true);
  709.             initialState = updateAdditionalStates(initialState);
  710.             initialState = initialState.addAdditionalState(equations.getName(), secondary0);
  711.             final AbsoluteDate target = stateMapper.mapDoubleToDate(finalTime);
  712.             equations.init(initialState, target);

  714.         }

  716.         /** {@inheritDoc} */
  717.         @Override
  718.         public double[] computeDerivatives(final double t, final double[] primary,
  719.                                            final double[] primaryDot, final double[] secondary) {

  721.             // update space dynamics view
  722.             SpacecraftState currentState = stateMapper.mapArrayToState(t, primary, primaryDot, true);
  723.             currentState = updateAdditionalStates(currentState);
  724.             currentState = currentState.addAdditionalState(equations.getName(), secondary);

  726.             // compute additional derivatives
  727.             final double[] secondaryDot = new double[secondary.length];
  728.             final double[] additionalMainDot =
  729.                     equations.computeDerivatives(currentState, secondaryDot);
  730.             if (additionalMainDot != null) {
  731.                 // the additional equations have an effect on main equations
  732.                 for (int i = 0; i < additionalMainDot.length; ++i) {
  733.                     primaryDot[i] += additionalMainDot[i];
  734.                 }
  735.             }
  736.             return secondaryDot;

  738.         }

  740.     }

  742.     /** Adapt an {@link org.orekit.propagation.events.EventDetector}
  743.      * to Hipparchus {@link org.hipparchus.ode.events.ODEEventHandler} interface.
  744.      * @param <T> class type for the generic version
  745.      * @author Fabien Maussion
  746.      */
  747.     private class AdaptedEventDetector implements ODEEventHandler {

  749.         /** Underlying event detector. */
  750.         private final EventDetector detector;

  752.         /** Time of the previous call to g. */
  753.         private double lastT;

  755.         /** Value from the previous call to g. */
  756.         private double lastG;

  758.         /** Build a wrapped event detector.
  759.          * @param detector event detector to wrap
  760.         */
  761.         AdaptedEventDetector(final EventDetector detector) {
  762.             this.detector = detector;
  763.             this.lastT    = Double.NaN;
  764.             this.lastG    = Double.NaN;
  765.         }

  767.         /** {@inheritDoc} */
  768.         public void init(final ODEStateAndDerivative s0, final double t) {
  769.             detector.init(getCompleteState(s0.getTime(), s0.getCompleteState(), s0.getCompleteDerivative()),
  770.                           stateMapper.mapDoubleToDate(t));
  771.             this.lastT = Double.NaN;
  772.             this.lastG = Double.NaN;
  773.         }

  775.         /** {@inheritDoc} */
  776.         public double g(final ODEStateAndDerivative s) {
  777.             if (!Precision.equals(lastT, s.getTime(), 0)) {
  778.                 lastT = s.getTime();
  779.                 lastG = detector.g(getCompleteState(s.getTime(), s.getCompleteState(), s.getCompleteDerivative()));
  780.             }
  781.             return lastG;
  782.         }

  784.         /** {@inheritDoc} */
  785.         public Action eventOccurred(final ODEStateAndDerivative s, final boolean increasing) {
  786.             final EventHandler.Action whatNext = detector.eventOccurred(getCompleteState(s.getTime(),
  787.                                                                                          s.getCompleteState(),
  788.                                                                                          s.getCompleteDerivative()),
  789.                                                                         increasing);

  791.             switch (whatNext) {
  792.                 case STOP :
  793.                     return Action.STOP;
  794.                 case RESET_STATE :
  795.                     return Action.RESET_STATE;
  796.                 case RESET_DERIVATIVES :
  797.                     return Action.RESET_DERIVATIVES;
  798.                 default :
  799.                     return Action.CONTINUE;
  800.             }
  801.         }

  803.         /** {@inheritDoc} */
  804.         public ODEState resetState(final ODEStateAndDerivative s) {

  806.             final SpacecraftState oldState = getCompleteState(s.getTime(), s.getCompleteState(), s.getCompleteDerivative());
  807.             final SpacecraftState newState = detector.resetState(oldState);

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

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

  819.             return new ODEState(newState.getDate().durationFrom(getStartDate()),
  820.                                 primary, secondary);

  822.         }

  824.     }

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

  832.         /** Underlying handler. */
  833.         private final OrekitStepHandler handler;

  835.         /** Flag for handler . */
  836.         private boolean activate;

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

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

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

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

  866.     }

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

  874.         /** Underlying raw rawInterpolator. */
  875.         private final ODEStateInterpolator mathInterpolator;

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

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

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

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

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

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

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

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

  931.             return s;

  933.         }

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

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

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

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

  957.         }

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

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

  979.     }

  981.     private class EphemerisModeHandler implements ModeHandler, ODEStepHandler {

  983.         /** Underlying raw mathematical model. */
  984.         private DenseOutputModel model;

  986.         /** Generated ephemeris. */
  987.         private BoundedPropagator ephemeris;

  989.         /** Flag for handler . */
  990.         private boolean activate;

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

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

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

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

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

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

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

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

  1041.                 model.handleStep(interpolator, isLast);
  1042.                 if (isLast) {

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

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

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

  1078.                     // create the ephemeris
  1079.                     ephemeris = new IntegratedEphemeris(startDate, minDate, maxDate,
  1080.                                                         stateMapper, meanOrbit, model, unmanaged,
  1081.                                                         getAdditionalStateProviders(), names);

  1083.                 }
  1084.             }
  1085.         }

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

  1095.     }

  1097. }
Created with JaCoCo 0.8.2.201808211720