FieldAbstractIntegratedPropagator.java

  1. /* Copyright 2002-2021 CS GROUP
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.propagation.integration;

  19. import java.util.ArrayList;
  20. import java.util.Collection;
  21. import java.util.Collections;
  22. import java.util.HashMap;
  23. import java.util.List;
  24. import java.util.Map;

  26. import org.hipparchus.CalculusFieldElement;
  27. import org.hipparchus.Field;
  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.FieldEventHandlerConfiguration;
  40. import org.hipparchus.ode.events.FieldODEEventHandler;
  41. import org.hipparchus.ode.sampling.AbstractFieldODEStateInterpolator;
  42. import org.hipparchus.ode.sampling.FieldODEStateInterpolator;
  43. import org.hipparchus.ode.sampling.FieldODEStepHandler;
  44. import org.hipparchus.util.MathArrays;
  45. import org.hipparchus.util.Precision;
  46. import org.orekit.attitudes.AttitudeProvider;
  47. import org.orekit.errors.OrekitException;
  48. import org.orekit.errors.OrekitInternalError;
  49. import org.orekit.errors.OrekitMessages;
  50. import org.orekit.frames.Frame;
  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.FieldEphemerisGenerator;
  56. import org.orekit.propagation.FieldSpacecraftState;
  57. import org.orekit.propagation.PropagationType;
  58. import org.orekit.propagation.events.FieldEventDetector;
  59. import org.orekit.propagation.sampling.FieldOrekitStepHandler;
  60. import org.orekit.propagation.sampling.FieldOrekitStepInterpolator;
  61. import org.orekit.time.FieldAbsoluteDate;


  64. /** Common handling of {@link org.orekit.propagation.FieldPropagator FieldPropagator}
  65.  *  methods for both numerical and semi-analytical propagators.
  66.  * @param <T> the type of the field elements
  67.  *  @author Luc Maisonobe
  68.  */
  69. public abstract class FieldAbstractIntegratedPropagator<T extends CalculusFieldElement<T>> extends FieldAbstractPropagator<T> {

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

  74.     /** Step handlers dedicated to ephemeris generation. */
  75.     private final List<FieldStoringStepHandler> generators;

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

  80.     /** Additional equations. */
  81.     private List<FieldAdditionalEquations<T>> additionalEquations;

  83.     /** Counter for differential equations calls. */
  84.     private int calls;

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

  89.     /** Equations mapper. */
  90.     private FieldEquationsMapper<T> equationsMapper;

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

  95.     /** Type of orbit to output (mean or osculating) <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 PropagationType propagationType;

  103.     /** Build a new instance.
  104.      * @param integrator numerical integrator to use for propagation.
  105.      * @param propagationType type of orbit to output (mean or osculating).
  106.      * @param field Field used by default
  107.      */
  108.     protected FieldAbstractIntegratedPropagator(final Field<T> field, final FieldODEIntegrator<T> integrator, final PropagationType propagationType) {
  109.         super(field);
  110.         detectors            = new ArrayList<>();
  111.         generators           = new ArrayList<>();
  112.         additionalEquations  = new ArrayList<>();
  113.         this.integrator      = integrator;
  114.         this.propagationType = propagationType;
  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.      * @param field Field used by default
  136.      */
  137.     protected void initMapper(final Field<T> field) {
  138.         final T zero = field.getZero();
  139.         stateMapper = createMapper(null, zero.add(Double.NaN), null, null, null, null);
  140.     }

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

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

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

  166.     /** Check if only the mean elements should be used in a semianalitical propagation.
  167.      * @return {@link PropagationType MEAN} if only mean elements have to be used or
  168.      *         {@link PropagationType OSCULATING} if osculating elements have to be also used.
  169.      */
  170.     protected PropagationType isMeanOrbit() {
  171.         return propagationType;
  172.     }

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

  189.     /** Get propagation parameter type.
  190.      * @return angle type to use for propagation
  191.      */
  192.     protected PositionAngle getPositionAngleType() {
  193.         return stateMapper.getPositionAngleType();
  194.     }

  196.     /** Set the central attraction coefficient μ.
  197.      * @param mu central attraction coefficient (m³/s²)
  198.      */
  199.     public void setMu(final T mu) {
  200.         stateMapper = createMapper(stateMapper.getReferenceDate(), mu,
  201.                                    stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  202.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  203.     }

  205.     /** Get the central attraction coefficient μ.
  206.      * @return mu central attraction coefficient (m³/s²)
  207.      * @see #setMu(CalculusFieldElement)
  208.      */
  209.     public T getMu() {
  210.         return stateMapper.getMu();
  211.     }

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

  222.     /** {@inheritDoc} */
  223.     @Override
  224.     public boolean isAdditionalStateManaged(final String name) {

  226.         // first look at already integrated states
  227.         if (super.isAdditionalStateManaged(name)) {
  228.             return true;
  229.         }

  231.         // then look at states we integrate ourselves
  232.         for (final FieldAdditionalEquations<T> equation : additionalEquations) {
  233.             if (equation.getName().equals(name)) {
  234.                 return true;
  235.             }
  236.         }

  238.         return false;
  239.     }

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

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

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

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

  268.     }

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

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

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

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

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

  304.     /** {@inheritDoc} */
  305.     @Override
  306.     public FieldEphemerisGenerator<T> getEphemerisGenerator() {
  307.         final FieldStoringStepHandler storingHandler = new FieldStoringStepHandler();
  308.         generators.add(storingHandler);
  309.         return storingHandler;
  310.     }

  312.     /** Create a mapper between raw double components and spacecraft state.
  313.     /** Simple constructor.
  314.      * <p>
  315.      * The position parameter type is meaningful only if {@link
  316.      * #getOrbitType() propagation orbit type}
  317.      * support it. As an example, it is not meaningful for propagation
  318.      * in {@link OrbitType#CARTESIAN Cartesian} parameters.
  319.      * </p>
  320.      * @param referenceDate reference date
  321.      * @param mu central attraction coefficient (m³/s²)
  322.      * @param orbitType orbit type to use for mapping
  323.      * @param positionAngleType angle type to use for propagation
  324.      * @param attitudeProvider attitude provider
  325.      * @param frame inertial frame
  326.      * @return new mapper
  327.      */
  328.     protected abstract FieldStateMapper<T> createMapper(FieldAbsoluteDate<T> referenceDate, T mu,
  329.                                                         OrbitType orbitType, PositionAngle positionAngleType,
  330.                                                         AttitudeProvider attitudeProvider, Frame frame);

  332.     /** Get the differential equations to integrate (for main state only).
  333.      * @param integ numerical integrator to use for propagation.
  334.      * @return differential equations for main state
  335.      */
  336.     protected abstract MainStateEquations<T> getMainStateEquations(FieldODEIntegrator<T> integ);

  338.     /** {@inheritDoc} */
  339.     public FieldSpacecraftState<T> propagate(final FieldAbsoluteDate<T> target) {
  340.         if (getStartDate() == null) {
  341.             if (getInitialState() == null) {
  342.                 throw new OrekitException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
  343.             }
  344.             setStartDate(getInitialState().getDate());
  345.         }
  346.         return propagate(getStartDate(), target);
  347.     }

  349.     /** {@inheritDoc} */
  350.     public FieldSpacecraftState<T> propagate(final FieldAbsoluteDate<T> tStart, final FieldAbsoluteDate<T> tEnd) {

  352.         if (getInitialState() == null) {
  353.             throw new OrekitException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
  354.         }

  356.         // make sure the integrator will be reset properly even if we change its events handlers and step handlers
  357.         try (IntegratorResetter<T> resetter = new IntegratorResetter<>(integrator)) {

  359.             if (!tStart.equals(getInitialState().getDate())) {
  360.                 // if propagation start date is not initial date,
  361.                 // propagate from initial to start date without event detection
  362.                 integrateDynamics(tStart);
  363.             }

  365.             // set up events added by user
  366.             setUpUserEventDetectors();

  368.             // set up step handlers
  369.             for (final FieldOrekitStepHandler<T> handler : getMultiplexer().getHandlers()) {
  370.                 integrator.addStepHandler(new FieldAdaptedStepHandler(handler));
  371.             }
  372.             for (final FieldStoringStepHandler generator : generators) {
  373.                 generator.setEndDate(tEnd);
  374.                 integrator.addStepHandler(generator);
  375.             }

  377.             // propagate from start date to end date with event detection
  378.             return integrateDynamics(tEnd);

  380.         }

  382.     }

  384.     /** Propagation with or without event detection.
  385.      * @param tEnd target date to which orbit should be propagated
  386.      * @return state at end of propagation
  387.      */
  388.     private FieldSpacecraftState<T> integrateDynamics(final FieldAbsoluteDate<T> tEnd) {
  389.         try {

  391.             initializePropagation();

  393.             if (getInitialState().getDate().equals(tEnd)) {
  394.                 // don't extrapolate
  395.                 return getInitialState();
  396.             }
  397.             // space dynamics view
  398.             stateMapper = createMapper(getInitialState().getDate(), stateMapper.getMu(),
  399.                                        stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  400.                                        stateMapper.getAttitudeProvider(), getInitialState().getFrame());


  403.             // set propagation orbit type
  404.             //final FieldOrbit<T> initialOrbit = stateMapper.getOrbitType().convertType(getInitialState().getOrbit());
  405.             if (Double.isNaN(getMu().getReal())) {
  406.                 setMu(getInitialState().getMu());
  407.             }
  408.             if (getInitialState().getMass().getReal() <= 0.0) {
  409.                 throw new OrekitException(OrekitMessages.SPACECRAFT_MASS_BECOMES_NEGATIVE,
  410.                                                getInitialState().getMass());
  411.             }

  413.             // convert space flight dynamics API to math API
  414.             final FieldSpacecraftState<T> initialIntegrationState = getInitialIntegrationState();
  415.             final FieldODEState<T> mathInitialState = createInitialState(initialIntegrationState);
  416.             final FieldExpandableODE<T> mathODE = createODE(integrator, mathInitialState);
  417.             equationsMapper = mathODE.getMapper();

  419.             // mathematical integration
  420.             final FieldODEStateAndDerivative<T> mathFinalState;
  421.             beforeIntegration(initialIntegrationState, tEnd);
  422.             mathFinalState = integrator.integrate(mathODE, mathInitialState,
  423.                                                   tEnd.durationFrom(getInitialState().getDate()));

  425.             afterIntegration();

  427.             // get final state
  428.             FieldSpacecraftState<T> finalState =
  429.                             stateMapper.mapArrayToState(stateMapper.mapDoubleToDate(mathFinalState.getTime(), tEnd),
  430.                                                         mathFinalState.getPrimaryState(),
  431.                                                         mathFinalState.getPrimaryDerivative(),
  432.                                                         propagationType);
  433.             finalState = updateAdditionalStates(finalState);
  434.             for (int i = 0; i < additionalEquations.size(); ++i) {
  435.                 final T[] secondary = mathFinalState.getSecondaryState(i + 1);
  436.                 finalState = finalState.addAdditionalState(additionalEquations.get(i).getName(),
  437.                                                            secondary);
  438.             }
  439.             if (resetAtEnd) {
  440.                 resetInitialState(finalState);
  441.                 setStartDate(finalState.getDate());
  442.             }

  444.             return finalState;

  446.         } catch (OrekitException pe) {
  447.             throw pe;
  448.         } catch (MathIllegalArgumentException miae) {
  449.             throw OrekitException.unwrap(miae);
  450.         } catch (MathIllegalStateException mise) {
  451.             throw OrekitException.unwrap(mise);
  452.         }
  453.     }

  455.     /** Get the initial state for integration.
  456.      * @return initial state for integration
  457.      */
  458.     protected FieldSpacecraftState<T> getInitialIntegrationState() {
  459.         return getInitialState();
  460.     }

  462.     /** Create an initial state.
  463.      * @param initialState initial state in flight dynamics world
  464.      * @return initial state in mathematics world
  465.      */
  466.     private FieldODEState<T> createInitialState(final FieldSpacecraftState<T> initialState) {

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

  472.         // secondary part of the ODE
  473.         final T[][] secondary = MathArrays.buildArray(initialState.getA().getField(), additionalEquations.size(), -1);
  474.         for (int i = 0; i < additionalEquations.size(); ++i) {
  475.             final FieldAdditionalEquations<T> additional = additionalEquations.get(i);
  476.             final T[] addState = getInitialState().getAdditionalState(additional.getName());
  477.             secondary[i] = MathArrays.buildArray(initialState.getA().getField(), addState.length);
  478.             for (int j = 0; j < addState.length; j++) {
  479.                 secondary[i][j] = addState[j];
  480.             }
  481.         }

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

  485.     }

  487.     /** Create an ODE with all equations.
  488.      * @param integ numerical integrator to use for propagation.
  489.      * @param mathInitialState initial state
  490.      * @return a new ode
  491.      */
  492.     private FieldExpandableODE<T> createODE(final FieldODEIntegrator<T> integ,
  493.                                     final FieldODEState<T> mathInitialState) {

  495.         final FieldExpandableODE<T> ode =
  496.                 new FieldExpandableODE<>(new ConvertedMainStateEquations(getMainStateEquations(integ)));

  498.         // secondary part of the ODE
  499.         for (int i = 0; i < additionalEquations.size(); ++i) {
  500.             final FieldAdditionalEquations<T> additional = additionalEquations.get(i);
  501.             final FieldSecondaryODE<T> secondary =
  502.                     new ConvertedSecondaryStateEquations(additional,
  503.                                                          mathInitialState.getSecondaryStateDimension(i + 1));
  504.             ode.addSecondaryEquations(secondary);
  505.         }

  507.         return ode;

  509.     }

  511.     /** Method called just before integration.
  512.      * <p>
  513.      * The default implementation does nothing, it may be specialized in subclasses.
  514.      * </p>
  515.      * @param initialState initial state
  516.      * @param tEnd target date at which state should be propagated
  517.      */
  518.     protected void beforeIntegration(final FieldSpacecraftState<T> initialState,
  519.                                      final FieldAbsoluteDate<T> tEnd) {
  520.         // do nothing by default
  521.     }

  523.     /** Method called just after integration.
  524.      * <p>
  525.      * The default implementation does nothing, it may be specialized in subclasses.
  526.      * </p>
  527.      */
  528.     protected void afterIntegration() {
  529.         // do nothing by default
  530.     }

  532.     /** Get state vector dimension without additional parameters.
  533.      * @return state vector dimension without additional parameters.
  534.      */
  535.     public int getBasicDimension() {
  536.         return 7;

  538.     }

  540.     /** Get the integrator used by the propagator.
  541.      * @return the integrator.
  542.      */
  543.     protected FieldODEIntegrator<T> getIntegrator() {
  544.         return integrator;
  545.     }

  547.     /** Get a complete state with all additional equations.
  548.      * @param t current value of the independent <I>time</I> variable
  549.      * @param ts array containing the current value of the state vector
  550.      * @param tsDot array containing the current value of the state vector derivative
  551.      * @return complete state
  552.      */
  553.     private FieldSpacecraftState<T> getCompleteState(final T t, final T[] ts, final T[] tsDot) {

  555.         // main state
  556.         FieldSpacecraftState<T> state = stateMapper.mapArrayToState(t, ts, tsDot, PropagationType.MEAN);  //not sure of the mean orbit, should be true
  557.         // pre-integrated additional states
  558.         state = updateAdditionalStates(state);

  560.         // additional states integrated here
  561.         if (!additionalEquations.isEmpty()) {

  563.             for (int i = 0; i < additionalEquations.size(); ++i) {
  564.                 state = state.addAdditionalState(additionalEquations.get(i).getName(),
  565.                                                  equationsMapper.extractEquationData(i + 1, ts));
  566.             }

  568.         }

  570.         return state;

  572.     }

  574.     /** Get the interpolated state.
  575.      * @param os mathematical state
  576.      * @return interpolated state at the current interpolation date
  577.                        * the date
  578.      * @see #getInterpolatedDate()
  579.      * @see #setInterpolatedDate(FieldAbsoluteDate<T>)
  580.      */
  581.     private FieldSpacecraftState<T> convert(final FieldODEStateAndDerivative<T> os) {
  582.         try {

  584.             FieldSpacecraftState<T> s =
  585.                     stateMapper.mapArrayToState(os.getTime(),
  586.                                                 os.getPrimaryState(),
  587.                                                 os.getPrimaryDerivative(),
  588.                                                 propagationType);
  589.             s = updateAdditionalStates(s);
  590.             for (int i = 0; i < additionalEquations.size(); ++i) {
  591.                 final T[] secondary = os.getSecondaryState(i + 1);
  592.                 s = s.addAdditionalState(additionalEquations.get(i).getName(), secondary);
  593.             }

  595.             return s;

  597.         } catch (OrekitException oe) {
  598.             throw new OrekitException(oe);
  599.         }
  600.     }

  602.     /** Convert a state from space flight dynamics world to mathematical world.
  603.      * @param state space flight dynamics state
  604.      * @return mathematical state
  605.      */
  606.     private FieldODEStateAndDerivative<T> convert(final FieldSpacecraftState<T> state) {

  608.         // retrieve initial state
  609.         final T[] primary    = MathArrays.buildArray(getField(), getBasicDimension());
  610.         final T[] primaryDot = MathArrays.buildArray(getField(), getBasicDimension());
  611.         stateMapper.mapStateToArray(state, primary, primaryDot);

  613.         // secondary part of the ODE
  614.         final T[][] secondary    = MathArrays.buildArray(getField(), additionalEquations.size(), -1);
  615.         for (int i = 0; i < additionalEquations.size(); ++i) {
  616.             final FieldAdditionalEquations<T> additional = additionalEquations.get(i);
  617.             secondary[i] = state.getAdditionalState(additional.getName());
  618.         }

  620.         return new FieldODEStateAndDerivative<>(stateMapper.mapDateToDouble(state.getDate()),
  621.                                                 primary, primaryDot,
  622.                                                 secondary, null);

  624.     }

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

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

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

  647.     }

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

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

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

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

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

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

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

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

  689.         }

  691.     }

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

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

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

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

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

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

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

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

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

  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.             return detector.eventOccurred(
  802.                     getCompleteState(
  803.                             s.getTime(),
  804.                             s.getCompleteState(),
  805.                             s.getCompleteDerivative()),
  806.                     increasing);
  807.         }

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

  812.             final FieldSpacecraftState<T> oldState = getCompleteState(s.getTime(), s.getCompleteState(), s.getCompleteDerivative());
  813.             final FieldSpacecraftState<T> newState = detector.resetState(oldState);
  814.             stateChanged(newState);

  816.             // main part
  817.             final T[] primary    = MathArrays.buildArray(getField(), s.getPrimaryStateDimension());
  818.             stateMapper.mapStateToArray(newState, primary, null);

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

  823.             for (int i = 0; i < additionalEquations.size(); ++i) {
  824.                 final FieldAdditionalEquations<T> additional = additionalEquations.get(i);
  825.                 final T[] NState = newState.getAdditionalState(additional.getName());
  826.                 secondary[i] = MathArrays.buildArray(getField(), NState.length);
  827.                 for (int j = 0; j < NState.length; j++) {
  828.                     secondary[i][j] = NState[j];
  829.                 }
  830.             }

  832.             return new FieldODEState<>(newState.getDate().durationFrom(getStartDate()),
  833.                                        primary, secondary);
  834.         }

  836.     }

  838.     /** Adapt an {@link org.orekit.propagation.sampling.FieldOrekitStepHandler<T>}
  839.      * to Hipparchus {@link FieldODEStepHandler<T>} interface.
  840.      * @author Luc Maisonobe
  841.      */
  842.     private class FieldAdaptedStepHandler implements FieldODEStepHandler<T> {

  844.         /** Underlying handler. */
  845.         private final FieldOrekitStepHandler<T> handler;

  847.         /** Build an instance.
  848.          * @param handler underlying handler to wrap
  849.          */
  850.         FieldAdaptedStepHandler(final FieldOrekitStepHandler<T> handler) {
  851.             this.handler = handler;
  852.         }

  854.         /** {@inheritDoc} */
  855.         public void init(final FieldODEStateAndDerivative<T> s0, final T t) {
  856.             handler.init(getCompleteState(s0.getTime(), s0.getCompleteState(), s0.getCompleteDerivative()),
  857.                          stateMapper.mapDoubleToDate(t));
  858.         }

  860.         /** {@inheritDoc} */
  861.         public void handleStep(final FieldODEStateInterpolator<T> interpolator) {
  862.             handler.handleStep(new FieldAdaptedStepInterpolator(interpolator));
  863.         }

  865.         /** {@inheritDoc} */
  866.         @Override
  867.         public void finish(final FieldODEStateAndDerivative<T> finalState) {
  868.             handler.finish(convert(finalState));
  869.         }

  871.     }

  873.     /** Adapt an {@link org.orekit.propagation.sampling.FieldOrekitStepInterpolator<T>}
  874.      * to Hipparchus {@link FieldODEStepInterpolator<T>} interface.
  875.      * @author Luc Maisonobe
  876.      */
  877.     private class FieldAdaptedStepInterpolator implements FieldOrekitStepInterpolator<T> {

  879.         /** Underlying raw rawInterpolator. */
  880.         private final FieldODEStateInterpolator<T> mathInterpolator;

  882.         /** Build an instance.
  883.          * @param mathInterpolator underlying raw interpolator
  884.          */
  885.         FieldAdaptedStepInterpolator(final FieldODEStateInterpolator<T> mathInterpolator) {
  886.             this.mathInterpolator = mathInterpolator;
  887.         }

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

  895.         /** {@inheritDoc}} */
  896.         @Override
  897.         public FieldSpacecraftState<T> getCurrentState() {
  898.             return convert(mathInterpolator.getCurrentState());
  899.         }

  901.         /** {@inheritDoc}} */
  902.         @Override
  903.         public FieldSpacecraftState<T> getInterpolatedState(final FieldAbsoluteDate<T> date) {
  904.             return convert(mathInterpolator.getInterpolatedState(date.durationFrom(getStartDate())));
  905.         }

  907.         /** Check is integration direction is forward in date.
  908.          * @return true if integration is forward in date
  909.          */
  910.         public boolean isForward() {
  911.             return mathInterpolator.isForward();
  912.         }

  914.         /** {@inheritDoc}} */
  915.         @Override
  916.         public FieldAdaptedStepInterpolator restrictStep(final FieldSpacecraftState<T> newPreviousState,
  917.                                                          final FieldSpacecraftState<T> newCurrentState) {
  918.             try {
  919.                 final AbstractFieldODEStateInterpolator<T> aosi = (AbstractFieldODEStateInterpolator<T>) mathInterpolator;
  920.                 return new FieldAdaptedStepInterpolator(aosi.restrictStep(convert(newPreviousState),
  921.                                                                           convert(newCurrentState)));
  922.             } catch (ClassCastException cce) {
  923.                 // this should never happen
  924.                 throw new OrekitInternalError(cce);
  925.             }
  926.         }

  928.     }

  930.     /** Specialized step handler storing interpolators for ephemeris generation.
  931.      * @since 11.0
  932.      */
  933.     private class FieldStoringStepHandler implements FieldODEStepHandler<T>, FieldEphemerisGenerator<T> {

  935.         /** Underlying raw mathematical model. */
  936.         private FieldDenseOutputModel<T> model;

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

  941.         /** Generated ephemeris. */
  942.         private FieldBoundedPropagator<T> ephemeris;

  944.         /** Set the end date.
  945.          * @param endDate end date
  946.          */
  947.         public void setEndDate(final FieldAbsoluteDate<T> endDate) {
  948.             this.endDate = endDate;
  949.         }

  951.         /** {@inheritDoc} */
  952.         @Override
  953.         public void init(final FieldODEStateAndDerivative<T> s0, final T t) {
  954.             this.model = new FieldDenseOutputModel<>();
  955.             model.init(s0, t);

  957.             // ephemeris will be generated when last step is processed
  958.             this.ephemeris = null;

  960.         }

  962.         /** {@inheritDoc} */
  963.         @Override
  964.         public FieldBoundedPropagator<T> getGeneratedEphemeris() {
  965.             return ephemeris;
  966.         }

  968.         /** {@inheritDoc} */
  969.         @Override
  970.         public void handleStep(final FieldODEStateInterpolator<T> interpolator) {
  971.             model.handleStep(interpolator);
  972.         }

  974.         /** {@inheritDoc} */
  975.         @Override
  976.         public void finish(final FieldODEStateAndDerivative<T> finalState) {

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

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

  1006.             // get the names of additional states managed by differential equations
  1007.             final String[] names = new String[additionalEquations.size()];
  1008.             for (int i = 0; i < names.length; ++i) {
  1009.                 names[i] = additionalEquations.get(i).getName();
  1010.             }

  1012.             // create the ephemeris
  1013.             ephemeris = new FieldIntegratedEphemeris<>(startDate, minDate, maxDate,
  1014.                                                        stateMapper, propagationType, model, unmanaged,
  1015.                                                        getAdditionalStateProviders(), names);

  1017.         }

  1019.     }

  1021.     /** Wrapper for resetting an integrator handlers.
  1022.      * <p>
  1023.      * This class is intended to be used in a try-with-resource statement.
  1024.      * If propagator-specific event handlers and step handlers are added to
  1025.      * the integrator in the try block, they will be removed automatically
  1026.      * when leaving the block, so the integrator only keep its own handlers
  1027.      * between calls to {@link AbstractIntegratedPropagator#propagate(AbsoluteDate, AbsoluteDate).
  1028.      * </p>
  1029.  * @param <T> the type of the field elements
  1030.      * @since 11.0
  1031.      */
  1032.     private static class IntegratorResetter<T extends CalculusFieldElement<T>> implements AutoCloseable {

  1034.         /** Wrapped integrator. */
  1035.         private final FieldODEIntegrator<T> integrator;

  1037.         /** Initial event handlers list. */
  1038.         private final List<FieldEventHandlerConfiguration<T>> eventHandlersConfigurations;

  1040.         /** Initial step handlers list. */
  1041.         private final List<FieldODEStepHandler<T>> stepHandlers;

  1043.         /** Simple constructor.
  1044.          * @param integrator wrapped integrator
  1045.          */
  1046.         IntegratorResetter(final FieldODEIntegrator<T> integrator) {
  1047.             this.integrator                  = integrator;
  1048.             this.eventHandlersConfigurations = new ArrayList<>(integrator.getEventHandlersConfigurations());
  1049.             this.stepHandlers                = new ArrayList<>(integrator.getStepHandlers());
  1050.         }

  1052.         /** {@inheritDoc}
  1053.          * <p>
  1054.          * Reset event handlers and step handlers back to the initial list
  1055.          * </p>
  1056.          */
  1057.         @Override
  1058.         public void close() {

  1060.             // reset event handlers
  1061.             integrator.clearEventHandlers();
  1062.             eventHandlersConfigurations.forEach(c -> integrator.addEventHandler(c.getEventHandler(),
  1063.                                                                                 c.getMaxCheckInterval(),
  1064.                                                                                 c.getConvergence().getReal(),
  1065.                                                                                 c.getMaxIterationCount(),
  1066.                                                                                 c.getSolver()));

  1068.             // reset step handlers
  1069.             integrator.clearStepHandlers();
  1070.             stepHandlers.forEach(stepHandler -> integrator.addStepHandler(stepHandler));

  1072.         }

  1074.     }

  1076. }
Created with JaCoCo 0.8.7.202105040129