FieldAbstractIntegratedPropagator.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.Field;
  27. import org.hipparchus.RealFieldElement;
  28. import org.hipparchus.exception.MathIllegalArgumentException;
  29. import org.hipparchus.exception.MathIllegalStateException;
  30. import org.hipparchus.ode.FieldDenseOutputModel;
  31. import org.hipparchus.ode.FieldEquationsMapper;
  32. import org.hipparchus.ode.FieldExpandableODE;
  33. import org.hipparchus.ode.FieldODEIntegrator;
  34. import org.hipparchus.ode.FieldODEState;
  35. import org.hipparchus.ode.FieldODEStateAndDerivative;
  36. import org.hipparchus.ode.FieldOrdinaryDifferentialEquation;
  37. import org.hipparchus.ode.FieldSecondaryODE;
  38. import org.hipparchus.ode.events.Action;
  39. import org.hipparchus.ode.events.FieldODEEventHandler;
  40. import org.hipparchus.ode.sampling.FieldODEStateInterpolator;
  41. import org.hipparchus.ode.sampling.FieldODEStepHandler;
  42. import org.hipparchus.util.MathArrays;
  43. import org.hipparchus.util.Precision;
  44. import org.orekit.attitudes.AttitudeProvider;
  45. import org.orekit.errors.OrekitException;
  46. import org.orekit.errors.OrekitIllegalStateException;
  47. import org.orekit.errors.OrekitMessages;
  48. import org.orekit.frames.Frame;
  49. import org.orekit.orbits.FieldOrbit;
  50. import org.orekit.orbits.OrbitType;
  51. import org.orekit.orbits.PositionAngle;
  52. import org.orekit.propagation.FieldAbstractPropagator;
  53. import org.orekit.propagation.FieldBoundedPropagator;
  54. import org.orekit.propagation.FieldSpacecraftState;
  55. import org.orekit.propagation.events.FieldEventDetector;
  56. import org.orekit.propagation.events.handlers.FieldEventHandler;
  57. import org.orekit.propagation.sampling.FieldOrekitStepHandler;
  58. import org.orekit.propagation.sampling.FieldOrekitStepInterpolator;
  59. import org.orekit.time.FieldAbsoluteDate;


  62. /** Common handling of {@link org.orekit.propagation.FieldPropagator FieldPropagator}
  63.  *  methods for both numerical and semi-analytical propagators.
  64.  *  @author Luc Maisonobe
  65.  */
  66. public abstract class FieldAbstractIntegratedPropagator<T extends RealFieldElement<T>> extends FieldAbstractPropagator<T> {

  68.     /** Event detectors not related to force models. */
  69.     private final List<FieldEventDetector<T>> detectors;

  71.     /** Integrator selected by the user for the orbital extrapolation process. */
  72.     private final FieldODEIntegrator<T> integrator;

  74.     /** Mode handler. */
  75.     private FieldModeHandler<T> modeHandler;

  77.     /** Additional equations. */
  78.     private List<FieldAdditionalEquations<T>> additionalEquations;

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

  83.     /** Mapper between raw double components and space flight dynamics objects. */
  84.     private FieldStateMapper<T> stateMapper;

  86.     /** Equations mapper. */
  87.     private FieldEquationsMapper<T> equationsMapper;

  89.     /** Underlying raw rawInterpolator. */
  90.     private FieldODEStateInterpolator<T> mathInterpolator;

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

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

  103.     /** Build a new instance.
  104.      * @param integrator numerical integrator to use for propagation.
  105.      * @param meanOrbit output only the mean orbit.
  106.      * @param field Field used by default
  107.      */
  108.     protected FieldAbstractIntegratedPropagator(final Field<T> field, final FieldODEIntegrator<T> integrator, final boolean meanOrbit) {
  109.         super(field);
  110.         detectors           = new ArrayList<FieldEventDetector<T>>();
  111.         additionalEquations = new ArrayList<FieldAdditionalEquations<T>>();
  112.         this.integrator     = integrator;
  113.         this.meanOrbit      = meanOrbit;
  114.         this.resetAtEnd     = true;
  115.     }

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

  133.     /** Initialize the mapper. */
  134.     protected void initMapper() {
  135.         stateMapper = createMapper(null, Double.NaN, null, null, null, null);
  136.     }

  138.     /**  {@inheritDoc} */
  139.     public void setAttitudeProvider(final AttitudeProvider attitudeProvider) {
  140.         super.setAttitudeProvider(attitudeProvider);
  141.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  142.                                    stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  143.                                    attitudeProvider, stateMapper.getFrame());
  144.     }

  146.     /** Set propagation orbit type.
  147.      * @param orbitType orbit type to use for propagation
  148.      */
  149.     protected void setOrbitType(final OrbitType orbitType) {
  150.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  151.                                    orbitType, stateMapper.getPositionAngleType(),
  152.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  153.     }

  155.     /** Get propagation parameter type.
  156.      * @return orbit type used for propagation
  157.      */
  158.     protected OrbitType getOrbitType() {
  159.         return stateMapper.getOrbitType();
  160.     }

  162.     /** Check if only the mean elements should be used in a semianalitical propagation.
  163.      * @return true if only mean elements have to be used
  164.      */
  165.     protected boolean isMeanOrbit() {
  166.         return meanOrbit;
  167.     }

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

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

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

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

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

  217.     /** {@inheritDoc} */
  218.     @Override
  219.     public boolean isAdditionalStateManaged(final String name) {

  221.         // first look at already integrated states
  222.         if (super.isAdditionalStateManaged(name)) {
  223.             return true;
  224.         }

  226.         // then look at states we integrate ourselves
  227.         for (final FieldAdditionalEquations<T> equation : additionalEquations) {
  228.             if (equation.getName().equals(name)) {
  229.                 return true;
  230.             }
  231.         }

  233.         return false;
  234.     }

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

  248.     /** Add a set of user-specified equations to be integrated along with the orbit propagation.
  249.      * @param additional additional equations
  250.      */
  251.     public void addAdditionalEquations(final FieldAdditionalEquations<T> additional) {

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

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

  263.     }

  265.     /** {@inheritDoc} */
  266.     public <D extends FieldEventDetector<T>> void addEventDetector(final D detector) {
  267.         detectors.add(detector);
  268.     }

  270.     /** {@inheritDoc} */
  271.     public Collection<FieldEventDetector<T>> getEventsDetectors() {
  272.         return Collections.unmodifiableCollection(detectors);
  273.     }

  275.     /** {@inheritDoc} */
  276.     public void clearEventsDetectors() {
  277.         detectors.clear();
  278.     }

  280.     /** Set up all user defined event detectors.
  281.      */
  282.     protected void setUpUserEventDetectors() {
  283.         for (final FieldEventDetector<T> detector : detectors) {
  284.             setUpEventDetector(integrator, detector);
  285.         }
  286.     }

  288.     /** Wrap an Orekit event detector and register it to the integrator.
  289.      * @param integ integrator into which event detector should be registered
  290.      * @param detector event detector to wrap
  291.      */
  292.     protected void setUpEventDetector(final FieldODEIntegrator<T> integ, final FieldEventDetector<T> detector) {
  293.         integ.addEventHandler(new FieldAdaptedEventDetector(detector),
  294.                               detector.getMaxCheckInterval().getReal(),
  295.                               detector.getThreshold().getReal(),
  296.                               detector.getMaxIterationCount());
  297.     }

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

  313.     /** {@inheritDoc}
  314.      * <p>Note that this method has the side effect of replacing the step handlers
  315.      * of the underlying integrator set up in the {@link
  316.      * #FieldAbstractIntegratedPropagator(Field, FieldODEIntegrator, boolean) constructor}. So if a specific
  317.      * step handler is needed, it should be added after this method has been called.</p>
  318.      */
  319.     public void setMasterMode(final FieldOrekitStepHandler<T> handler) {
  320.         super.setMasterMode(handler);
  321.         integrator.clearStepHandlers();
  322.         final FieldAdaptedStepHandler wrapped = new FieldAdaptedStepHandler(handler);
  323.         integrator.addStepHandler(wrapped);
  324.         modeHandler = wrapped;
  325.     }

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

  341.     /** {@inheritDoc} */
  342.     public FieldBoundedPropagator<T> getGeneratedEphemeris()
  343.         throws IllegalStateException {
  344.         if (getMode() != EPHEMERIS_GENERATION_MODE) {
  345.             throw new OrekitIllegalStateException(OrekitMessages.PROPAGATOR_NOT_IN_EPHEMERIS_GENERATION_MODE);
  346.         }
  347.         return ((FieldEphemerisModeHandler) modeHandler).getEphemeris();
  348.     }

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

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

  376.     /** {@inheritDoc} */
  377.     public FieldSpacecraftState<T> propagate(final FieldAbsoluteDate<T> target) {
  378.         try {
  379.             if (getStartDate() == null) {
  380.                 if (getInitialState() == null) {
  381.                     throw new OrekitException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
  382.                 }
  383.                 setStartDate(getInitialState().getDate());
  384.             }
  385.             return propagate(getStartDate(), target);
  386.         } catch (OrekitException oe) {

  388.             // recover a possible embedded OrekitException
  389.             for (Throwable t = oe; t != null; t = t.getCause()) {
  390.                 if (t instanceof OrekitException) {
  391.                     throw (OrekitException) t;
  392.                 }
  393.             }
  394.             throw new OrekitException(oe);

  396.         }
  397.     }

  399.     /** {@inheritDoc} */
  400.     public FieldSpacecraftState<T> propagate(final FieldAbsoluteDate<T> tStart, final FieldAbsoluteDate<T> tEnd) {
  401.         try {

  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.         } catch (OrekitException oe) {

  418.             // recover a possible embedded OrekitException
  419.             for (Throwable t = oe; t != null; t = t.getCause()) {
  420.                 if (t instanceof OrekitException) {
  421.                     throw (OrekitException) t;
  422.                 }
  423.             }
  424.             throw new OrekitException(oe);

  426.         }
  427.     }

  429.     /** Propagation with or without event detection.
  430.      * @param tEnd target date to which orbit should be propagated
  431.      * @param activateHandlers if true, step and event handlers should be activated
  432.      * @return state at end of propagation
  433.      */
  434.     protected FieldSpacecraftState<T> propagate(final FieldAbsoluteDate<T> tEnd, final boolean activateHandlers) {
  435.         try {

  437.             if (getInitialState().getDate().equals(tEnd)) {
  438.                 // don't extrapolate
  439.                 return getInitialState();
  440.             }
  441.             // space dynamics view
  442.             stateMapper = createMapper(getInitialState().getDate(), stateMapper.getMu(),
  443.                                        stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  444.                                        stateMapper.getAttitudeProvider(), getInitialState().getFrame());


  447.             // set propagation orbit type
  448.             final FieldOrbit<T> initialOrbit = stateMapper.getOrbitType().convertType(getInitialState().getOrbit());
  449.             if (Double.isNaN(getMu())) {
  450.                 setMu(initialOrbit.getMu());
  451.             }
  452.             if (getInitialState().getMass().getReal() <= 0.0) {
  453.                 throw new OrekitException(OrekitMessages.SPACECRAFT_MASS_BECOMES_NEGATIVE,
  454.                                                getInitialState().getMass());
  455.             }

  457.             integrator.clearEventHandlers();
  458.             // set up events added by user
  459.             setUpUserEventDetectors();

  461.             // convert space flight dynamics API to math API
  462.             final FieldODEState<T> mathInitialState = createInitialState(getInitialIntegrationState());
  463.             final FieldExpandableODE<T> mathODE = createODE(integrator, mathInitialState);
  464.             equationsMapper = mathODE.getMapper();
  465.             mathInterpolator = null;
  466.             // initialize mode handler
  467.             if (modeHandler != null) {
  468.                 modeHandler.initialize(activateHandlers, tEnd);
  469.             }
  470.             // mathematical integration
  471.             final FieldODEStateAndDerivative<T> mathFinalState;

  473.             beforeIntegration(getInitialState(), tEnd);
  474.             mathFinalState = integrator.integrate(mathODE, mathInitialState,
  475.                                                   tEnd.durationFrom(getInitialState().getDate()));

  477.             afterIntegration();

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

  497.             return finalState;

  499.         } catch (OrekitException pe) {
  500.             throw pe;
  501.         } catch (MathIllegalArgumentException miae) {
  502.             throw OrekitException.unwrap(miae);
  503.         } catch (MathIllegalStateException mise) {
  504.             throw OrekitException.unwrap(mise);
  505.         }
  506.     }

  508.     /** Get the initial state for integration.
  509.      * @return initial state for integration
  510.      */
  511.     protected FieldSpacecraftState<T> getInitialIntegrationState() {
  512.         return getInitialState();
  513.     }

  515.     /** Create an initial state.
  516.      * @param initialState initial state in flight dynamics world
  517.      * @return initial state in mathematics world
  518.      */
  519.     private FieldODEState<T> createInitialState(final FieldSpacecraftState<T> initialState) {

  521.         // retrieve initial state
  522.         final T[] primary  = MathArrays.buildArray(initialState.getA().getField(), getBasicDimension());
  523.         stateMapper.mapStateToArray(initialState, primary, null);

  525.         // secondary part of the ODE
  526.         final T[][] secondary = MathArrays.buildArray(initialState.getA().getField(), additionalEquations.size(), -1);
  527.         for (int i = 0; i < additionalEquations.size(); ++i) {
  528.             final FieldAdditionalEquations<T> additional = additionalEquations.get(i);
  529.             final T[] addState = getInitialState().getAdditionalState(additional.getName());
  530.             secondary[i] = MathArrays.buildArray(initialState.getA().getField(), addState.length);
  531.             for (int j = 0; j < addState.length; j++) {
  532.                 secondary[i][j] = addState[j];
  533.             }
  534.         }

  536.         return new FieldODEState<>(initialState.getA().getField().getZero(), primary, secondary);

  538.     }

  540.     /** Create an ODE with all equations.
  541.      * @param integ numerical integrator to use for propagation.
  542.      * @param mathInitialState initial state
  543.      * @return a new ode
  544.      */
  545.     private FieldExpandableODE<T> createODE(final FieldODEIntegrator<T> integ,
  546.                                     final FieldODEState<T> mathInitialState) {

  548.         final FieldExpandableODE<T> ode =
  549.                 new FieldExpandableODE<>(new ConvertedMainStateEquations(getMainStateEquations(integ)));

  551.         // secondary part of the ODE
  552.         for (int i = 0; i < additionalEquations.size(); ++i) {
  553.             final FieldAdditionalEquations<T> additional = additionalEquations.get(i);
  554.             final FieldSecondaryODE<T> secondary =
  555.                     new ConvertedSecondaryStateEquations(additional,
  556.                                                          mathInitialState.getSecondaryStateDimension(i + 1));
  557.             ode.addSecondaryEquations(secondary);
  558.         }

  560.         return ode;

  562.     }

  564.     /** Method called just before integration.
  565.      * <p>
  566.      * The default implementation does nothing, it may be specialized in subclasses.
  567.      * </p>
  568.      * @param initialState initial state
  569.      * @param tEnd target date at which state should be propagated
  570.      */
  571.     protected void beforeIntegration(final FieldSpacecraftState<T> initialState,
  572.                                      final FieldAbsoluteDate<T> tEnd) {
  573.         // do nothing by default
  574.     }

  576.     /** Method called just after integration.
  577.      * <p>
  578.      * The default implementation does nothing, it may be specialized in subclasses.
  579.      * </p>
  580.      */
  581.     protected void afterIntegration() {
  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 FieldODEIntegrator<T> 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 ts array containing the current value of the state vector
  603.      * @param tsDot array containing the current value of the state vector derivative
  604.      * @return complete state
  605.      */
  606.     private FieldSpacecraftState<T> getCompleteState(final T t, final T[] ts, final T[] tsDot) {

  608.         // main state
  609.         FieldSpacecraftState<T> state = stateMapper.mapArrayToState(t, ts, tsDot, true);  //not sure of the mean orbit, should be true
  610.         // pre-integrated additional states
  611.         state = updateAdditionalStates(state);

  613.         // additional states integrated here
  614.         if (!additionalEquations.isEmpty()) {

  616.             for (int i = 0; i < additionalEquations.size(); ++i) {
  617.                 state = state.addAdditionalState(additionalEquations.get(i).getName(),
  618.                                                  equationsMapper.extractEquationData(i + 1, ts));
  619.             }

  621.         }

  623.         return state;

  625.     }

  627.     /** Differential equations for the main state (orbit, attitude and mass). */
  628.     public interface MainStateEquations<T extends RealFieldElement<T>> {

  630.         /**
  631.          * Initialize the equations at the start of propagation. This method will be
  632.          * called before any calls to {@link #computeDerivatives(FieldSpacecraftState)}.
  633.          *
  634.          * <p> The default implementation of this method does nothing.
  635.          *
  636.          * @param initialState initial state information at the start of propagation.
  637.          * @param target       date of propagation. Not equal to {@code
  638.          *                     initialState.getDate()}.
  639.          */
  640.         void init(FieldSpacecraftState<T> initialState, FieldAbsoluteDate<T> target);

  642.         /** Compute differential equations for main state.
  643.          * @param state current state
  644.          * @return derivatives of main state
  645.          */
  646.         T[] computeDerivatives(FieldSpacecraftState<T> state);

  648.     }

  650.     /** Differential equations for the main state (orbit, attitude and mass), with converted API. */
  651.     private class ConvertedMainStateEquations implements FieldOrdinaryDifferentialEquation<T> {

  653.         /** Main state equations. */
  654.         private final MainStateEquations<T> main;

  656.         /** Simple constructor.
  657.          * @param main main state equations
  658.          */
  659.         ConvertedMainStateEquations(final MainStateEquations<T> main) {
  660.             this.main = main;
  661.             calls = 0;
  662.         }

  664.         /** {@inheritDoc} */
  665.         public int getDimension() {
  666.             return getBasicDimension();
  667.         }

  669.         @Override
  670.         public void init(final T t0, final T[] y0, final T finalTime) {
  671.             // update space dynamics view
  672.             FieldSpacecraftState<T> initialState = stateMapper.mapArrayToState(t0, y0, null, true);
  673.             initialState = updateAdditionalStates(initialState);
  674.             final FieldAbsoluteDate<T> target = stateMapper.mapDoubleToDate(finalTime);
  675.             main.init(initialState, target);
  676.         }
  677.         /** {@inheritDoc} */
  678.         public T[] computeDerivatives(final T t, final T[] y) {

  680.             // increment calls counter
  681.             ++calls;

  683.             // update space dynamics view
  684.             FieldSpacecraftState<T> currentState = stateMapper.mapArrayToState(t, y, null, true);
  685.             currentState = updateAdditionalStates(currentState);

  687.             // compute main state differentials
  688.             return main.computeDerivatives(currentState);

  690.         }

  692.     }

  694.     /** Differential equations for the secondary state (Jacobians, user variables ...), with converted API. */
  695.     private class ConvertedSecondaryStateEquations implements FieldSecondaryODE<T> {

  697.         /** Additional equations. */
  698.         private final FieldAdditionalEquations<T> equations;

  700.         /** Dimension of the additional state. */
  701.         private final int dimension;

  703.         /** Simple constructor.
  704.          * @param equations additional equations
  705.          * @param dimension dimension of the additional state
  706.          */
  707.         ConvertedSecondaryStateEquations(final FieldAdditionalEquations<T> equations,
  708.                                          final int dimension) {
  709.             this.equations = equations;
  710.             this.dimension = dimension;
  711.         }

  713.         /** {@inheritDoc} */
  714.         @Override
  715.         public int getDimension() {
  716.             return dimension;
  717.         }

  719.         /** {@inheritDoc} */
  720.         @Override
  721.         public void init(final T t0, final T[] primary0,
  722.                          final T[] secondary0, final T finalTime) {
  723.             // update space dynamics view
  724.             FieldSpacecraftState<T> initialState = stateMapper.mapArrayToState(t0, primary0, null, true);
  725.             initialState = updateAdditionalStates(initialState);
  726.             initialState = initialState.addAdditionalState(equations.getName(), secondary0);
  727.             final FieldAbsoluteDate<T> target = stateMapper.mapDoubleToDate(finalTime);
  728.             equations.init(initialState, target);
  729.         }

  731.         /** {@inheritDoc} */
  732.         @Override
  733.         public T[] computeDerivatives(final T t, final T[] primary,
  734.                                       final T[] primaryDot, final T[] secondary) {

  736.             // update space dynamics view
  737.             FieldSpacecraftState<T> currentState = stateMapper.mapArrayToState(t, primary, primaryDot, true);
  738.             currentState = updateAdditionalStates(currentState);
  739.             currentState = currentState.addAdditionalState(equations.getName(), secondary);

  741.             // compute additional derivatives
  742.             final T[] secondaryDot = MathArrays.buildArray(getField(), secondary.length);
  743.             final T[] additionalMainDot =
  744.                     equations.computeDerivatives(currentState, secondaryDot);
  745.             if (additionalMainDot != null) {
  746.                 // the additional equations have an effect on main equations
  747.                 for (int i = 0; i < additionalMainDot.length; ++i) {
  748.                     primaryDot[i] = primaryDot[i].add(additionalMainDot[i]);
  749.                 }
  750.             }
  751.             return secondaryDot;

  753.         }

  755.     }

  757.     /** Adapt an {@link org.orekit.propagation.events.FieldEventDetector<T>}
  758.      * to Hipparchus {@link org.hipparchus.ode.events.FieldODEEventHandler<T>} interface.
  759.      * @param <T> class type for the generic version
  760.      * @author Fabien Maussion
  761.      */
  762.     private class FieldAdaptedEventDetector implements FieldODEEventHandler<T> {

  764.         /** Underlying event detector. */
  765.         private final FieldEventDetector<T> detector;

  767.         /** Time of the previous call to g. */
  768.         private T lastT;

  770.         /** Value from the previous call to g. */
  771.         private T lastG;

  773.         /** Build a wrapped event detector.
  774.          * @param detector event detector to wrap
  775.         */
  776.         FieldAdaptedEventDetector(final FieldEventDetector<T> detector) {
  777.             this.detector = detector;
  778.             this.lastT    = getField().getZero().add(Double.NaN);
  779.             this.lastG    = getField().getZero().add(Double.NaN);
  780.         }

  782.         /** {@inheritDoc} */
  783.         public void init(final FieldODEStateAndDerivative<T> s0, final T t) {
  784.             detector.init(getCompleteState(s0.getTime(), s0.getCompleteState(), s0.getCompleteDerivative()),
  785.                           stateMapper.mapDoubleToDate(t));
  786.             this.lastT = getField().getZero().add(Double.NaN);
  787.             this.lastG = getField().getZero().add(Double.NaN);
  788.         }

  790.         /** {@inheritDoc} */
  791.         public T g(final FieldODEStateAndDerivative<T> s) {
  792.             if (!Precision.equals(lastT.getReal(), s.getTime().getReal(), 0)) {
  793.                 lastT = s.getTime();
  794.                 lastG = detector.g(getCompleteState(s.getTime(), s.getCompleteState(), s.getCompleteDerivative()));
  795.             }
  796.             return lastG;
  797.         }

  799.         /** {@inheritDoc} */
  800.         public Action eventOccurred(final FieldODEStateAndDerivative<T> s, final boolean increasing) {
  801.             final FieldEventHandler.Action whatNext = detector.eventOccurred(getCompleteState(s.getTime(),
  802.                                                                                               s.getCompleteState(),
  803.                                                                                               s.getCompleteDerivative()),
  804.                                                                         increasing);

  806.             switch (whatNext) {
  807.                 case STOP :
  808.                     return Action.STOP;
  809.                 case RESET_STATE :
  810.                     return Action.RESET_STATE;
  811.                 case RESET_DERIVATIVES :
  812.                     return Action.RESET_DERIVATIVES;
  813.                 default :
  814.                     return Action.CONTINUE;
  815.             }
  816.         }

  818.         /** {@inheritDoc} */
  819.         public FieldODEState<T> resetState(final FieldODEStateAndDerivative<T> s) {

  821.             final FieldSpacecraftState<T> oldState = getCompleteState(s.getTime(), s.getCompleteState(), s.getCompleteDerivative());
  822.             final FieldSpacecraftState<T> newState = detector.resetState(oldState);

  824.             // main part
  825.             final T[] primary    = MathArrays.buildArray(getField(), s.getPrimaryStateDimension());
  826.             stateMapper.mapStateToArray(newState, primary, null);

  828.             // secondary part
  829.             final T[][] secondary    = MathArrays.buildArray(getField(), additionalEquations.size(), -1);

  831.             for (int i = 0; i < additionalEquations.size(); ++i) {
  832.                 final FieldAdditionalEquations<T> additional = additionalEquations.get(i);
  833.                 final T[] NState = newState.getAdditionalState(additional.getName());
  834.                 secondary[i] = MathArrays.buildArray(getField(), NState.length);
  835.                 for (int j = 0; j < NState.length; j++)
  836.                     secondary[i][j] = NState[j];
  837.             }

  839.             return new FieldODEState<>(newState.getDate().durationFrom(getStartDate()),
  840.                                        primary, secondary);
  841.         }

  843.     }

  845.     /** Adapt an {@link org.orekit.propagation.sampling.FieldOrekitStepHandler<T>}
  846.      * to Hipparchus {@link FieldODEStepHandler<T>} interface.
  847.      * @author Luc Maisonobe
  848.      */
  849.     private class FieldAdaptedStepHandler
  850.         implements FieldOrekitStepInterpolator<T>, FieldODEStepHandler<T>, FieldModeHandler<T> {

  852.         /** Underlying handler. */
  853.         private final FieldOrekitStepHandler<T> handler;

  855.         /** Flag for handler . */
  856.         private boolean activate;

  858.         /** Build an instance.
  859.          * @param handler underlying handler to wrap
  860.          */
  861.         FieldAdaptedStepHandler(final FieldOrekitStepHandler<T> handler) {
  862.             this.handler = handler;
  863.         }

  865.         /** {@inheritDoc} */
  866.         public void initialize(final boolean activateHandlers,
  867.                                final FieldAbsoluteDate<T> targetDate) {
  868.             this.activate = activateHandlers;
  869.         }
  870.         /** {@inheritDoc} */
  871.         public void init(final FieldODEStateAndDerivative<T> s0, final T t) {
  872.             handler.init(getCompleteState(s0.getTime(), s0.getCompleteState(), s0.getCompleteDerivative()),
  873.                          stateMapper.mapDoubleToDate(t));
  874.         }

  876.         /** {@inheritDoc} */
  877.         public void handleStep(final FieldODEStateInterpolator<T> interpolator, final boolean isLast) {
  878.             mathInterpolator = interpolator;
  879.             if (activate) {
  880.                 handler.handleStep(this, isLast);
  881.             }
  882.         }

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

  890.         /** {@inheritDoc}} */
  891.         @Override
  892.         public FieldSpacecraftState<T> getCurrentState() {
  893.             return convert(mathInterpolator.getCurrentState());
  894.         }

  896.         /** {@inheritDoc}} */
  897.         @Override
  898.         public FieldSpacecraftState<T> getInterpolatedState(final FieldAbsoluteDate<T> date) {
  899.             return convert(mathInterpolator.getInterpolatedState(date.durationFrom(getStartDate())));
  900.         }

  902.         /** Get the interpolated state.
  903.          * @param os mathematical state
  904.          * @return interpolated state at the current interpolation date
  905.                            * the date
  906.          * @see #getInterpolatedDate()
  907.          * @see #setInterpolatedDate(FieldAbsoluteDate<T>)
  908.          */
  909.         private FieldSpacecraftState<T> convert(final FieldODEStateAndDerivative<T> os) {
  910.             try {

  912.                 FieldSpacecraftState<T> s =
  913.                         stateMapper.mapArrayToState(os.getTime(),
  914.                                                     os.getPrimaryState(),
  915.                                                     os.getPrimaryDerivative(),
  916.                                                     meanOrbit);
  917.                 s = updateAdditionalStates(s);
  918.                 for (int i = 0; i < additionalEquations.size(); ++i) {
  919.                     final T[] secondary = os.getSecondaryState(i + 1);
  920.                     s = s.addAdditionalState(additionalEquations.get(i).getName(), secondary);
  921.                 }

  923.                 return s;

  925.             } catch (OrekitException oe) {
  926.                 throw new OrekitException(oe);
  927.             }
  928.         }

  930.         /** Check is integration direction is forward in date.
  931.          * @return true if integration is forward in date
  932.          */
  933.         public boolean isForward() {
  934.             return mathInterpolator.isForward();
  935.         }

  937.     }

  939.     private class FieldEphemerisModeHandler implements FieldModeHandler<T>, FieldODEStepHandler<T> {

  941.         /** Underlying raw mathematical model. */
  942.         private FieldDenseOutputModel<T> model;

  944.         /** Generated ephemeris. */
  945.         private FieldBoundedPropagator<T> ephemeris;

  947.         /** Flag for handler . */
  948.         private boolean activate;

  950.         /** the user supplied end date. Propagation may not end on this date. */
  951.         private FieldAbsoluteDate<T> endDate;

  953.         /** Creates a new instance of FieldEphemerisModeHandler which must be
  954.          *  filled by the propagator.
  955.          */
  956.         FieldEphemerisModeHandler() {
  957.         }

  959.         /** {@inheritDoc} */
  960.         public void initialize(final boolean activateHandlers,
  961.                                final FieldAbsoluteDate<T> targetDate) {
  962.             this.activate = activateHandlers;
  963.             this.model    = new FieldDenseOutputModel<>();
  964.             this.endDate  = targetDate;

  966.             // ephemeris will be generated when last step is processed
  967.             this.ephemeris = null;

  969.         }

  971.         /** Get the generated ephemeris.
  972.          * @return a new instance of the generated ephemeris
  973.          */
  974.         public FieldBoundedPropagator<T> getEphemeris() {
  975.             return ephemeris;
  976.         }

  978.         /** {@inheritDoc} */
  979.         public void handleStep(final FieldODEStateInterpolator<T> interpolator, final boolean isLast) {
  980.             if (activate) {
  981.                 model.handleStep(interpolator, isLast);
  982.                 if (isLast) {

  984.                     // set up the boundary dates
  985.                     final T tI = model.getInitialTime();
  986.                     final T tF = model.getFinalTime();
  987.                     // tI is almost? always zero
  988.                     final FieldAbsoluteDate<T> startDate =
  989.                             stateMapper.mapDoubleToDate(tI);
  990.                     final FieldAbsoluteDate<T> finalDate =
  991.                             stateMapper.mapDoubleToDate(tF, this.endDate);
  992.                     final FieldAbsoluteDate<T> minDate;
  993.                     final FieldAbsoluteDate<T> maxDate;
  994.                     if (tF.getReal() < tI.getReal()) {
  995.                         minDate = finalDate;
  996.                         maxDate = startDate;
  997.                     } else {
  998.                         minDate = startDate;
  999.                         maxDate = finalDate;
  1000.                     }

  1002.                     // get the initial additional states that are not managed
  1003.                     final Map<String, T[]> unmanaged = new HashMap<String, T[]>();
  1004.                     for (final Map.Entry<String, T[]> initial : getInitialState().getAdditionalStates().entrySet()) {
  1005.                         if (!isAdditionalStateManaged(initial.getKey())) {
  1006.                             // this additional state was in the initial state, but is unknown to the propagator
  1007.                             // we simply copy its initial value as is
  1008.                             unmanaged.put(initial.getKey(), initial.getValue());
  1009.                         }
  1010.                     }

  1012.                     // get the names of additional states managed by differential equations
  1013.                     final String[] names = new String[additionalEquations.size()];
  1014.                     for (int i = 0; i < names.length; ++i) {
  1015.                         names[i] = additionalEquations.get(i).getName();
  1016.                     }

  1018.                     // create the ephemeris
  1019.                     ephemeris = new FieldIntegratedEphemeris<>(startDate, minDate, maxDate,
  1020.                                                                stateMapper, meanOrbit, model, unmanaged,
  1021.                                                                getAdditionalStateProviders(), names);

  1023.                 }
  1024.             }
  1025.         }

  1027.         /** {@inheritDoc} */
  1028.         public void init(final FieldODEStateAndDerivative<T> s0, final T t) {
  1029.             model.init(s0, t);
  1030.         }

  1032.     }

  1034. }
Created with JaCoCo 0.8.2.201808211720