AbstractIntegratedPropagator.java

  1. /* Copyright 2002-2018 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.OrekitExceptionWrapper;
  44. import org.orekit.errors.OrekitIllegalStateException;
  45. import org.orekit.errors.OrekitInternalError;
  46. import org.orekit.errors.OrekitMessages;
  47. import org.orekit.frames.Frame;
  48. import org.orekit.orbits.Orbit;
  49. import org.orekit.orbits.OrbitType;
  50. import org.orekit.orbits.PositionAngle;
  51. import org.orekit.propagation.AbstractPropagator;
  52. import org.orekit.propagation.BoundedPropagator;
  53. import org.orekit.propagation.SpacecraftState;
  54. import org.orekit.propagation.events.EventDetector;
  55. import org.orekit.propagation.events.handlers.EventHandler;
  56. import org.orekit.propagation.sampling.OrekitStepHandler;
  57. import org.orekit.propagation.sampling.OrekitStepInterpolator;
  58. import org.orekit.time.AbsoluteDate;


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

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

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

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

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

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

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

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

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

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

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

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

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

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

  140.     /** Set propagation orbit type.
  141.      * @param orbitType orbit type to use for propagation
  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
  151.      */
  152.     protected OrbitType getOrbitType() {
  153.         return stateMapper.getOrbitType();
  154.     }

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

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

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

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

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

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

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

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

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

  227.         return false;
  228.     }

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

  242.     /** Add a set of user-specified equations to be integrated along with the orbit propagation.
  243.      * @param additional additional equations
  244.      * @exception OrekitException if a set of equations with the same name is already present
  245.      */
  246.     public void addAdditionalEquations(final AdditionalEquations additional)
  247.         throws OrekitException {

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

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

  259.     }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  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.      * @exception OrekitException if orbit cannot be propagated
  423.      */
  424.     protected SpacecraftState propagate(final AbsoluteDate tEnd, final boolean activateHandlers)
  425.         throws OrekitException {
  426.         try {

  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.             // set propagation orbit type
  440.             final Orbit initialOrbit = stateMapper.getOrbitType().convertType(getInitialState().getOrbit());
  441.             if (Double.isNaN(getMu())) {
  442.                 setMu(initialOrbit.getMu());
  443.             }

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

  450.             integrator.clearEventHandlers();

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

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

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

  467.             // mathematical integration
  468.             final ODEStateAndDerivative mathFinalState;
  469.             try {
  470.                 beforeIntegration(getInitialState(), tEnd);
  471.                 mathFinalState = integrator.integrate(mathODE, mathInitialState,
  472.                                                       tEnd.durationFrom(getInitialState().getDate()));
  473.                 afterIntegration();
  474.             } catch (OrekitExceptionWrapper oew) {
  475.                 throw oew.getException();
  476.             }

  478.             // get final state
  479.             SpacecraftState finalState =
  480.                             stateMapper.mapArrayToState(stateMapper.mapDoubleToDate(mathFinalState.getTime(),
  481.                                                                                     tEnd),
  482.                                                         mathFinalState.getPrimaryState(),
  483.                                                         mathFinalState.getPrimaryDerivative(),
  484.                                                         meanOrbit);
  485.             finalState = updateAdditionalStates(finalState);
  486.             for (int i = 0; i < additionalEquations.size(); ++i) {
  487.                 final double[] secondary = mathFinalState.getSecondaryState(i + 1);
  488.                 finalState = finalState.addAdditionalState(additionalEquations.get(i).getName(),
  489.                                                            secondary);
  490.             }
  491.             if (resetAtEnd) {
  492.                 resetInitialState(finalState);
  493.                 setStartDate(finalState.getDate());
  494.             }

  496.             return finalState;

  498.         } catch (MathRuntimeException mre) {
  499.             throw OrekitException.unwrap(mre);
  500.         }
  501.     }

  503.     /** Get the initial state for integration.
  504.      * @return initial state for integration
  505.      * @exception OrekitException if initial state cannot be retrieved
  506.      */
  507.     protected SpacecraftState getInitialIntegrationState() throws OrekitException {
  508.         return getInitialState();
  509.     }

  511.     /** Create an initial state.
  512.      * @param initialState initial state in flight dynamics world
  513.      * @return initial state in mathematics world
  514.      * @exception OrekitException if initial state cannot be mapped
  515.      */
  516.     private ODEState createInitialState(final SpacecraftState initialState)
  517.         throws OrekitException {

  519.         // retrieve initial state
  520.         final double[] primary  = new double[getBasicDimension()];
  521.         stateMapper.mapStateToArray(initialState, primary, null);

  523.         // secondary part of the ODE
  524.         final double[][] secondary = new double[additionalEquations.size()][];
  525.         for (int i = 0; i < additionalEquations.size(); ++i) {
  526.             final AdditionalEquations additional = additionalEquations.get(i);
  527.             secondary[i] = initialState.getAdditionalState(additional.getName());
  528.         }

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

  532.     }

  534.     /** Create an ODE with all equations.
  535.      * @param integ numerical integrator to use for propagation.
  536.      * @param mathInitialState initial state
  537.      * @return a new ode
  538.      * @exception OrekitException if initial state cannot be mapped
  539.      */
  540.     private ExpandableODE createODE(final ODEIntegrator integ,
  541.                                     final ODEState mathInitialState)
  542.         throws OrekitException {

  544.         final ExpandableODE ode =
  545.                 new ExpandableODE(new ConvertedMainStateEquations(getMainStateEquations(integ)));

  547.         // secondary part of the ODE
  548.         for (int i = 0; i < additionalEquations.size(); ++i) {
  549.             final AdditionalEquations additional = additionalEquations.get(i);
  550.             final SecondaryODE secondary =
  551.                     new ConvertedSecondaryStateEquations(additional,
  552.                                                          mathInitialState.getSecondaryStateDimension(i + 1));
  553.             ode.addSecondaryEquations(secondary);
  554.         }

  556.         return ode;

  558.     }

  560.     /** Method called just before integration.
  561.      * <p>
  562.      * The default implementation does nothing, it may be specialized in subclasses.
  563.      * </p>
  564.      * @param initialState initial state
  565.      * @param tEnd target date at which state should be propagated
  566.      * @exception OrekitException if hook cannot be run
  567.      */
  568.     protected void beforeIntegration(final SpacecraftState initialState,
  569.                                      final AbsoluteDate tEnd)
  570.         throws OrekitException {
  571.         // do nothing by default
  572.     }

  574.     /** Method called just after integration.
  575.      * <p>
  576.      * The default implementation does nothing, it may be specialized in subclasses.
  577.      * </p>
  578.      * @exception OrekitException if hook cannot be run
  579.      */
  580.     protected void afterIntegration()
  581.         throws OrekitException {
  582.         // do nothing by default
  583.     }

  585.     /** Get state vector dimension without additional parameters.
  586.      * @return state vector dimension without additional parameters.
  587.      */
  588.     public int getBasicDimension() {
  589.         return 7;

  591.     }

  593.     /** Get the integrator used by the propagator.
  594.      * @return the integrator.
  595.      */
  596.     protected ODEIntegrator getIntegrator() {
  597.         return integrator;
  598.     }

  600.     /** Get a complete state with all additional equations.
  601.      * @param t current value of the independent <I>time</I> variable
  602.      * @param y array containing the current value of the state vector
  603.      * @param yDot array containing the current value of the state vector derivative
  604.      * @return complete state
  605.      * @exception OrekitException if state cannot be mapped
  606.      */
  607.     private SpacecraftState getCompleteState(final double t, final double[] y, final double[] yDot)
  608.         throws OrekitException {

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

  613.         // pre-integrated additional states
  614.         state = updateAdditionalStates(state);

  616.         // additional states integrated here
  617.         if (!additionalEquations.isEmpty()) {

  619.             for (int i = 0; i < additionalEquations.size(); ++i) {
  620.                 state = state.addAdditionalState(additionalEquations.get(i).getName(),
  621.                                                  equationsMapper.extractEquationData(i + 1, y));
  622.             }

  624.         }

  626.         return state;

  628.     }

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

  633.         /**
  634.          * Initialize the equations at the start of propagation. This method will be
  635.          * called before any calls to {@link #computeDerivatives(SpacecraftState)}.
  636.          *
  637.          * <p> The default implementation of this method does nothing.
  638.          *
  639.          * @param initialState initial state information at the start of propagation.
  640.          * @param target       date of propagation. Not equal to {@code
  641.          *                     initialState.getDate()}.
  642.          * @throws OrekitException if there is an Orekit related error during
  643.          *                         initialization.
  644.          */
  645.         default void init(final SpacecraftState initialState, final AbsoluteDate target)
  646.             throws OrekitException {
  647.         }

  649.         /** Compute differential equations for main state.
  650.          * @param state current state
  651.          * @return derivatives of main state
  652.          * @throws OrekitException if differentials cannot be computed
  653.          */
  654.         double[] computeDerivatives(SpacecraftState state) throws OrekitException;

  656.     }

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

  661.         /** Main state equations. */
  662.         private final MainStateEquations main;

  664.         /** Simple constructor.
  665.          * @param main main state equations
  666.          */
  667.         ConvertedMainStateEquations(final MainStateEquations main) {
  668.             this.main = main;
  669.             calls = 0;
  670.         }

  672.         /** {@inheritDoc} */
  673.         public int getDimension() {
  674.             return getBasicDimension();
  675.         }

  677.         @Override
  678.         public void init(final double t0, final double[] y0, final double finalTime) {
  679.             try {
  680.                 // update space dynamics view
  681.                 SpacecraftState initialState = stateMapper.mapArrayToState(t0, y0, null, true);
  682.                 initialState = updateAdditionalStates(initialState);
  683.                 final AbsoluteDate target = stateMapper.mapDoubleToDate(finalTime);
  684.                 main.init(initialState, target);
  685.             } catch (OrekitException oe) {
  686.                 throw new OrekitExceptionWrapper(oe);
  687.             }
  688.         }

  690.         /** {@inheritDoc} */
  691.         public double[] computeDerivatives(final double t, final double[] y)
  692.             throws OrekitExceptionWrapper {
  693.             try {

  695.                 // increment calls counter
  696.                 ++calls;

  698.                 // update space dynamics view
  699.                 SpacecraftState currentState = stateMapper.mapArrayToState(t, y, null, true);
  700.                 currentState = updateAdditionalStates(currentState);

  702.                 // compute main state differentials
  703.                 return main.computeDerivatives(currentState);

  705.             } catch (OrekitException oe) {
  706.                 throw new OrekitExceptionWrapper(oe);
  707.             }
  708.         }

  710.     }

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

  715.         /** Additional equations. */
  716.         private final AdditionalEquations equations;

  718.         /** Dimension of the additional state. */
  719.         private final int dimension;

  721.         /** Simple constructor.
  722.          * @param equations additional equations
  723.          * @param dimension dimension of the additional state
  724.          */
  725.         ConvertedSecondaryStateEquations(final AdditionalEquations equations,
  726.                                          final int dimension) {
  727.             this.equations = equations;
  728.             this.dimension = dimension;
  729.         }

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

  737.         /** {@inheritDoc} */
  738.         @Override
  739.         public void init(final double t0, final double[] primary0,
  740.                          final double[] secondary0, final double finalTime) {
  741.             try {
  742.                 // update space dynamics view
  743.                 SpacecraftState initialState = stateMapper.mapArrayToState(t0, primary0, null, true);
  744.                 initialState = updateAdditionalStates(initialState);
  745.                 initialState = initialState.addAdditionalState(equations.getName(), secondary0);
  746.                 final AbsoluteDate target = stateMapper.mapDoubleToDate(finalTime);
  747.                 equations.init(initialState, target);
  748.             } catch (OrekitException oe) {
  749.                 throw new OrekitExceptionWrapper(oe);
  750.             }

  752.         }

  754.         /** {@inheritDoc} */
  755.         @Override
  756.         public double[] computeDerivatives(final double t, final double[] primary,
  757.                                            final double[] primaryDot, final double[] secondary)
  758.             throws OrekitExceptionWrapper {

  760.             try {

  762.                 // update space dynamics view
  763.                 SpacecraftState currentState = stateMapper.mapArrayToState(t, primary, primaryDot, true);
  764.                 currentState = updateAdditionalStates(currentState);
  765.                 currentState = currentState.addAdditionalState(equations.getName(), secondary);

  767.                 // compute additional derivatives
  768.                 final double[] secondaryDot = new double[secondary.length];
  769.                 final double[] additionalMainDot =
  770.                         equations.computeDerivatives(currentState, secondaryDot);
  771.                 if (additionalMainDot != null) {
  772.                     // the additional equations have an effect on main equations
  773.                     for (int i = 0; i < additionalMainDot.length; ++i) {
  774.                         primaryDot[i] += additionalMainDot[i];
  775.                     }
  776.                 }
  777.                 return secondaryDot;

  779.             } catch (OrekitException oe) {
  780.                 throw new OrekitExceptionWrapper(oe);
  781.             }

  783.         }

  785.     }

  787.     /** Adapt an {@link org.orekit.propagation.events.EventDetector}
  788.      * to Hipparchus {@link org.hipparchus.ode.events.ODEEventHandler} interface.
  789.      * @param <T> class type for the generic version
  790.      * @author Fabien Maussion
  791.      */
  792.     private class AdaptedEventDetector implements ODEEventHandler {

  794.         /** Underlying event detector. */
  795.         private final EventDetector detector;

  797.         /** Time of the previous call to g. */
  798.         private double lastT;

  800.         /** Value from the previous call to g. */
  801.         private double lastG;

  803.         /** Build a wrapped event detector.
  804.          * @param detector event detector to wrap
  805.         */
  806.         AdaptedEventDetector(final EventDetector detector) {
  807.             this.detector = detector;
  808.             this.lastT    = Double.NaN;
  809.             this.lastG    = Double.NaN;
  810.         }

  812.         /** {@inheritDoc} */
  813.         public void init(final ODEStateAndDerivative s0, final double t) {
  814.             try {

  816.                 detector.init(getCompleteState(s0.getTime(), s0.getCompleteState(), s0.getCompleteDerivative()),
  817.                               stateMapper.mapDoubleToDate(t));
  818.                 this.lastT = Double.NaN;
  819.                 this.lastG = Double.NaN;

  821.             } catch (OrekitException oe) {
  822.                 throw new OrekitExceptionWrapper(oe);
  823.             }
  824.         }

  826.         /** {@inheritDoc} */
  827.         public double g(final ODEStateAndDerivative s) {
  828.             try {
  829.                 if (!Precision.equals(lastT, s.getTime(), 0)) {
  830.                     lastT = s.getTime();
  831.                     lastG = detector.g(getCompleteState(s.getTime(), s.getCompleteState(), s.getCompleteDerivative()));
  832.                 }
  833.                 return lastG;
  834.             } catch (OrekitException oe) {
  835.                 throw new OrekitExceptionWrapper(oe);
  836.             }
  837.         }

  839.         /** {@inheritDoc} */
  840.         public Action eventOccurred(final ODEStateAndDerivative s, final boolean increasing) {
  841.             try {

  843.                 final EventHandler.Action whatNext = detector.eventOccurred(getCompleteState(s.getTime(),
  844.                                                                                              s.getCompleteState(),
  845.                                                                                              s.getCompleteDerivative()),
  846.                                                                             increasing);

  848.                 switch (whatNext) {
  849.                     case STOP :
  850.                         return Action.STOP;
  851.                     case RESET_STATE :
  852.                         return Action.RESET_STATE;
  853.                     case RESET_DERIVATIVES :
  854.                         return Action.RESET_DERIVATIVES;
  855.                     default :
  856.                         return Action.CONTINUE;
  857.                 }
  858.             } catch (OrekitException oe) {
  859.                 throw new OrekitExceptionWrapper(oe);
  860.             }
  861.         }

  863.         /** {@inheritDoc} */
  864.         public ODEState resetState(final ODEStateAndDerivative s) {
  865.             try {

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

  870.                 // main part
  871.                 final double[] primary    = new double[s.getPrimaryStateDimension()];
  872.                 stateMapper.mapStateToArray(newState, primary, null);

  874.                 // secondary part
  875.                 final double[][] secondary    = new double[additionalEquations.size()][];
  876.                 for (int i = 0; i < additionalEquations.size(); ++i) {
  877.                     secondary[i] = newState.getAdditionalState(additionalEquations.get(i).getName());
  878.                 }

  880.                 return new ODEState(newState.getDate().durationFrom(getStartDate()),
  881.                                     primary, secondary);

  883.             } catch (OrekitException oe) {
  884.                 throw new OrekitExceptionWrapper(oe);
  885.             }
  886.         }

  888.     }

  890.     /** Adapt an {@link org.orekit.propagation.sampling.OrekitStepHandler}
  891.      * to Hipparchus {@link ODEStepHandler} interface.
  892.      * @author Luc Maisonobe
  893.      */
  894.     private class AdaptedStepHandler implements ODEStepHandler, ModeHandler {

  896.         /** Underlying handler. */
  897.         private final OrekitStepHandler handler;

  899.         /** Flag for handler . */
  900.         private boolean activate;

  902.         /** Build an instance.
  903.          * @param handler underlying handler to wrap
  904.          */
  905.         AdaptedStepHandler(final OrekitStepHandler handler) {
  906.             this.handler = handler;
  907.         }

  909.         /** {@inheritDoc} */
  910.         public void initialize(final boolean activateHandlers,
  911.                                final AbsoluteDate targetDate) {
  912.             this.activate = activateHandlers;
  913.         }

  915.         /** {@inheritDoc} */
  916.         public void init(final ODEStateAndDerivative s0, final double t) {
  917.             try {
  918.                 if (activate) {
  919.                     handler.init(getCompleteState(s0.getTime(), s0.getCompleteState(), s0.getCompleteDerivative()),
  920.                                  stateMapper.mapDoubleToDate(t));
  921.                 }
  922.             } catch (OrekitException oe) {
  923.                 throw new OrekitExceptionWrapper(oe);
  924.             }
  925.         }

  927.         /** {@inheritDoc} */
  928.         public void handleStep(final ODEStateInterpolator interpolator, final boolean isLast) {
  929.             try {
  930.                 if (activate) {
  931.                     handler.handleStep(new AdaptedStepInterpolator(interpolator), isLast);
  932.                 }
  933.             } catch (OrekitException pe) {
  934.                 throw new OrekitExceptionWrapper(pe);
  935.             }
  936.         }

  938.     }

  940.     /** Adapt an Hipparchus {@link ODEStateInterpolator}
  941.      * to an orekit {@link OrekitStepInterpolator} interface.
  942.      * @author Luc Maisonobe
  943.      */
  944.     private class AdaptedStepInterpolator implements OrekitStepInterpolator {

  946.         /** Underlying raw rawInterpolator. */
  947.         private final ODEStateInterpolator mathInterpolator;

  949.         /** Simple constructor.
  950.          * @param mathInterpolator underlying raw interpolator
  951.          */
  952.         AdaptedStepInterpolator(final ODEStateInterpolator mathInterpolator) {
  953.             this.mathInterpolator = mathInterpolator;
  954.         }

  956.         /** {@inheritDoc}} */
  957.         @Override
  958.         public SpacecraftState getPreviousState()
  959.             throws OrekitException {
  960.             return convert(mathInterpolator.getPreviousState());
  961.         }

  963.         /** {@inheritDoc}} */
  964.         @Override
  965.         public boolean isPreviousStateInterpolated() {
  966.             return mathInterpolator.isPreviousStateInterpolated();
  967.         }

  969.         /** {@inheritDoc}} */
  970.         @Override
  971.         public SpacecraftState getCurrentState()
  972.             throws OrekitException {
  973.             return convert(mathInterpolator.getCurrentState());
  974.         }

  976.         /** {@inheritDoc}} */
  977.         @Override
  978.         public boolean isCurrentStateInterpolated() {
  979.             return mathInterpolator.isCurrentStateInterpolated();
  980.         }

  982.         /** {@inheritDoc}} */
  983.         @Override
  984.         public SpacecraftState getInterpolatedState(final AbsoluteDate date)
  985.             throws OrekitException {
  986.             return convert(mathInterpolator.getInterpolatedState(date.durationFrom(stateMapper.getReferenceDate())));
  987.         }

  989.         /** Convert a state from mathematical world to space flight dynamics world.
  990.          * @param os mathematical state
  991.          * @return space flight dynamics state
  992.          * @exception OrekitException if arrays are inconsistent
  993.          */
  994.         private SpacecraftState convert(final ODEStateAndDerivative os)
  995.             throws OrekitException {

  997.             SpacecraftState s =
  998.                             stateMapper.mapArrayToState(os.getTime(),
  999.                                                         os.getPrimaryState(),
  1000.                                                         os.getPrimaryDerivative(),
  1001.                                                         meanOrbit);
  1002.             s = updateAdditionalStates(s);
  1003.             for (int i = 0; i < additionalEquations.size(); ++i) {
  1004.                 final double[] secondary = os.getSecondaryState(i + 1);
  1005.                 s = s.addAdditionalState(additionalEquations.get(i).getName(), secondary);
  1006.             }

  1008.             return s;

  1010.         }

  1012.         /** Convert a state from space flight dynamics world to mathematical world.
  1013.          * @param state space flight dynamics state
  1014.          * @return mathematical state
  1015.          * @exception OrekitException if arrays are inconsistent
  1016.          */
  1017.         private ODEStateAndDerivative convert(final SpacecraftState state)
  1018.             throws OrekitException {

  1020.             // retrieve initial state
  1021.             final double[] primary    = new double[getBasicDimension()];
  1022.             final double[] primaryDot = new double[getBasicDimension()];
  1023.             stateMapper.mapStateToArray(state, primary, primaryDot);

  1025.             // secondary part of the ODE
  1026.             final double[][] secondary    = new double[additionalEquations.size()][];
  1027.             for (int i = 0; i < additionalEquations.size(); ++i) {
  1028.                 final AdditionalEquations additional = additionalEquations.get(i);
  1029.                 secondary[i] = state.getAdditionalState(additional.getName());
  1030.             }

  1032.             return new ODEStateAndDerivative(stateMapper.mapDateToDouble(state.getDate()),
  1033.                                              primary, primaryDot,
  1034.                                              secondary, null);

  1036.         }

  1038.         /** {@inheritDoc}} */
  1039.         @Override
  1040.         public boolean isForward() {
  1041.             return mathInterpolator.isForward();
  1042.         }

  1044.         /** {@inheritDoc}} */
  1045.         @Override
  1046.         public AdaptedStepInterpolator restrictStep(final SpacecraftState newPreviousState,
  1047.                                                     final SpacecraftState newCurrentState)
  1048.             throws OrekitException {
  1049.             try {
  1050.                 final AbstractODEStateInterpolator aosi = (AbstractODEStateInterpolator) mathInterpolator;
  1051.                 return new AdaptedStepInterpolator(aosi.restrictStep(convert(newPreviousState),
  1052.                                                                      convert(newCurrentState)));
  1053.             } catch (ClassCastException cce) {
  1054.                 // this should never happen
  1055.                 throw new OrekitInternalError(cce);
  1056.             }
  1057.         }

  1059.     }

  1061.     private class EphemerisModeHandler implements ModeHandler, ODEStepHandler {

  1063.         /** Underlying raw mathematical model. */
  1064.         private DenseOutputModel model;

  1066.         /** Generated ephemeris. */
  1067.         private BoundedPropagator ephemeris;

  1069.         /** Flag for handler . */
  1070.         private boolean activate;

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

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

  1078.         /** Creates a new instance of EphemerisModeHandler which must be
  1079.          *  filled by the propagator.
  1080.          */
  1081.         EphemerisModeHandler() {
  1082.             this.handler = null;
  1083.         }

  1085.         /** Creates a new instance of EphemerisModeHandler which must be
  1086.          *  filled by the propagator.
  1087.          *  @param handler the handler to notify of every integrator step.
  1088.          */
  1089.         EphemerisModeHandler(final OrekitStepHandler handler) {
  1090.             this.handler = new AdaptedStepHandler(handler);
  1091.         }

  1093.         /** {@inheritDoc} */
  1094.         public void initialize(final boolean activateHandlers,
  1095.                                final AbsoluteDate targetDate) {
  1096.             this.activate = activateHandlers;
  1097.             this.model    = new DenseOutputModel();
  1098.             this.endDate  = targetDate;

  1100.             // ephemeris will be generated when last step is processed
  1101.             this.ephemeris = null;
  1102.             if (this.handler != null) {
  1103.                 this.handler.initialize(activateHandlers, targetDate);
  1104.             }
  1105.         }

  1107.         /** Get the generated ephemeris.
  1108.          * @return a new instance of the generated ephemeris
  1109.          */
  1110.         public BoundedPropagator getEphemeris() {
  1111.             return ephemeris;
  1112.         }

  1114.         /** {@inheritDoc} */
  1115.         public void handleStep(final ODEStateInterpolator interpolator, final boolean isLast)
  1116.             throws OrekitExceptionWrapper {
  1117.             try {
  1118.                 if (activate) {
  1119.                     if (this.handler != null) {
  1120.                         this.handler.handleStep(interpolator, isLast);
  1121.                     }

  1123.                     model.handleStep(interpolator, isLast);
  1124.                     if (isLast) {

  1126.                         // set up the boundary dates
  1127.                         final double tI = model.getInitialTime();
  1128.                         final double tF = model.getFinalTime();
  1129.                         // tI is almost? always zero
  1130.                         final AbsoluteDate startDate =
  1131.                                 stateMapper.mapDoubleToDate(tI);
  1132.                         final AbsoluteDate finalDate =
  1133.                                 stateMapper.mapDoubleToDate(tF, this.endDate);
  1134.                         final AbsoluteDate minDate;
  1135.                         final AbsoluteDate maxDate;
  1136.                         if (tF < tI) {
  1137.                             minDate = finalDate;
  1138.                             maxDate = startDate;
  1139.                         } else {
  1140.                             minDate = startDate;
  1141.                             maxDate = finalDate;
  1142.                         }

  1144.                         // get the initial additional states that are not managed
  1145.                         final Map<String, double[]> unmanaged = new HashMap<String, double[]>();
  1146.                         for (final Map.Entry<String, double[]> initial : getInitialState().getAdditionalStates().entrySet()) {
  1147.                             if (!isAdditionalStateManaged(initial.getKey())) {
  1148.                                 // this additional state was in the initial state, but is unknown to the propagator
  1149.                                 // we simply copy its initial value as is
  1150.                                 unmanaged.put(initial.getKey(), initial.getValue());
  1151.                             }
  1152.                         }

  1154.                         // get the names of additional states managed by differential equations
  1155.                         final String[] names = new String[additionalEquations.size()];
  1156.                         for (int i = 0; i < names.length; ++i) {
  1157.                             names[i] = additionalEquations.get(i).getName();
  1158.                         }

  1160.                         // create the ephemeris
  1161.                         ephemeris = new IntegratedEphemeris(startDate, minDate, maxDate,
  1162.                                                             stateMapper, meanOrbit, model, unmanaged,
  1163.                                                             getAdditionalStateProviders(), names);

  1165.                     }
  1166.                 }
  1167.             } catch (OrekitException oe) {
  1168.                 throw new OrekitExceptionWrapper(oe);
  1169.             }
  1170.         }

  1172.         /** {@inheritDoc} */
  1173.         public void init(final ODEStateAndDerivative s0, final double t) {
  1174.             model.init(s0, t);
  1175.             if (this.handler != null) {
  1176.                 this.handler.init(s0, t);
  1177.             }
  1178.         }

  1180.     }

  1182. }
Created with JaCoCo 0.7.9.201702052155