AbstractIntegratedPropagator.java

  1. /* Copyright 2002-2016 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.apache.commons.math3.exception.MathIllegalArgumentException;
  27. import org.apache.commons.math3.exception.MathIllegalStateException;
  28. import org.apache.commons.math3.ode.AbstractIntegrator;
  29. import org.apache.commons.math3.ode.ContinuousOutputModel;
  30. import org.apache.commons.math3.ode.EquationsMapper;
  31. import org.apache.commons.math3.ode.ExpandableStatefulODE;
  32. import org.apache.commons.math3.ode.FirstOrderDifferentialEquations;
  33. import org.apache.commons.math3.ode.SecondaryEquations;
  34. import org.apache.commons.math3.ode.sampling.StepHandler;
  35. import org.apache.commons.math3.ode.sampling.StepInterpolator;
  36. import org.apache.commons.math3.util.Precision;
  37. import org.orekit.attitudes.AttitudeProvider;
  38. import org.orekit.errors.OrekitException;
  39. import org.orekit.errors.OrekitExceptionWrapper;
  40. import org.orekit.errors.OrekitIllegalStateException;
  41. import org.orekit.errors.OrekitMessages;
  42. import org.orekit.errors.PropagationException;
  43. import org.orekit.frames.Frame;
  44. import org.orekit.orbits.Orbit;
  45. import org.orekit.orbits.OrbitType;
  46. import org.orekit.orbits.PositionAngle;
  47. import org.orekit.propagation.AbstractPropagator;
  48. import org.orekit.propagation.BoundedPropagator;
  49. import org.orekit.propagation.SpacecraftState;
  50. import org.orekit.propagation.events.EventDetector;
  51. import org.orekit.propagation.events.handlers.EventHandler;
  52. import org.orekit.propagation.sampling.OrekitStepHandler;
  53. import org.orekit.propagation.sampling.OrekitStepInterpolator;
  54. import org.orekit.time.AbsoluteDate;


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

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

  66.     /** Integrator selected by the user for the orbital extrapolation process. */
  67.     private final AbstractIntegrator integrator;

  69.     /** Mode handler. */
  70.     private ModeHandler modeHandler;

  72.     /** Additional equations. */
  73.     private List<AdditionalEquations> additionalEquations;

  75.     /** Counter for differential equations calls. */
  76.     private int calls;

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

  81.     /** Complete equation to be integrated. */
  82.     private ExpandableStatefulODE mathODE;

  84.     /** Underlying raw rawInterpolator. */
  85.     private StepInterpolator mathInterpolator;

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

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

  106.     /** Initialize the mapper. */
  107.     protected void initMapper() {
  108.         stateMapper = createMapper(null, Double.NaN, null, null, null, null);
  109.     }

  111.     /**  {@inheritDoc} */
  112.     public void setAttitudeProvider(final AttitudeProvider attitudeProvider) {
  113.         super.setAttitudeProvider(attitudeProvider);
  114.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  115.                                    stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  116.                                    attitudeProvider, stateMapper.getFrame());
  117.     }

  119.     /** Set propagation orbit type.
  120.      * @param orbitType orbit type to use for propagation
  121.      */
  122.     protected void setOrbitType(final OrbitType orbitType) {
  123.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  124.                                    orbitType, stateMapper.getPositionAngleType(),
  125.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  126.     }

  128.     /** Get propagation parameter type.
  129.      * @return orbit type used for propagation
  130.      */
  131.     protected OrbitType getOrbitType() {
  132.         return stateMapper.getOrbitType();
  133.     }

  135.     /** Check if only the mean elements should be used in a semianalitical propagation.
  136.      * @return true if only mean elements have to be used
  137.      */
  138.     protected boolean isMeanOrbit() {
  139.         return meanOrbit;
  140.     }

  142.     /** Set position angle type.
  143.      * <p>
  144.      * The position parameter type is meaningful only if {@link
  145.      * #getOrbitType() propagation orbit type}
  146.      * support it. As an example, it is not meaningful for propagation
  147.      * in {@link OrbitType#CARTESIAN Cartesian} parameters.
  148.      * </p>
  149.      * @param positionAngleType angle type to use for propagation
  150.      */
  151.     protected void setPositionAngleType(final PositionAngle positionAngleType) {
  152.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  153.                                    stateMapper.getOrbitType(), positionAngleType,
  154.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  155.     }

  157.     /** Get propagation parameter type.
  158.      * @return angle type to use for propagation
  159.      */
  160.     protected PositionAngle getPositionAngleType() {
  161.         return stateMapper.getPositionAngleType();
  162.     }

  164.     /** Set the central attraction coefficient μ.
  165.      * @param mu central attraction coefficient (m³/s²)
  166.      */
  167.     public void setMu(final double mu) {
  168.         stateMapper = createMapper(stateMapper.getReferenceDate(), mu,
  169.                                    stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  170.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  171.     }

  173.     /** Get the central attraction coefficient μ.
  174.      * @return mu central attraction coefficient (m³/s²)
  175.      * @see #setMu(double)
  176.      */
  177.     public double getMu() {
  178.         return stateMapper.getMu();
  179.     }

  181.     /** Get the number of calls to the differential equations computation method.
  182.      * <p>The number of calls is reset each time the {@link #propagate(AbsoluteDate)}
  183.      * method is called.</p>
  184.      * @return number of calls to the differential equations computation method
  185.      */
  186.     public int getCalls() {
  187.         return calls;
  188.     }

  190.     /** {@inheritDoc} */
  191.     @Override
  192.     public boolean isAdditionalStateManaged(final String name) {

  194.         // first look at already integrated states
  195.         if (super.isAdditionalStateManaged(name)) {
  196.             return true;
  197.         }

  199.         // then look at states we integrate ourselves
  200.         for (final AdditionalEquations equation : additionalEquations) {
  201.             if (equation.getName().equals(name)) {
  202.                 return true;
  203.             }
  204.         }

  206.         return false;
  207.     }

  209.     /** {@inheritDoc} */
  210.     @Override
  211.     public String[] getManagedAdditionalStates() {
  212.         final String[] alreadyIntegrated = super.getManagedAdditionalStates();
  213.         final String[] managed = new String[alreadyIntegrated.length + additionalEquations.size()];
  214.         System.arraycopy(alreadyIntegrated, 0, managed, 0, alreadyIntegrated.length);
  215.         for (int i = 0; i < additionalEquations.size(); ++i) {
  216.             managed[i + alreadyIntegrated.length] = additionalEquations.get(i).getName();
  217.         }
  218.         return managed;
  219.     }

  221.     /** Add a set of user-specified equations to be integrated along with the orbit propagation.
  222.      * @param additional additional equations
  223.      * @exception OrekitException if a set of equations with the same name is already present
  224.      */
  225.     public void addAdditionalEquations(final AdditionalEquations additional)
  226.         throws OrekitException {

  228.         // check if the name is already used
  229.         if (isAdditionalStateManaged(additional.getName())) {
  230.             // this set of equations is already registered, complain
  231.             throw new OrekitException(OrekitMessages.ADDITIONAL_STATE_NAME_ALREADY_IN_USE,
  232.                                       additional.getName());
  233.         }

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

  238.     }

  240.     /** {@inheritDoc} */
  241.     public void addEventDetector(final EventDetector detector) {
  242.         detectors.add(detector);
  243.     }

  245.     /** {@inheritDoc} */
  246.     public Collection<EventDetector> getEventsDetectors() {
  247.         return Collections.unmodifiableCollection(detectors);
  248.     }

  250.     /** {@inheritDoc} */
  251.     public void clearEventsDetectors() {
  252.         detectors.clear();
  253.     }

  255.     /** Set up all user defined event detectors.
  256.      */
  257.     protected void setUpUserEventDetectors() {
  258.         for (final EventDetector detector : detectors) {
  259.             setUpEventDetector(integrator, detector);
  260.         }
  261.     }

  263.     /** Wrap an Orekit event detector and register it to the integrator.
  264.      * @param integ integrator into which event detector should be registered
  265.      * @param detector event detector to wrap
  266.      */
  267.     protected void setUpEventDetector(final AbstractIntegrator integ, final EventDetector detector) {
  268.         integ.addEventHandler(new AdaptedEventDetector(detector),
  269.                               detector.getMaxCheckInterval(),
  270.                               detector.getThreshold(),
  271.                               detector.getMaxIterationCount());
  272.     }

  274.     /** {@inheritDoc}
  275.      * <p>Note that this method has the side effect of replacing the step handlers
  276.      * of the underlying integrator set up in the {@link
  277.      * #AbstractIntegratedPropagator(AbstractIntegrator, boolean) constructor}. So if a specific
  278.      * step handler is needed, it should be added after this method has been callled.</p>
  279.      */
  280.     public void setSlaveMode() {
  281.         super.setSlaveMode();
  282.         if (integrator != null) {
  283.             integrator.clearStepHandlers();
  284.         }
  285.         modeHandler = null;
  286.     }

  288.     /** {@inheritDoc}
  289.      * <p>Note that this method has the side effect of replacing the step handlers
  290.      * of the underlying integrator set up in the {@link
  291.      * #AbstractIntegratedPropagator(AbstractIntegrator, boolean) constructor}. So if a specific
  292.      * step handler is needed, it should be added after this method has been callled.</p>
  293.      */
  294.     public void setMasterMode(final OrekitStepHandler handler) {
  295.         super.setMasterMode(handler);
  296.         integrator.clearStepHandlers();
  297.         final AdaptedStepHandler wrapped = new AdaptedStepHandler(handler);
  298.         integrator.addStepHandler(wrapped);
  299.         modeHandler = wrapped;
  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.      * #AbstractIntegratedPropagator(AbstractIntegrator, boolean) constructor}. So if a specific
  306.      * step handler is needed, it should be added after this method has been called.</p>
  307.      */
  308.     public void setEphemerisMode() {
  309.         super.setEphemerisMode();
  310.         integrator.clearStepHandlers();
  311.         final EphemerisModeHandler ephemeris = new EphemerisModeHandler();
  312.         modeHandler = ephemeris;
  313.         integrator.addStepHandler(ephemeris);
  314.     }

  316.     /** {@inheritDoc} */
  317.     public BoundedPropagator getGeneratedEphemeris()
  318.         throws IllegalStateException {
  319.         if (getMode() != EPHEMERIS_GENERATION_MODE) {
  320.             throw new OrekitIllegalStateException(OrekitMessages.PROPAGATOR_NOT_IN_EPHEMERIS_GENERATION_MODE);
  321.         }
  322.         return ((EphemerisModeHandler) modeHandler).getEphemeris();
  323.     }

  325.     /** Create a mapper between raw double components and spacecraft state.
  326.     /** Simple constructor.
  327.      * <p>
  328.      * The position parameter type is meaningful only if {@link
  329.      * #getOrbitType() propagation orbit type}
  330.      * support it. As an example, it is not meaningful for propagation
  331.      * in {@link OrbitType#CARTESIAN Cartesian} parameters.
  332.      * </p>
  333.      * @param referenceDate reference date
  334.      * @param mu central attraction coefficient (m³/s²)
  335.      * @param orbitType orbit type to use for mapping
  336.      * @param positionAngleType angle type to use for propagation
  337.      * @param attitudeProvider attitude provider
  338.      * @param frame inertial frame
  339.      * @return new mapper
  340.      */
  341.     protected abstract StateMapper createMapper(final AbsoluteDate referenceDate, final double mu,
  342.                                                 final OrbitType orbitType, final PositionAngle positionAngleType,
  343.                                                 final AttitudeProvider attitudeProvider, final Frame frame);

  345.     /** Get the differential equations to integrate (for main state only).
  346.      * @param integ numerical integrator to use for propagation.
  347.      * @return differential equations for main state
  348.      */
  349.     protected abstract MainStateEquations getMainStateEquations(final AbstractIntegrator integ);

  351.     /** {@inheritDoc} */
  352.     public SpacecraftState propagate(final AbsoluteDate target) throws PropagationException {
  353.         try {
  354.             if (getStartDate() == null) {
  355.                 if (getInitialState() == null) {
  356.                     throw new PropagationException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
  357.                 }
  358.                 setStartDate(getInitialState().getDate());
  359.             }
  360.             return propagate(getStartDate(), target);
  361.         } catch (OrekitException oe) {

  363.             // recover a possible embedded PropagationException
  364.             for (Throwable t = oe; t != null; t = t.getCause()) {
  365.                 if (t instanceof PropagationException) {
  366.                     throw (PropagationException) t;
  367.                 }
  368.             }
  369.             throw new PropagationException(oe);

  371.         }
  372.     }

  374.     /** {@inheritDoc} */
  375.     public SpacecraftState propagate(final AbsoluteDate tStart, final AbsoluteDate tEnd)
  376.         throws PropagationException {
  377.         try {

  379.             if (getInitialState() == null) {
  380.                 throw new PropagationException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
  381.             }

  383.             if (!tStart.equals(getInitialState().getDate())) {
  384.                 // if propagation start date is not initial date,
  385.                 // propagate from initial to start date without event detection
  386.                 propagate(tStart, false);
  387.             }

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

  392.         } catch (OrekitException oe) {

  394.             // recover a possible embedded PropagationException
  395.             for (Throwable t = oe; t != null; t = t.getCause()) {
  396.                 if (t instanceof PropagationException) {
  397.                     throw (PropagationException) t;
  398.                 }
  399.             }
  400.             throw new PropagationException(oe);

  402.         }
  403.     }

  405.     /** Propagation with or without event detection.
  406.      * @param tEnd target date to which orbit should be propagated
  407.      * @param activateHandlers if true, step and event handlers should be activated
  408.      * @return state at end of propagation
  409.      * @exception PropagationException if orbit cannot be propagated
  410.      */
  411.     protected SpacecraftState propagate(final AbsoluteDate tEnd, final boolean activateHandlers)
  412.         throws PropagationException {
  413.         try {

  415.             if (getInitialState().getDate().equals(tEnd)) {
  416.                 // don't extrapolate
  417.                 return getInitialState();
  418.             }

  420.             // space dynamics view
  421.             stateMapper = createMapper(getInitialState().getDate(), stateMapper.getMu(),
  422.                                        stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  423.                                        stateMapper.getAttitudeProvider(), getInitialState().getFrame());


  426.             // set propagation orbit type
  427.             final Orbit initialOrbit = stateMapper.getOrbitType().convertType(getInitialState().getOrbit());
  428.             if (Double.isNaN(getMu())) {
  429.                 setMu(initialOrbit.getMu());
  430.             }

  432.             if (getInitialState().getMass() <= 0.0) {
  433.                 throw new PropagationException(OrekitMessages.SPACECRAFT_MASS_BECOMES_NEGATIVE,
  434.                                                getInitialState().getMass());
  435.             }

  437.             integrator.clearEventHandlers();

  439.             // set up events added by user
  440.             setUpUserEventDetectors();

  442.             // convert space flight dynamics API to math API
  443.             mathODE = createODE(integrator);
  444.             mathInterpolator = null;

  446.             // initialize mode handler
  447.             if (modeHandler != null) {
  448.                 modeHandler.initialize(activateHandlers, tEnd);
  449.             }

  451.             // mathematical integration
  452.             try {
  453.                 beforeIntegration(getInitialState(), tEnd);
  454.                 integrator.integrate(mathODE, tEnd.durationFrom(getInitialState().getDate()));
  455.                 afterIntegration();
  456.             } catch (OrekitExceptionWrapper oew) {
  457.                 throw oew.getException();
  458.             }

  460.             // get final state
  461.             SpacecraftState finalState = stateMapper.mapArrayToState(
  462.                     stateMapper.mapDoubleToDate(mathODE.getTime(), tEnd),
  463.                     mathODE.getPrimaryState(),
  464.                     meanOrbit);
  465.             finalState = updateAdditionalStates(finalState);
  466.             for (int i = 0; i < additionalEquations.size(); ++i) {
  467.                 final double[] secondary = mathODE.getSecondaryState(i);
  468.                 finalState = finalState.addAdditionalState(additionalEquations.get(i).getName(),
  469.                                                            secondary);
  470.             }
  471.             resetInitialState(finalState);
  472.             setStartDate(finalState.getDate());

  474.             return finalState;

  476.         } catch (PropagationException pe) {
  477.             throw pe;
  478.         } catch (OrekitException oe) {
  479.             throw new PropagationException(oe);
  480.         } catch (MathIllegalArgumentException miae) {
  481.             throw PropagationException.unwrap(miae);
  482.         } catch (MathIllegalStateException mise) {
  483.             throw PropagationException.unwrap(mise);
  484.         }
  485.     }

  487.     /** Get the initial state for integration.
  488.      * @return initial state for integration
  489.      * @exception PropagationException if initial state cannot be retrieved
  490.      */
  491.     protected SpacecraftState getInitialIntegrationState() throws PropagationException {
  492.         return getInitialState();
  493.     }

  495.     /** Create an ODE with all equations.
  496.      * @param integ numerical integrator to use for propagation.
  497.      * @return a new ode
  498.      * @exception OrekitException if initial state cannot be mapped
  499.      */
  500.     private ExpandableStatefulODE createODE(final AbstractIntegrator integ)
  501.         throws OrekitException {

  503.         // retrieve initial state
  504.         final double[] initialStateVector  = new double[getBasicDimension()];
  505.         stateMapper.mapStateToArray(getInitialIntegrationState(), initialStateVector);

  507.         // main part of the ODE
  508.         final ExpandableStatefulODE ode =
  509.                 new ExpandableStatefulODE(new ConvertedMainStateEquations(getMainStateEquations(integ)));
  510.         ode.setTime(0.0);
  511.         ode.setPrimaryState(initialStateVector);

  513.         // secondary part of the ODE
  514.         for (int i = 0; i < additionalEquations.size(); ++i) {
  515.             final AdditionalEquations additional = additionalEquations.get(i);
  516.             final double[] data = getInitialState().getAdditionalState(additional.getName());
  517.             final SecondaryEquations secondary =
  518.                     new ConvertedSecondaryStateEquations(additional, data.length);
  519.             ode.addSecondaryEquations(secondary);
  520.             ode.setSecondaryState(i, data);
  521.         }

  523.         return ode;

  525.     }

  527.     /** Method called just before integration.
  528.      * <p>
  529.      * The default implementation does nothing, it may be specialized in subclasses.
  530.      * </p>
  531.      * @param initialState initial state
  532.      * @param tEnd target date at which state should be propagated
  533.      * @exception OrekitException if hook cannot be run
  534.      */
  535.     protected void beforeIntegration(final SpacecraftState initialState,
  536.                                      final AbsoluteDate tEnd)
  537.         throws OrekitException {
  538.         // do nothing by default
  539.     }

  541.     /** Method called just after integration.
  542.      * <p>
  543.      * The default implementation does nothing, it may be specialized in subclasses.
  544.      * </p>
  545.      * @exception OrekitException if hook cannot be run
  546.      */
  547.     protected void afterIntegration()
  548.         throws OrekitException {
  549.         // do nothing by default
  550.     }

  552.     /** Get state vector dimension without additional parameters.
  553.      * @return state vector dimension without additional parameters.
  554.      */
  555.     public int getBasicDimension() {
  556.         return 7;

  558.     }

  560.     /** Get the integrator used by the propagator.
  561.      * @return the integrator.
  562.      */
  563.     protected AbstractIntegrator getIntegrator() {
  564.         return integrator;
  565.     }

  567.     /** Get a complete state with all additional equations.
  568.      * @param t current value of the independent <I>time</I> variable
  569.      * @param y array containing the current value of the state vector
  570.      * @return complete state
  571.      * @exception OrekitException if state cannot be mapped
  572.      */
  573.     private SpacecraftState getCompleteState(final double t, final double[] y)
  574.         throws OrekitException {

  576.         // main state
  577.         SpacecraftState state = stateMapper.mapArrayToState(t, y, true);  //not sure of the mean orbit, should be true

  579.         // pre-integrated additional states
  580.         state = updateAdditionalStates(state);

  582.         // additional states integrated here
  583.         if (!additionalEquations.isEmpty()) {

  585.             final EquationsMapper[] em = mathODE.getSecondaryMappers();
  586.             for (int i = 0; i < additionalEquations.size(); ++i) {
  587.                 final double[] secondary = new double[em[i].getDimension()];
  588.                 System.arraycopy(y, em[i].getFirstIndex(), secondary, 0, secondary.length);
  589.                 state = state.addAdditionalState(additionalEquations.get(i).getName(), secondary);
  590.             }

  592.         }

  594.         return state;

  596.     }

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

  601.         /** Compute differential equations for main state.
  602.          * @param state current state
  603.          * @return derivatives of main state
  604.          * @throws OrekitException if differentials cannot be computed
  605.          */
  606.         double[] computeDerivatives(final SpacecraftState state) throws OrekitException;

  608.     }

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

  613.         /** Main state equations. */
  614.         private final MainStateEquations main;

  616.         /** Simple constructor.
  617.          * @param main main state equations
  618.          */
  619.         ConvertedMainStateEquations(final MainStateEquations main) {
  620.             this.main = main;
  621.             calls = 0;
  622.         }

  624.         /** {@inheritDoc} */
  625.         public int getDimension() {
  626.             return getBasicDimension();
  627.         }

  629.         /** {@inheritDoc} */
  630.         public void computeDerivatives(final double t, final double[] y, final double[] yDot)
  631.             throws OrekitExceptionWrapper {

  633.             try {

  635.                 // update space dynamics view
  636.                 // use only ODE elements
  637.                 SpacecraftState currentState = stateMapper.mapArrayToState(t, y, true);
  638.                 currentState = updateAdditionalStates(currentState);

  640.                 // compute main state differentials
  641.                 final double[] mainDot = main.computeDerivatives(currentState);
  642.                 System.arraycopy(mainDot, 0, yDot, 0, mainDot.length);

  644.             } catch (OrekitException oe) {
  645.                 throw new OrekitExceptionWrapper(oe);
  646.             }


  649.             // increment calls counter
  650.             ++calls;

  652.         }

  654.     }

  656.     /** Differential equations for the secondary state (Jacobians, user variables ...), with converted API. */
  657.     private class ConvertedSecondaryStateEquations implements SecondaryEquations {

  659.         /** Additional equations. */
  660.         private final AdditionalEquations equations;

  662.         /** Dimension of the additional state. */
  663.         private final int dimension;

  665.         /** Simple constructor.
  666.          * @param equations additional equations
  667.          * @param dimension dimension of the additional state
  668.          */
  669.         ConvertedSecondaryStateEquations(final AdditionalEquations equations,
  670.                                                 final int dimension) {
  671.             this.equations = equations;
  672.             this.dimension = dimension;
  673.         }

  675.         /** {@inheritDoc} */
  676.         public int getDimension() {
  677.             return dimension;
  678.         }

  680.         /** {@inheritDoc} */
  681.         public void computeDerivatives(final double t, final double[] primary,
  682.                                        final double[] primaryDot, final double[] secondary,
  683.                                        final double[] secondaryDot)
  684.             throws OrekitExceptionWrapper {

  686.             try {

  688.                 // update space dynamics view
  689.                 // the state contains only the ODE elements
  690.                 SpacecraftState currentState = stateMapper.mapArrayToState(t, primary, true);
  691.                 currentState = updateAdditionalStates(currentState);
  692.                 currentState = currentState.addAdditionalState(equations.getName(), secondary);

  694.                 // compute additional derivatives
  695.                 final double[] additionalMainDot =
  696.                         equations.computeDerivatives(currentState, secondaryDot);
  697.                 if (additionalMainDot != null) {
  698.                     // the additional equations have an effect on main equations
  699.                     for (int i = 0; i < additionalMainDot.length; ++i) {
  700.                         primaryDot[i] += additionalMainDot[i];
  701.                     }
  702.                 }

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

  708.         }

  710.     }

  712.     /** Adapt an {@link org.orekit.propagation.events.EventDetector}
  713.      * to commons-math {@link org.apache.commons.math3.ode.events.EventHandler} interface.
  714.      * @param <T> class type for the generic version
  715.      * @author Fabien Maussion
  716.      */
  717.     private class AdaptedEventDetector
  718.         implements org.apache.commons.math3.ode.events.EventHandler {

  720.         /** Underlying event detector. */
  721.         private final EventDetector detector;

  723.         /** Time of the previous call to g. */
  724.         private double lastT;

  726.         /** Value from the previous call to g. */
  727.         private double lastG;

  729.         /** Build a wrapped event detector.
  730.          * @param detector event detector to wrap
  731.         */
  732.         AdaptedEventDetector(final EventDetector detector) {
  733.             this.detector = detector;
  734.             this.lastT    = Double.NaN;
  735.             this.lastG    = Double.NaN;
  736.         }

  738.         /** {@inheritDoc} */
  739.         public void init(final double t0, final double[] y0, final double t) {
  740.             try {

  742.                 detector.init(getCompleteState(t0, y0), stateMapper.mapDoubleToDate(t));
  743.                 this.lastT = Double.NaN;
  744.                 this.lastG = Double.NaN;

  746.             } catch (OrekitException oe) {
  747.                 throw new OrekitExceptionWrapper(oe);
  748.             }
  749.         }

  751.         /** {@inheritDoc} */
  752.         public double g(final double t, final double[] y) {
  753.             try {
  754.                 if (!Precision.equals(lastT, t, 1)) {
  755.                     lastT = t;
  756.                     lastG = detector.g(getCompleteState(t, y));
  757.                 }
  758.                 return lastG;
  759.             } catch (OrekitException oe) {
  760.                 throw new OrekitExceptionWrapper(oe);
  761.             }
  762.         }

  764.         /** {@inheritDoc} */
  765.         public Action eventOccurred(final double t, final double[] y, final boolean increasing) {
  766.             try {

  768.                 final EventHandler.Action whatNext = detector.eventOccurred(getCompleteState(t, y), increasing);

  770.                 switch (whatNext) {
  771.                     case STOP :
  772.                         return Action.STOP;
  773.                     case RESET_STATE :
  774.                         return Action.RESET_STATE;
  775.                     case RESET_DERIVATIVES :
  776.                         return Action.RESET_DERIVATIVES;
  777.                     default :
  778.                         return Action.CONTINUE;
  779.                 }
  780.             } catch (OrekitException oe) {
  781.                 throw new OrekitExceptionWrapper(oe);
  782.             }
  783.         }

  785.         /** {@inheritDoc} */
  786.         public void resetState(final double t, final double[] y) {
  787.             try {
  788.                 final SpacecraftState newState = detector.resetState(getCompleteState(t, y));

  790.                 // main part
  791.                 stateMapper.mapStateToArray(newState, y);

  793.                 // secondary part
  794.                 final EquationsMapper[] em = mathODE.getSecondaryMappers();
  795.                 for (int i = 0; i < additionalEquations.size(); ++i) {
  796.                     final double[] secondary =
  797.                             newState.getAdditionalState(additionalEquations.get(i).getName());
  798.                     System.arraycopy(secondary, 0, y, em[i].getFirstIndex(), secondary.length);
  799.                 }

  801.             } catch (OrekitException oe) {
  802.                 throw new OrekitExceptionWrapper(oe);
  803.             }
  804.         }

  806.     }

  808.     /** Adapt an {@link org.orekit.propagation.sampling.OrekitStepHandler}
  809.      * to commons-math {@link StepHandler} interface.
  810.      * @author Luc Maisonobe
  811.      */
  812.     private class AdaptedStepHandler
  813.         implements OrekitStepInterpolator, StepHandler, ModeHandler {

  815.         /** Underlying handler. */
  816.         private final OrekitStepHandler handler;

  818.         /** Flag for handler . */
  819.         private boolean activate;

  821.         /** Build an instance.
  822.          * @param handler underlying handler to wrap
  823.          */
  824.         AdaptedStepHandler(final OrekitStepHandler handler) {
  825.             this.handler = handler;
  826.         }

  828.         /** {@inheritDoc} */
  829.         public void initialize(final boolean activateHandlers,
  830.                                final AbsoluteDate targetDate) {
  831.             this.activate = activateHandlers;
  832.         }

  834.         /** {@inheritDoc} */
  835.         public void init(final double t0, final double[] y0, final double t) {
  836.             try {
  837.                 handler.init(getCompleteState(t0, y0), stateMapper.mapDoubleToDate(t));
  838.             } catch (OrekitException oe) {
  839.                 throw new OrekitExceptionWrapper(oe);
  840.             }
  841.         }

  843.         /** {@inheritDoc} */
  844.         public void handleStep(final StepInterpolator interpolator, final boolean isLast) {
  845.             try {
  846.                 mathInterpolator = interpolator;
  847.                 if (activate) {
  848.                     handler.handleStep(this, isLast);
  849.                 }
  850.             } catch (PropagationException pe) {
  851.                 throw new OrekitExceptionWrapper(pe);
  852.             }
  853.         }

  855.         /** Get the current grid date.
  856.          * @return current grid date
  857.          */
  858.         public AbsoluteDate getCurrentDate() {
  859.             return stateMapper.mapDoubleToDate(mathInterpolator.getCurrentTime());
  860.         }

  862.         /** Get the previous grid date.
  863.          * @return previous grid date
  864.          */
  865.         public AbsoluteDate getPreviousDate() {
  866.             return stateMapper.mapDoubleToDate(mathInterpolator.getPreviousTime());
  867.         }

  869.         /** Get the interpolated date.
  870.          * <p>If {@link #setInterpolatedDate(AbsoluteDate) setInterpolatedDate}
  871.          * has not been called, the date returned is the same as  {@link
  872.          * #getCurrentDate() getCurrentDate}.</p>
  873.          * @return interpolated date
  874.          * @see #setInterpolatedDate(AbsoluteDate)
  875.          * @see #getInterpolatedState()
  876.          */
  877.         public AbsoluteDate getInterpolatedDate() {
  878.             return stateMapper.mapDoubleToDate(mathInterpolator.getInterpolatedTime());
  879.         }

  881.         /** Set the interpolated date.
  882.          * <p>It is possible to set the interpolation date outside of the current
  883.          * step range, but accuracy will decrease as date is farther.</p>
  884.          * @param date interpolated date to set
  885.          * @see #getInterpolatedDate()
  886.          * @see #getInterpolatedState()
  887.          */
  888.         public void setInterpolatedDate(final AbsoluteDate date) {
  889.             mathInterpolator.setInterpolatedTime(stateMapper.mapDateToDouble(date));
  890.         }

  892.         /** Get the interpolated state.
  893.          * @return interpolated state at the current interpolation date
  894.          * @exception OrekitException if state cannot be interpolated or converted
  895.          * @see #getInterpolatedDate()
  896.          * @see #setInterpolatedDate(AbsoluteDate)
  897.          */
  898.         public SpacecraftState getInterpolatedState() throws OrekitException {
  899.             try {

  901.                 SpacecraftState s =
  902.                         stateMapper.mapArrayToState(mathInterpolator.getInterpolatedTime(),
  903.                                                     mathInterpolator.getInterpolatedState(),
  904.                                                     meanOrbit);
  905.                 s = updateAdditionalStates(s);
  906.                 for (int i = 0; i < additionalEquations.size(); ++i) {
  907.                     final double[] secondary = mathInterpolator.getInterpolatedSecondaryState(i);
  908.                     s = s.addAdditionalState(additionalEquations.get(i).getName(), secondary);
  909.                 }

  911.                 return s;

  913.             } catch (OrekitExceptionWrapper oew) {
  914.                 throw oew.getException();
  915.             }
  916.         }

  918.         /** Check is integration direction is forward in date.
  919.          * @return true if integration is forward in date
  920.          */
  921.         public boolean isForward() {
  922.             return mathInterpolator.isForward();
  923.         }

  925.     }

  927.     private class EphemerisModeHandler implements ModeHandler, StepHandler {

  929.         /** Underlying raw mathematical model. */
  930.         private ContinuousOutputModel model;

  932.         /** Generated ephemeris. */
  933.         private BoundedPropagator ephemeris;

  935.         /** Flag for handler . */
  936.         private boolean activate;

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

  941.         /** Creates a new instance of EphemerisModeHandler which must be
  942.          *  filled by the propagator.
  943.          */
  944.         EphemerisModeHandler() {
  945.         }

  947.         /** {@inheritDoc} */
  948.         public void initialize(final boolean activateHandlers,
  949.                                final AbsoluteDate targetDate) {
  950.             this.activate = activateHandlers;
  951.             this.model    = new ContinuousOutputModel();
  952.             this.endDate  = targetDate;

  954.             // ephemeris will be generated when last step is processed
  955.             this.ephemeris = null;

  957.         }

  959.         /** Get the generated ephemeris.
  960.          * @return a new instance of the generated ephemeris
  961.          */
  962.         public BoundedPropagator getEphemeris() {
  963.             return ephemeris;
  964.         }

  966.         /** {@inheritDoc} */
  967.         public void handleStep(final StepInterpolator interpolator, final boolean isLast)
  968.             throws OrekitExceptionWrapper {
  969.             try {
  970.                 if (activate) {
  971.                     model.handleStep(interpolator, isLast);
  972.                     if (isLast) {

  974.                         // set up the boundary dates
  975.                         final double tI = model.getInitialTime();
  976.                         final double tF = model.getFinalTime();
  977.                         // tI is almost? always zero
  978.                         final AbsoluteDate startDate =
  979.                                 stateMapper.mapDoubleToDate(tI);
  980.                         final AbsoluteDate finalDate =
  981.                                 stateMapper.mapDoubleToDate(tF, this.endDate);
  982.                         final AbsoluteDate minDate;
  983.                         final AbsoluteDate maxDate;
  984.                         if (tF < tI) {
  985.                             minDate = finalDate;
  986.                             maxDate = startDate;
  987.                         } else {
  988.                             minDate = startDate;
  989.                             maxDate = finalDate;
  990.                         }

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

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

  1008.                         // create the ephemeris
  1009.                         ephemeris = new IntegratedEphemeris(startDate, minDate, maxDate,
  1010.                                                             stateMapper, meanOrbit, model, unmanaged,
  1011.                                                             getAdditionalStateProviders(), names);

  1013.                     }
  1014.                 }
  1015.             } catch (OrekitException oe) {
  1016.                 throw new OrekitExceptionWrapper(oe);
  1017.             }
  1018.         }

  1020.         /** {@inheritDoc} */
  1021.         public void init(final double t0, final double[] y0, final double t) {
  1022.             model.init(t0, y0, t);
  1023.         }

  1025.     }

  1027. }
Created with JaCoCo 0.7.4.201502262128