FieldAbstractIntegratedPropagator.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.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.OrekitExceptionWrapper;
  47. import org.orekit.errors.OrekitIllegalStateException;
  48. import org.orekit.errors.OrekitMessages;
  49. import org.orekit.frames.Frame;
  50. import org.orekit.orbits.FieldOrbit;
  51. import org.orekit.orbits.OrbitType;
  52. import org.orekit.orbits.PositionAngle;
  53. import org.orekit.propagation.FieldAbstractPropagator;
  54. import org.orekit.propagation.FieldBoundedPropagator;
  55. import org.orekit.propagation.FieldSpacecraftState;
  56. import org.orekit.propagation.events.FieldEventDetector;
  57. import org.orekit.propagation.events.handlers.FieldEventHandler;
  58. import org.orekit.propagation.sampling.FieldOrekitStepHandler;
  59. import org.orekit.propagation.sampling.FieldOrekitStepInterpolator;
  60. import org.orekit.time.FieldAbsoluteDate;


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  234.         return false;
  235.     }

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

  249.     /** Add a set of user-specified equations to be integrated along with the orbit propagation.
  250.      * @param additional additional equations
  251.      * @exception OrekitException if a set of equations with the same name is already present
  252.      */
  253.     public void addAdditionalEquations(final FieldAdditionalEquations<T> additional)
  254.         throws OrekitException {

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

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

  266.     }

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

  273.     /** {@inheritDoc} */
  274.     public Collection<FieldEventDetector<T>> getEventsDetectors() {
  275.         return Collections.unmodifiableCollection(detectors);
  276.     }

  278.     /** {@inheritDoc} */
  279.     public void clearEventsDetectors() {
  280.         detectors.clear();
  281.     }

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

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

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

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

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

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

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

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

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

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

  399.         }
  400.     }

  402.     /** {@inheritDoc} */
  403.     public FieldSpacecraftState<T> propagate(final FieldAbsoluteDate<T> tStart, final FieldAbsoluteDate<T> tEnd)
  404.         throws OrekitException {
  405.         try {

  407.             if (getInitialState() == null) {
  408.                 throw new OrekitException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
  409.             }

  411.             if (!tStart.equals(getInitialState().getDate())) {
  412.                 // if propagation start date is not initial date,
  413.                 // propagate from initial to start date without event detection
  414.                 propagate(tStart, false);
  415.             }

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

  420.         } catch (OrekitException oe) {

  422.             // recover a possible embedded OrekitException
  423.             for (Throwable t = oe; t != null; t = t.getCause()) {
  424.                 if (t instanceof OrekitException) {
  425.                     throw (OrekitException) t;
  426.                 }
  427.             }
  428.             throw new OrekitException(oe);

  430.         }
  431.     }

  433.     /** Propagation with or without event detection.
  434.      * @param tEnd target date to which orbit should be propagated
  435.      * @param activateHandlers if true, step and event handlers should be activated
  436.      * @return state at end of propagation
  437.      * @exception OrekitException if orbit cannot be propagated
  438.      */
  439.     protected FieldSpacecraftState<T> propagate(final FieldAbsoluteDate<T> tEnd, final boolean activateHandlers)
  440.         throws OrekitException {
  441.         try {

  443.             if (getInitialState().getDate().equals(tEnd)) {
  444.                 // don't extrapolate
  445.                 return getInitialState();
  446.             }
  447.             // space dynamics view
  448.             stateMapper = createMapper(getInitialState().getDate(), stateMapper.getMu(),
  449.                                        stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  450.                                        stateMapper.getAttitudeProvider(), getInitialState().getFrame());


  453.             // set propagation orbit type
  454.             final FieldOrbit<T> initialOrbit = stateMapper.getOrbitType().convertType(getInitialState().getOrbit());
  455.             if (Double.isNaN(getMu())) {
  456.                 setMu(initialOrbit.getMu());
  457.             }
  458.             if (getInitialState().getMass().getReal() <= 0.0) {
  459.                 throw new OrekitException(OrekitMessages.SPACECRAFT_MASS_BECOMES_NEGATIVE,
  460.                                                getInitialState().getMass());
  461.             }

  463.             integrator.clearEventHandlers();
  464.             // set up events added by user
  465.             setUpUserEventDetectors();

  467.             // convert space flight dynamics API to math API
  468.             final FieldODEState<T> mathInitialState = createInitialState(getInitialIntegrationState());
  469.             final FieldExpandableODE<T> mathODE = createODE(integrator, mathInitialState);
  470.             equationsMapper = mathODE.getMapper();
  471.             mathInterpolator = null;
  472.             // initialize mode handler
  473.             if (modeHandler != null) {
  474.                 modeHandler.initialize(activateHandlers, tEnd);
  475.             }
  476.             // mathematical integration
  477.             final FieldODEStateAndDerivative<T> mathFinalState;
  478.             try {

  480.                 beforeIntegration(getInitialState(), tEnd);
  481.                 mathFinalState = integrator.integrate(mathODE, mathInitialState,
  482.                                                       tEnd.durationFrom(getInitialState().getDate()));

  484.                 afterIntegration();
  485.             } catch (OrekitExceptionWrapper oew) {
  486.                 throw oew.getException();
  487.             }
  488.             // get final state
  489.             FieldSpacecraftState<T> finalState =
  490.                             stateMapper.mapArrayToState(stateMapper.mapDoubleToDate(mathFinalState.getTime(),
  491.                                                                                     tEnd),
  492.                                                         mathFinalState.getPrimaryState(),
  493.                                                         mathFinalState.getPrimaryDerivative(),
  494.                                                         meanOrbit);
  495.             finalState = updateAdditionalStates(finalState);
  496.             for (int i = 0; i < additionalEquations.size(); ++i) {
  497.                 final T[] secondary = mathFinalState.getSecondaryState(i + 1);
  498.                 finalState = finalState.addAdditionalState(additionalEquations.get(i).getName(),
  499.                                                            secondary);
  500.             }
  501.             if (resetAtEnd) {
  502.                 resetInitialState(finalState);
  503.                 setStartDate(finalState.getDate());
  504.             }

  506.             return finalState;

  508.         } catch (OrekitException pe) {
  509.             throw pe;
  510.         } catch (MathIllegalArgumentException miae) {
  511.             throw OrekitException.unwrap(miae);
  512.         } catch (MathIllegalStateException mise) {
  513.             throw OrekitException.unwrap(mise);
  514.         }
  515.     }

  517.     /** Get the initial state for integration.
  518.      * @return initial state for integration
  519.      * @exception OrekitException if initial state cannot be retrieved
  520.      */
  521.     protected FieldSpacecraftState<T> getInitialIntegrationState() throws OrekitException {
  522.         return getInitialState();
  523.     }

  525.     /** Create an initial state.
  526.      * @param initialState initial state in flight dynamics world
  527.      * @return initial state in mathematics world
  528.      * @exception OrekitException if initial state cannot be mapped
  529.      */
  530.     private FieldODEState<T> createInitialState(final FieldSpacecraftState<T> initialState)
  531.         throws OrekitException {

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

  537.         // secondary part of the ODE
  538.         final T[][] secondary = MathArrays.buildArray(initialState.getA().getField(), additionalEquations.size(), -1);
  539.         for (int i = 0; i < additionalEquations.size(); ++i) {
  540.             final FieldAdditionalEquations<T> additional = additionalEquations.get(i);
  541.             final T[] addState = getInitialState().getAdditionalState(additional.getName());
  542.             secondary[i] = MathArrays.buildArray(initialState.getA().getField(), addState.length);
  543.             for (int j = 0; j < addState.length; j++) {
  544.                 secondary[i][j] = addState[j];
  545.             }
  546.         }

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

  550.     }

  552.     /** Create an ODE with all equations.
  553.      * @param integ numerical integrator to use for propagation.
  554.      * @param mathInitialState initial state
  555.      * @return a new ode
  556.      * @exception OrekitException if initial state cannot be mapped
  557.      */
  558.     private FieldExpandableODE<T> createODE(final FieldODEIntegrator<T> integ,
  559.                                     final FieldODEState<T> mathInitialState)
  560.         throws OrekitException {

  562.         final FieldExpandableODE<T> ode =
  563.                 new FieldExpandableODE<>(new ConvertedMainStateEquations(getMainStateEquations(integ)));

  565.         // secondary part of the ODE
  566.         for (int i = 0; i < additionalEquations.size(); ++i) {
  567.             final FieldAdditionalEquations<T> additional = additionalEquations.get(i);
  568.             final FieldSecondaryODE<T> secondary =
  569.                     new ConvertedSecondaryStateEquations(additional,
  570.                                                          mathInitialState.getSecondaryStateDimension(i + 1));
  571.             ode.addSecondaryEquations(secondary);
  572.         }

  574.         return ode;

  576.     }

  578.     /** Method called just before integration.
  579.      * <p>
  580.      * The default implementation does nothing, it may be specialized in subclasses.
  581.      * </p>
  582.      * @param initialState initial state
  583.      * @param tEnd target date at which state should be propagated
  584.      * @exception OrekitException if hook cannot be run
  585.      */
  586.     protected void beforeIntegration(final FieldSpacecraftState<T> initialState,
  587.                                      final FieldAbsoluteDate<T> tEnd)
  588.         throws OrekitException {
  589.         // do nothing by default
  590.     }

  592.     /** Method called just after integration.
  593.      * <p>
  594.      * The default implementation does nothing, it may be specialized in subclasses.
  595.      * </p>
  596.      * @exception OrekitException if hook cannot be run
  597.      */
  598.     protected void afterIntegration()
  599.         throws OrekitException {
  600.         // do nothing by default
  601.     }

  603.     /** Get state vector dimension without additional parameters.
  604.      * @return state vector dimension without additional parameters.
  605.      */
  606.     public int getBasicDimension() {
  607.         return 7;

  609.     }

  611.     /** Get the integrator used by the propagator.
  612.      * @return the integrator.
  613.      */
  614.     protected FieldODEIntegrator<T> getIntegrator() {
  615.         return integrator;
  616.     }

  618.     /** Get a complete state with all additional equations.
  619.      * @param t current value of the independent <I>time</I> variable
  620.      * @param ts array containing the current value of the state vector
  621.      * @param tsDot array containing the current value of the state vector derivative
  622.      * @return complete state
  623.      * @exception OrekitException if state cannot be mapped
  624.      */
  625.     private FieldSpacecraftState<T> getCompleteState(final T t, final T[] ts, final T[] tsDot)
  626.         throws OrekitException {

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

  633.         // additional states integrated here
  634.         if (!additionalEquations.isEmpty()) {

  636.             for (int i = 0; i < additionalEquations.size(); ++i) {
  637.                 state = state.addAdditionalState(additionalEquations.get(i).getName(),
  638.                                                  equationsMapper.extractEquationData(i + 1, ts));
  639.             }

  641.         }

  643.         return state;

  645.     }

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

  650.         /**
  651.          * Initialize the equations at the start of propagation. This method will be
  652.          * called before any calls to {@link #computeDerivatives(FieldSpacecraftState)}.
  653.          *
  654.          * <p> The default implementation of this method does nothing.
  655.          *
  656.          * @param initialState initial state information at the start of propagation.
  657.          * @param target       date of propagation. Not equal to {@code
  658.          *                     initialState.getDate()}.
  659.          * @throws OrekitException if there is an Orekit related error during
  660.          *                         initialization.
  661.          */
  662.         void init(FieldSpacecraftState<T> initialState, FieldAbsoluteDate<T> target)
  663.             throws OrekitException;

  665.         /** Compute differential equations for main state.
  666.          * @param state current state
  667.          * @return derivatives of main state
  668.          * @throws OrekitException if differentials cannot be computed
  669.          */
  670.         T[] computeDerivatives(FieldSpacecraftState<T> state) throws OrekitException;

  672.     }

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

  677.         /** Main state equations. */
  678.         private final MainStateEquations<T> main;

  680.         /** Simple constructor.
  681.          * @param main main state equations
  682.          */
  683.         ConvertedMainStateEquations(final MainStateEquations<T> main) {
  684.             this.main = main;
  685.             calls = 0;
  686.         }

  688.         /** {@inheritDoc} */
  689.         public int getDimension() {
  690.             return getBasicDimension();
  691.         }

  693.         @Override
  694.         public void init(final T t0, final T[] y0, final T finalTime) {
  695.             try {
  696.                 // update space dynamics view
  697.                 FieldSpacecraftState<T> initialState = stateMapper.mapArrayToState(t0, y0, null, true);
  698.                 initialState = updateAdditionalStates(initialState);
  699.                 final FieldAbsoluteDate<T> target = stateMapper.mapDoubleToDate(finalTime);
  700.                 main.init(initialState, target);
  701.             } catch (OrekitException oe) {
  702.                 throw new OrekitExceptionWrapper(oe);
  703.             }
  704.         }
  705.         /** {@inheritDoc} */
  706.         public T[] computeDerivatives(final T t, final T[] y)
  707.             throws OrekitExceptionWrapper {
  708.             try {

  710.                 // increment calls counter
  711.                 ++calls;

  713.                 // update space dynamics view
  714.                 FieldSpacecraftState<T> currentState = stateMapper.mapArrayToState(t, y, null, true);
  715.                 currentState = updateAdditionalStates(currentState);

  717.                 // compute main state differentials
  718.                 return main.computeDerivatives(currentState);

  720.             } catch (OrekitException oe) {
  721.                 throw new OrekitExceptionWrapper(oe);
  722.             }
  723.         }

  725.     }

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

  730.         /** Additional equations. */
  731.         private final FieldAdditionalEquations<T> equations;

  733.         /** Dimension of the additional state. */
  734.         private final int dimension;

  736.         /** Simple constructor.
  737.          * @param equations additional equations
  738.          * @param dimension dimension of the additional state
  739.          */
  740.         ConvertedSecondaryStateEquations(final FieldAdditionalEquations<T> equations,
  741.                                          final int dimension) {
  742.             this.equations = equations;
  743.             this.dimension = dimension;
  744.         }

  746.         /** {@inheritDoc} */
  747.         @Override
  748.         public int getDimension() {
  749.             return dimension;
  750.         }

  752.         /** {@inheritDoc} */
  753.         @Override
  754.         public void init(final T t0, final T[] primary0,
  755.                          final T[] secondary0, final T finalTime) {
  756.             try {
  757.                 // update space dynamics view
  758.                 FieldSpacecraftState<T> initialState = stateMapper.mapArrayToState(t0, primary0, null, true);
  759.                 initialState = updateAdditionalStates(initialState);
  760.                 initialState = initialState.addAdditionalState(equations.getName(), secondary0);
  761.                 final FieldAbsoluteDate<T> target = stateMapper.mapDoubleToDate(finalTime);
  762.                 equations.init(initialState, target);
  763.             } catch (OrekitException oe) {
  764.                 throw new OrekitExceptionWrapper(oe);
  765.             }

  767.         }

  769.         /** {@inheritDoc} */
  770.         @Override
  771.         public T[] computeDerivatives(final T t, final T[] primary,
  772.                                       final T[] primaryDot, final T[] secondary)
  773.             throws OrekitExceptionWrapper {

  775.             try {

  777.                 // update space dynamics view
  778.                 FieldSpacecraftState<T> currentState = stateMapper.mapArrayToState(t, primary, primaryDot, true);
  779.                 currentState = updateAdditionalStates(currentState);
  780.                 currentState = currentState.addAdditionalState(equations.getName(), secondary);

  782.                 // compute additional derivatives
  783.                 final T[] secondaryDot = MathArrays.buildArray(getField(), secondary.length);
  784.                 final T[] additionalMainDot =
  785.                         equations.computeDerivatives(currentState, secondaryDot);
  786.                 if (additionalMainDot != null) {
  787.                     // the additional equations have an effect on main equations
  788.                     for (int i = 0; i < additionalMainDot.length; ++i) {
  789.                         primaryDot[i] = primaryDot[i].add(additionalMainDot[i]);
  790.                     }
  791.                 }
  792.                 return secondaryDot;

  794.             } catch (OrekitException oe) {
  795.                 throw new OrekitExceptionWrapper(oe);
  796.             }

  798.         }

  800.     }

  802.     /** Adapt an {@link org.orekit.propagation.events.FieldEventDetector<T>}
  803.      * to Hipparchus {@link org.hipparchus.ode.events.FieldODEEventHandler<T>} interface.
  804.      * @param <T> class type for the generic version
  805.      * @author Fabien Maussion
  806.      */
  807.     private class FieldAdaptedEventDetector implements FieldODEEventHandler<T> {

  809.         /** Underlying event detector. */
  810.         private final FieldEventDetector<T> detector;

  812.         /** Time of the previous call to g. */
  813.         private T lastT;

  815.         /** Value from the previous call to g. */
  816.         private T lastG;

  818.         /** Build a wrapped event detector.
  819.          * @param detector event detector to wrap
  820.         */
  821.         FieldAdaptedEventDetector(final FieldEventDetector<T> detector) {
  822.             this.detector = detector;
  823.             this.lastT    = getField().getZero().add(Double.NaN);
  824.             this.lastG    = getField().getZero().add(Double.NaN);
  825.         }

  827.         /** {@inheritDoc} */
  828.         public void init(final FieldODEStateAndDerivative<T> s0, final T t) {
  829.             try {

  831.                 detector.init(getCompleteState(s0.getTime(), s0.getCompleteState(), s0.getCompleteDerivative()),
  832.                               stateMapper.mapDoubleToDate(t));
  833.                 this.lastT = getField().getZero().add(Double.NaN);
  834.                 this.lastG = getField().getZero().add(Double.NaN);

  836.             } catch (OrekitException oe) {
  837.                 throw new OrekitExceptionWrapper(oe);
  838.             }
  839.         }

  841.         /** {@inheritDoc} */
  842.         public T g(final FieldODEStateAndDerivative<T> s) {
  843.             try {
  844.                 if (!Precision.equals(lastT.getReal(), s.getTime().getReal(), 0)) {
  845.                     lastT = s.getTime();
  846.                     lastG = detector.g(getCompleteState(s.getTime(), s.getCompleteState(), s.getCompleteDerivative()));
  847.                 }
  848.                 return lastG;
  849.             } catch (OrekitException oe) {
  850.                 throw new OrekitExceptionWrapper(oe);
  851.             }
  852.         }

  854.         /** {@inheritDoc} */
  855.         public Action eventOccurred(final FieldODEStateAndDerivative<T> s, final boolean increasing) {
  856.             try {

  858.                 final FieldEventHandler.Action whatNext = detector.eventOccurred(getCompleteState(s.getTime(),
  859.                                                                                                   s.getCompleteState(),
  860.                                                                                                   s.getCompleteDerivative()),
  861.                                                                             increasing);

  863.                 switch (whatNext) {
  864.                     case STOP :
  865.                         return Action.STOP;
  866.                     case RESET_STATE :
  867.                         return Action.RESET_STATE;
  868.                     case RESET_DERIVATIVES :
  869.                         return Action.RESET_DERIVATIVES;
  870.                     default :
  871.                         return Action.CONTINUE;
  872.                 }
  873.             } catch (OrekitException oe) {
  874.                 throw new OrekitExceptionWrapper(oe);
  875.             }
  876.         }

  878.         /** {@inheritDoc} */
  879.         public FieldODEState<T> resetState(final FieldODEStateAndDerivative<T> s) {
  880.             try {

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

  885.                 // main part
  886.                 final T[] primary    = MathArrays.buildArray(getField(), s.getPrimaryStateDimension());
  887.                 stateMapper.mapStateToArray(newState, primary, null);

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

  892.                 for (int i = 0; i < additionalEquations.size(); ++i) {
  893.                     final FieldAdditionalEquations<T> additional = additionalEquations.get(i);
  894.                     final T[] NState = newState.getAdditionalState(additional.getName());
  895.                     secondary[i] = MathArrays.buildArray(getField(), NState.length);
  896.                     for (int j = 0; j < NState.length; j++)
  897.                         secondary[i][j] = NState[j];
  898.                 }

  900.                 return new FieldODEState<>(newState.getDate().durationFrom(getStartDate()),
  901.                                            primary, secondary);

  903.             } catch (OrekitException oe) {
  904.                 throw new OrekitExceptionWrapper(oe);
  905.             }
  906.         }

  908.     }

  910.     /** Adapt an {@link org.orekit.propagation.sampling.FieldOrekitStepHandler<T>}
  911.      * to Hipparchus {@link FieldODEStepHandler<T>} interface.
  912.      * @author Luc Maisonobe
  913.      */
  914.     private class FieldAdaptedStepHandler
  915.         implements FieldOrekitStepInterpolator<T>, FieldODEStepHandler<T>, FieldModeHandler<T> {

  917.         /** Underlying handler. */
  918.         private final FieldOrekitStepHandler<T> handler;

  920.         /** Flag for handler . */
  921.         private boolean activate;

  923.         /** Build an instance.
  924.          * @param handler underlying handler to wrap
  925.          */
  926.         FieldAdaptedStepHandler(final FieldOrekitStepHandler<T> handler) {
  927.             this.handler = handler;
  928.         }

  930.         /** {@inheritDoc} */
  931.         public void initialize(final boolean activateHandlers,
  932.                                final FieldAbsoluteDate<T> targetDate) {
  933.             this.activate = activateHandlers;
  934.         }
  935.         /** {@inheritDoc} */
  936.         public void init(final FieldODEStateAndDerivative<T> s0, final T t) {
  937.             try {
  938.                 handler.init(getCompleteState(s0.getTime(), s0.getCompleteState(), s0.getCompleteDerivative()),
  939.                              stateMapper.mapDoubleToDate(t));
  940.             } catch (OrekitException oe) {
  941.                 throw new OrekitExceptionWrapper(oe);
  942.             }
  943.         }

  945.         /** {@inheritDoc} */
  946.         public void handleStep(final FieldODEStateInterpolator<T> interpolator, final boolean isLast) {
  947.             try {
  948.                 mathInterpolator = interpolator;
  949.                 if (activate) {
  950.                     handler.handleStep(this, isLast);
  951.                 }
  952.             } catch (OrekitException oe) {
  953.                 throw new OrekitExceptionWrapper(oe);
  954.             }
  955.         }

  957.         /** {@inheritDoc}} */
  958.         @Override
  959.         public FieldSpacecraftState<T> getPreviousState()
  960.             throws OrekitException {
  961.             return convert(mathInterpolator.getPreviousState());
  962.         }

  964.         /** {@inheritDoc}} */
  965.         @Override
  966.         public FieldSpacecraftState<T> getCurrentState()
  967.             throws OrekitException {
  968.             return convert(mathInterpolator.getCurrentState());
  969.         }

  971.         /** {@inheritDoc}} */
  972.         @Override
  973.         public FieldSpacecraftState<T> getInterpolatedState(final FieldAbsoluteDate<T> date)
  974.             throws OrekitException {
  975.             return convert(mathInterpolator.getInterpolatedState(date.durationFrom(getStartDate())));
  976.         }

  978.         /** Get the interpolated state.
  979.          * @param os mathematical state
  980.          * @return interpolated state at the current interpolation date
  981.          * @exception OrekitException if state cannot be interpolated or converted
  982.          * @exception OrekitException if underlying interpolator cannot handle
  983.          * the date
  984.          * @see #getInterpolatedDate()
  985.          * @see #setInterpolatedDate(FieldAbsoluteDate<T>)
  986.          */
  987.         private FieldSpacecraftState<T> convert(final FieldODEStateAndDerivative<T> os)
  988.             throws OrekitException {
  989.             try {

  991.                 FieldSpacecraftState<T> s =
  992.                         stateMapper.mapArrayToState(os.getTime(),
  993.                                                     os.getPrimaryState(),
  994.                                                     os.getPrimaryDerivative(),
  995.                                                     meanOrbit);
  996.                 s = updateAdditionalStates(s);
  997.                 for (int i = 0; i < additionalEquations.size(); ++i) {
  998.                     final T[] secondary = os.getSecondaryState(i + 1);
  999.                     s = s.addAdditionalState(additionalEquations.get(i).getName(), secondary);
  1000.                 }

  1002.                 return s;

  1004.             } catch (OrekitException oe) {
  1005.                 throw new OrekitException(oe);
  1006.             } catch (OrekitExceptionWrapper oew) {
  1007.                 throw new OrekitException(oew.getException());
  1008.             }
  1009.         }

  1011.         /** Check is integration direction is forward in date.
  1012.          * @return true if integration is forward in date
  1013.          */
  1014.         public boolean isForward() {
  1015.             return mathInterpolator.isForward();
  1016.         }

  1018.     }

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

  1022.         /** Underlying raw mathematical model. */
  1023.         private FieldDenseOutputModel<T> model;

  1025.         /** Generated ephemeris. */
  1026.         private FieldBoundedPropagator<T> ephemeris;

  1028.         /** Flag for handler . */
  1029.         private boolean activate;

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

  1034.         /** Creates a new instance of FieldEphemerisModeHandler which must be
  1035.          *  filled by the propagator.
  1036.          */
  1037.         FieldEphemerisModeHandler() {
  1038.         }

  1040.         /** {@inheritDoc} */
  1041.         public void initialize(final boolean activateHandlers,
  1042.                                final FieldAbsoluteDate<T> targetDate) {
  1043.             this.activate = activateHandlers;
  1044.             this.model    = new FieldDenseOutputModel<>();
  1045.             this.endDate  = targetDate;

  1047.             // ephemeris will be generated when last step is processed
  1048.             this.ephemeris = null;

  1050.         }

  1052.         /** Get the generated ephemeris.
  1053.          * @return a new instance of the generated ephemeris
  1054.          */
  1055.         public FieldBoundedPropagator<T> getEphemeris() {
  1056.             return ephemeris;
  1057.         }

  1059.         /** {@inheritDoc} */
  1060.         public void handleStep(final FieldODEStateInterpolator<T> interpolator, final boolean isLast)
  1061.             throws OrekitExceptionWrapper {
  1062.             try {
  1063.                 if (activate) {
  1064.                     model.handleStep(interpolator, isLast);
  1065.                     if (isLast) {

  1067.                         // set up the boundary dates
  1068.                         final T tI = model.getInitialTime();
  1069.                         final T tF = model.getFinalTime();
  1070.                         // tI is almost? always zero
  1071.                         final FieldAbsoluteDate<T> startDate =
  1072.                                 stateMapper.mapDoubleToDate(tI);
  1073.                         final FieldAbsoluteDate<T> finalDate =
  1074.                                 stateMapper.mapDoubleToDate(tF, this.endDate);
  1075.                         final FieldAbsoluteDate<T> minDate;
  1076.                         final FieldAbsoluteDate<T> maxDate;
  1077.                         if (tF.getReal() < tI.getReal()) {
  1078.                             minDate = finalDate;
  1079.                             maxDate = startDate;
  1080.                         } else {
  1081.                             minDate = startDate;
  1082.                             maxDate = finalDate;
  1083.                         }

  1085.                         // get the initial additional states that are not managed
  1086.                         final Map<String, T[]> unmanaged = new HashMap<String, T[]>();
  1087.                         for (final Map.Entry<String, T[]> initial : getInitialState().getAdditionalStates().entrySet()) {
  1088.                             if (!isAdditionalStateManaged(initial.getKey())) {
  1089.                                 // this additional state was in the initial state, but is unknown to the propagator
  1090.                                 // we simply copy its initial value as is
  1091.                                 unmanaged.put(initial.getKey(), initial.getValue());
  1092.                             }
  1093.                         }

  1095.                         // get the names of additional states managed by differential equations
  1096.                         final String[] names = new String[additionalEquations.size()];
  1097.                         for (int i = 0; i < names.length; ++i) {
  1098.                             names[i] = additionalEquations.get(i).getName();
  1099.                         }

  1101.                         // create the ephemeris
  1102.                         ephemeris = new FieldIntegratedEphemeris<>(startDate, minDate, maxDate,
  1103.                                                                    stateMapper, meanOrbit, model, unmanaged,
  1104.                                                                    getAdditionalStateProviders(), names);

  1106.                     }
  1107.                 }
  1108.             } catch (OrekitException oe) {
  1109.                 throw new OrekitExceptionWrapper(oe);
  1110.             }
  1111.         }

  1113.         /** {@inheritDoc} */
  1114.         public void init(final FieldODEStateAndDerivative<T> s0, final T t) {
  1115.             model.init(s0, t);
  1116.         }

  1118.     }

  1120. }
Created with JaCoCo 0.7.9.201702052155