AbstractIntegratedPropagator.java

  1. /* Copyright 2002-2013 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.nonstiff.ClassicalRungeKuttaIntegrator;
  35. import org.apache.commons.math3.ode.sampling.StepHandler;
  36. import org.apache.commons.math3.ode.sampling.StepInterpolator;
  37. import org.apache.commons.math3.util.FastMath;
  38. import org.apache.commons.math3.util.Precision;
  39. import org.orekit.attitudes.AttitudeProvider;
  40. import org.orekit.errors.OrekitException;
  41. import org.orekit.errors.OrekitExceptionWrapper;
  42. import org.orekit.errors.OrekitMessages;
  43. import org.orekit.errors.PropagationException;
  44. import org.orekit.frames.Frame;
  45. import org.orekit.orbits.Orbit;
  46. import org.orekit.orbits.OrbitType;
  47. import org.orekit.orbits.PositionAngle;
  48. import org.orekit.propagation.AbstractPropagator;
  49. import org.orekit.propagation.BoundedPropagator;
  50. import org.orekit.propagation.SpacecraftState;
  51. import org.orekit.propagation.events.AbstractReconfigurableDetector;
  52. import org.orekit.propagation.events.EventDetector;
  53. import org.orekit.propagation.events.handlers.EventHandler;
  54. import org.orekit.propagation.sampling.OrekitStepHandler;
  55. import org.orekit.propagation.sampling.OrekitStepInterpolator;
  56. import org.orekit.time.AbsoluteDate;


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

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

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

  71.     /** Mode handler. */
  72.     private ModeHandler modeHandler;

  74.     /** Additional equations. */
  75.     private List<AdditionalEquations> additionalEquations;

  77.     /** Counter for differential equations calls. */
  78.     private int calls;

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

  83.     /** Complete equation to be integrated. */
  84.     private ExpandableStatefulODE mathODE;

  86.     /** Underlying raw rawInterpolator. */
  87.     private StepInterpolator mathInterpolator;

  89.     /** Build a new instance.
  90.      * @param integrator numerical integrator to use for propagation.
  91.      */
  92.     protected AbstractIntegratedPropagator(final AbstractIntegrator integrator) {
  93.         detectors           = new ArrayList<EventDetector>();
  94.         additionalEquations = new ArrayList<AdditionalEquations>();
  95.         this.integrator     = integrator;
  96.     }

  98.     /** Initialize the mapper. */
  99.     protected void initMapper() {
  100.         stateMapper = createMapper(null, Double.NaN, null, null, null, null);
  101.     }

  103.     /**  {@inheritDoc} */
  104.     public void setAttitudeProvider(final AttitudeProvider attitudeProvider) {
  105.         super.setAttitudeProvider(attitudeProvider);
  106.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  107.                                    stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  108.                                    attitudeProvider, stateMapper.getFrame());
  109.     }

  111.     /** Set propagation orbit type.
  112.      * @param orbitType orbit type to use for propagation
  113.      */
  114.     protected void setOrbitType(final OrbitType orbitType) {
  115.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  116.                                    orbitType, stateMapper.getPositionAngleType(),
  117.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  118.     }

  120.     /** Get propagation parameter type.
  121.      * @return orbit type used for propagation
  122.      */
  123.     protected OrbitType getOrbitType() {
  124.         return stateMapper.getOrbitType();
  125.     }

  127.     /** Set position angle type.
  128.      * <p>
  129.      * The position parameter type is meaningful only if {@link
  130.      * #getOrbitType() propagation orbit type}
  131.      * support it. As an example, it is not meaningful for propagation
  132.      * in {@link OrbitType#CARTESIAN Cartesian} parameters.
  133.      * </p>
  134.      * @param positionAngleType angle type to use for propagation
  135.      */
  136.     protected void setPositionAngleType(final PositionAngle positionAngleType) {
  137.         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
  138.                                    stateMapper.getOrbitType(), positionAngleType,
  139.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  140.     }

  142.     /** Get propagation parameter type.
  143.      * @return angle type to use for propagation
  144.      */
  145.     protected PositionAngle getPositionAngleType() {
  146.         return stateMapper.getPositionAngleType();
  147.     }

  149.     /** Set the central attraction coefficient &mu;.
  150.      * @param mu central attraction coefficient (m<sup>3</sup>/s<sup>2</sup>)
  151.      */
  152.     public void setMu(final double mu) {
  153.         stateMapper = createMapper(stateMapper.getReferenceDate(), mu,
  154.                                    stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  155.                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
  156.     }

  158.     /** Get the central attraction coefficient &mu;.
  159.      * @return mu central attraction coefficient (m<sup>3</sup>/s<sup>2</sup>)
  160.      * @see #setMu(double)
  161.      */
  162.     public double getMu() {
  163.         return stateMapper.getMu();
  164.     }

  166.     /** Get the number of calls to the differential equations computation method.
  167.      * <p>The number of calls is reset each time the {@link #propagate(AbsoluteDate)}
  168.      * method is called.</p>
  169.      * @return number of calls to the differential equations computation method
  170.      */
  171.     public int getCalls() {
  172.         return calls;
  173.     }

  175.     /** {@inheritDoc} */
  176.     @Override
  177.     public boolean isAdditionalStateManaged(final String name) {

  179.         // first look at already integrated states
  180.         if (super.isAdditionalStateManaged(name)) {
  181.             return true;
  182.         }

  184.         // then look at states we integrate ourselves
  185.         for (final AdditionalEquations equation : additionalEquations) {
  186.             if (equation.getName().equals(name)) {
  187.                 return true;
  188.             }
  189.         }

  191.         return false;
  192.     }

  194.     /** {@inheritDoc} */
  195.     @Override
  196.     public String[] getManagedAdditionalStates() {
  197.         final String[] alreadyIntegrated = super.getManagedAdditionalStates();
  198.         final String[] managed = new String[alreadyIntegrated.length + additionalEquations.size()];
  199.         System.arraycopy(alreadyIntegrated, 0, managed, 0, alreadyIntegrated.length);
  200.         for (int i = 0; i < additionalEquations.size(); ++i) {
  201.             managed[i + alreadyIntegrated.length] = additionalEquations.get(i).getName();
  202.         }
  203.         return managed;
  204.     }

  206.     /** Add a set of user-specified equations to be integrated along with the orbit propagation.
  207.      * @param additional additional equations
  208.      * @exception OrekitException if a set of equations with the same name is already present
  209.      */
  210.     public void addAdditionalEquations(final AdditionalEquations additional)
  211.         throws OrekitException {

  213.         // check if the name is already used
  214.         if (isAdditionalStateManaged(additional.getName())) {
  215.             // this set of equations is already registered, complain
  216.             throw new OrekitException(OrekitMessages.ADDITIONAL_STATE_NAME_ALREADY_IN_USE,
  217.                                       additional.getName());
  218.         }

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

  223.     }

  225.     /** {@inheritDoc} */
  226.     public <T extends EventDetector> void addEventDetector(final T detector) {
  227.         detectors.add(detector);
  228.     }

  230.     /** {@inheritDoc} */
  231.     public Collection<EventDetector> getEventsDetectors() {
  232.         return Collections.unmodifiableCollection(detectors);
  233.     }

  235.     /** {@inheritDoc} */
  236.     public void clearEventsDetectors() {
  237.         detectors.clear();
  238.     }

  240.     /** Set up all user defined event detectors.
  241.      */
  242.     protected void setUpUserEventDetectors() {
  243.         for (final EventDetector detector : detectors) {
  244.             setUpEventDetector(integrator, detector);
  245.         }
  246.     }

  248.     /** Wrap an Orekit event detector and register it to the integrator.
  249.      * @param integ integrator into which event detector should be registered
  250.      * @param detector event detector to wrap
  251.      * @param <T> class type for the generic version
  252.      */
  253.     protected <T extends EventDetector> void setUpEventDetector(final AbstractIntegrator integ,
  254.                                                                 final T detector) {
  255.         integ.addEventHandler(new AdaptedEventDetector<T>(detector),
  256.                               detector.getMaxCheckInterval(),
  257.                               detector.getThreshold(),
  258.                               detector.getMaxIterationCount());
  259.     }

  261.     /** {@inheritDoc}
  262.      * <p>Note that this method has the side effect of replacing the step handlers
  263.      * of the underlying integrator set up in the {@link
  264.      * #AbstractIntegratedPropagator(AbstractIntegrator) constructor}. So if a specific
  265.      * step handler is needed, it should be added after this method has been callled.</p>
  266.      */
  267.     public void setSlaveMode() {
  268.         super.setSlaveMode();
  269.         if (integrator != null) {
  270.             integrator.clearStepHandlers();
  271.         }
  272.         modeHandler = null;
  273.     }

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

  289.     /** {@inheritDoc}
  290.      * <p>Note that this method has the side effect of replacing the step handlers
  291.      * of the underlying integrator set up in the {@link
  292.      * #AbstractIntegratedPropagator(AbstractIntegrator) constructor}. So if a specific
  293.      * step handler is needed, it should be added after this method has been called.</p>
  294.      */
  295.     public void setEphemerisMode() {
  296.         super.setEphemerisMode();
  297.         integrator.clearStepHandlers();
  298.         final EphemerisModeHandler ephemeris = new EphemerisModeHandler();
  299.         modeHandler = ephemeris;
  300.         integrator.addStepHandler(ephemeris);
  301.     }

  303.     /** {@inheritDoc} */
  304.     public BoundedPropagator getGeneratedEphemeris()
  305.         throws IllegalStateException {
  306.         if (getMode() != EPHEMERIS_GENERATION_MODE) {
  307.             throw OrekitException.createIllegalStateException(OrekitMessages.PROPAGATOR_NOT_IN_EPHEMERIS_GENERATION_MODE);
  308.         }
  309.         return ((EphemerisModeHandler) modeHandler).getEphemeris();
  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<sup>3</sup>/s<sup>2</sup>)
  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 StateMapper createMapper(final AbsoluteDate referenceDate, final double mu,
  329.                                                 final OrbitType orbitType, final PositionAngle positionAngleType,
  330.                                                 final AttitudeProvider attitudeProvider, final 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 getMainStateEquations(final AbstractIntegrator integ);

  338.     /** {@inheritDoc} */
  339.     public SpacecraftState propagate(final AbsoluteDate target) throws PropagationException {
  340.         try {
  341.             if (getStartDate() == null) {
  342.                 if (getInitialState() == null) {
  343.                     throw new PropagationException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
  344.                 }
  345.                 setStartDate(getInitialState().getDate());
  346.             }
  347.             return propagate(getStartDate(), target);
  348.         } catch (OrekitException oe) {

  350.             // recover a possible embedded PropagationException
  351.             for (Throwable t = oe; t != null; t = t.getCause()) {
  352.                 if (t instanceof PropagationException) {
  353.                     throw (PropagationException) t;
  354.                 }
  355.             }
  356.             throw new PropagationException(oe);

  358.         }
  359.     }

  361.     /** {@inheritDoc} */
  362.     public SpacecraftState propagate(final AbsoluteDate tStart, final AbsoluteDate tEnd)
  363.         throws PropagationException {
  364.         try {

  366.             if (getInitialState() == null) {
  367.                 throw new PropagationException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
  368.             }

  370.             if (!tStart.equals(getInitialState().getDate())) {
  371.                 // if propagation start date is not initial date,
  372.                 // propagate from initial to start date without event detection
  373.                 propagate(tStart, false);
  374.             }

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

  379.         } catch (OrekitException oe) {

  381.             // recover a possible embedded PropagationException
  382.             for (Throwable t = oe; t != null; t = t.getCause()) {
  383.                 if (t instanceof PropagationException) {
  384.                     throw (PropagationException) t;
  385.                 }
  386.             }
  387.             throw new PropagationException(oe);

  389.         }
  390.     }

  392.     /** Propagation with or without event detection.
  393.      * @param tEnd target date to which orbit should be propagated
  394.      * @param activateHandlers if true, step and event handlers should be activated
  395.      * @return state at end of propagation
  396.      * @exception PropagationException if orbit cannot be propagated
  397.      */
  398.     protected SpacecraftState propagate(final AbsoluteDate tEnd, final boolean activateHandlers)
  399.         throws PropagationException {
  400.         try {

  402.             if (getInitialState().getDate().equals(tEnd)) {
  403.                 // don't extrapolate
  404.                 return getInitialState();
  405.             }

  407.             // space dynamics view
  408.             stateMapper = createMapper(getInitialState().getDate(), stateMapper.getMu(),
  409.                                        stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
  410.                                        stateMapper.getAttitudeProvider(), getInitialState().getFrame());


  413.             // set propagation orbit type
  414.             final Orbit initialOrbit = stateMapper.getOrbitType().convertType(getInitialState().getOrbit());
  415.             if (Double.isNaN(getMu())) {
  416.                 setMu(initialOrbit.getMu());
  417.             }

  419.             if (getInitialState().getMass() <= 0.0) {
  420.                 throw new PropagationException(OrekitMessages.SPACECRAFT_MASS_BECOMES_NEGATIVE,
  421.                                                getInitialState().getMass());
  422.             }

  424.             integrator.clearEventHandlers();

  426.             // set up events added by user
  427.             setUpUserEventDetectors();

  429.             // convert space flight dynamics API to math API
  430.             mathODE = createODE(integrator);
  431.             mathInterpolator = null;

  433.             // initialize mode handler
  434.             if (modeHandler != null) {
  435.                 modeHandler.initialize(activateHandlers);
  436.             }

  438.             // mathematical integration
  439.             try {
  440.                 beforeIntegration(getInitialState(), tEnd);
  441.                 integrator.integrate(mathODE, tEnd.durationFrom(getInitialState().getDate()));
  442.                 afterIntegration();
  443.             } catch (OrekitExceptionWrapper oew) {
  444.                 throw oew.getException();
  445.             }

  447.             // get final state
  448.             SpacecraftState finalState =
  449.                     stateMapper.mapArrayToState(mathODE.getTime(), mathODE.getPrimaryState());
  450.             finalState = updateAdditionalStates(finalState);
  451.             for (int i = 0; i < additionalEquations.size(); ++i) {
  452.                 final double[] secondary = mathODE.getSecondaryState(i);
  453.                 finalState = finalState.addAdditionalState(additionalEquations.get(i).getName(),
  454.                                                            secondary);
  455.             }
  456.             resetInitialState(finalState);
  457.             setStartDate(finalState.getDate());

  459.             return finalState;

  461.         } catch (PropagationException pe) {
  462.             throw pe;
  463.         } catch (OrekitException oe) {
  464.             throw new PropagationException(oe);
  465.         } catch (MathIllegalArgumentException miae) {
  466.             throw PropagationException.unwrap(miae);
  467.         } catch (MathIllegalStateException mise) {
  468.             throw PropagationException.unwrap(mise);
  469.         }
  470.     }

  472.     /** Create an ODE with all equations.
  473.      * @param integ numerical integrator to use for propagation.
  474.      * @return a new ode
  475.      * @exception OrekitException if initial state cannot be mapped
  476.      */
  477.     private ExpandableStatefulODE createODE(final AbstractIntegrator integ)
  478.         throws OrekitException {

  480.         // retrieve initial state
  481.         final double[] initialStateVector  = new double[getBasicDimension()];
  482.         stateMapper.mapStateToArray(getInitialState(), initialStateVector);

  484.         // main part of the ODE
  485.         final ExpandableStatefulODE ode =
  486.                 new ExpandableStatefulODE(new ConvertedMainStateEquations(getMainStateEquations(integ)));
  487.         ode.setTime(0.0);
  488.         ode.setPrimaryState(initialStateVector);

  490.         // secondary part of the ODE
  491.         for (int i = 0; i < additionalEquations.size(); ++i) {
  492.             final AdditionalEquations additional = additionalEquations.get(i);
  493.             final double[] data = getInitialState().getAdditionalState(additional.getName());
  494.             final SecondaryEquations secondary =
  495.                     new ConvertedSecondaryStateEquations(additional, data.length);
  496.             ode.addSecondaryEquations(secondary);
  497.             ode.setSecondaryState(i, data);
  498.         }

  500.         return ode;

  502.     }

  504.     /** Method called just before integration.
  505.      * <p>
  506.      * The default implementation does nothing, it may be specialized in subclasses.
  507.      * </p>
  508.      * @param initialState initial state
  509.      * @param tEnd target date at which state should be propagated
  510.      * @exception OrekitException if hook cannot be run
  511.      */
  512.     protected void beforeIntegration(final SpacecraftState initialState,
  513.                                      final AbsoluteDate tEnd)
  514.         throws OrekitException {
  515.         // do nothing by default
  516.     }

  518.     /** Method called just after integration.
  519.      * <p>
  520.      * The default implementation does nothing, it may be specialized in subclasses.
  521.      * </p>
  522.      * @exception OrekitException if hook cannot be run
  523.      */
  524.     protected void afterIntegration()
  525.         throws OrekitException {
  526.         // do nothing by default
  527.     }

  529.     /** Get state vector dimension without additional parameters.
  530.      * @return state vector dimension without additional parameters.
  531.      */
  532.     public int getBasicDimension() {
  533.         return 7;

  535.     }

  537.     /** Get a complete state with all additional equations.
  538.      * @param t current value of the independent <I>time</I> variable
  539.      * @param y array containing the current value of the state vector
  540.      * @return complete state
  541.      * @exception OrekitException if state cannot be mapped
  542.      */
  543.     private SpacecraftState getCompleteState(final double t, final double[] y)
  544.         throws OrekitException {

  546.         // main state
  547.         SpacecraftState state = stateMapper.mapArrayToState(t, y);

  549.         // pre-integrated additional states
  550.         state = updateAdditionalStates(state);

  552.         // additional states integrated here
  553.         if (!additionalEquations.isEmpty()) {

  555.             if (mathODE.getTotalDimension() <= y.length) {
  556.                 // the provided y vector already contains everything needed
  557.                 final EquationsMapper[] em = mathODE.getSecondaryMappers();
  558.                 for (int i = 0; i < additionalEquations.size(); ++i) {
  559.                     final double[] secondary = new double[em[i].getDimension()];
  560.                     System.arraycopy(y, em[i].getFirstIndex(), secondary, 0, secondary.length);
  561.                     state = state.addAdditionalState(additionalEquations.get(i).getName(),
  562.                                                      secondary);
  563.                 }
  564.             } else {
  565.                 // the y array doesn't contain the additional equations data

  567.                 // TODO: remove this case when MATH-965 fix is officially published
  568.                 // (i.e for the next Apache Commons Math version after 3.2)
  569.                 // The fix for MATH-965 ensures that y always contains all
  570.                 // needed data, including additional states, so the workaround
  571.                 // below will not be needed anymore

  573.                 if (mathInterpolator == null) {
  574.                     // we are still in the first step, before the step handler call
  575.                     // we build a temporary interpolator just for this step
  576.                     final double step = FastMath.abs(integrator.getCurrentSignedStepsize());
  577.                     final ClassicalRungeKuttaIntegrator firstStepIntegrator =
  578.                             new ClassicalRungeKuttaIntegrator(step);
  579.                     firstStepIntegrator.addStepHandler(new StepHandler() {

  581.                         /** {@inheritDoc} */
  582.                         public void init(final double t0, final double[] y0, final double t) {
  583.                         }

  585.                         /** {@inheritDoc} */
  586.                         public void handleStep(final StepInterpolator interpolator, final boolean isLast) {
  587.                             mathInterpolator = interpolator;
  588.                         }

  590.                     });
  591.                     final ExpandableStatefulODE localODE = createODE(firstStepIntegrator);
  592.                     firstStepIntegrator.clearEventHandlers();
  593.                     firstStepIntegrator.integrate(localODE, step);
  594.                 }

  596.                 // extract the additional data from the spied interpolator
  597.                 mathInterpolator.setInterpolatedTime(t);
  598.                 for (int i = 0; i < additionalEquations.size(); ++i) {
  599.                     final double[] secondary = mathInterpolator.getInterpolatedSecondaryState(i);
  600.                     state = state.addAdditionalState(additionalEquations.get(i).getName(),
  601.                                                      secondary);
  602.                 }

  604.             }

  606.         }

  608.         return state;

  610.     }

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

  615.         /** Compute differential equations for main state.
  616.          * @param state current state
  617.          * @return derivatives of main state
  618.          * @throws OrekitException if differentials cannot be computed
  619.          */
  620.         double[] computeDerivatives(final SpacecraftState state) throws OrekitException;

  622.     }

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

  627.         /** Main state equations. */
  628.         private final MainStateEquations main;

  630.         /** Simple constructor.
  631.          * @param main main state equations
  632.          */
  633.         public ConvertedMainStateEquations(final MainStateEquations main) {
  634.             this.main = main;
  635.             calls = 0;
  636.         }

  638.         /** {@inheritDoc} */
  639.         public int getDimension() {
  640.             return getBasicDimension();
  641.         }

  643.         /** {@inheritDoc} */
  644.         public void computeDerivatives(final double t, final double[] y, final double[] yDot)
  645.             throws OrekitExceptionWrapper {

  647.             try {

  649.                 // update space dynamics view
  650.                 SpacecraftState currentState = stateMapper.mapArrayToState(t, y);
  651.                 currentState = updateAdditionalStates(currentState);

  653.                 // compute main state differentials
  654.                 final double[] mainDot = main.computeDerivatives(currentState);
  655.                 System.arraycopy(mainDot, 0, yDot, 0, mainDot.length);

  657.             } catch (OrekitException oe) {
  658.                 throw new OrekitExceptionWrapper(oe);
  659.             }


  662.             // increment calls counter
  663.             ++calls;

  665.         }

  667.     }

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

  672.         /** Additional equations. */
  673.         private final AdditionalEquations equations;

  675.         /** Dimension of the additional state. */
  676.         private final int dimension;

  678.         /** Simple constructor.
  679.          * @param equations additional equations
  680.          * @param dimension dimension of the additional state
  681.          */
  682.         public ConvertedSecondaryStateEquations(final AdditionalEquations equations,
  683.                                                 final int dimension) {
  684.             this.equations = equations;
  685.             this.dimension = dimension;
  686.         }

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

  693.         /** {@inheritDoc} */
  694.         public void computeDerivatives(final double t, final double[] primary,
  695.                                        final double[] primaryDot, final double[] secondary,
  696.                                        final double[] secondaryDot)
  697.             throws OrekitExceptionWrapper {

  699.             try {

  701.                 // update space dynamics view
  702.                 SpacecraftState currentState = stateMapper.mapArrayToState(t, primary);
  703.                 currentState = updateAdditionalStates(currentState);
  704.                 currentState = currentState.addAdditionalState(equations.getName(), secondary);

  706.                 // compute additional derivatives
  707.                 final double[] additionalMainDot =
  708.                         equations.computeDerivatives(currentState, secondaryDot);
  709.                 if (additionalMainDot != null) {
  710.                     // the additional equations have an effect on main equations
  711.                     for (int i = 0; i < additionalMainDot.length; ++i) {
  712.                         primaryDot[i] += additionalMainDot[i];
  713.                     }
  714.                 }

  716.             } catch (OrekitException oe) {
  717.                 throw new OrekitExceptionWrapper(oe);
  718.             }

  720.         }

  722.     }

  724.     /** Adapt an {@link org.orekit.propagation.events.EventDetector}
  725.      * to commons-math {@link org.apache.commons.math3.ode.events.EventHandler} interface.
  726.      * @param <T> class type for the generic version
  727.      * @author Fabien Maussion
  728.      */
  729.     private class AdaptedEventDetector<T extends EventDetector> implements org.apache.commons.math3.ode.events.EventHandler {

  731.         /** Underlying event detector. */
  732.         private final T detector;

  734.         /** Time of the previous call to g. */
  735.         private double lastT;

  737.         /** Value from the previous call to g. */
  738.         private double lastG;

  740.         /** Build a wrapped event detector.
  741.          * @param detector event detector to wrap
  742.         */
  743.         public AdaptedEventDetector(final T detector) {
  744.             this.detector = detector;
  745.             this.lastT    = Double.NaN;
  746.             this.lastG    = Double.NaN;
  747.         }

  749.         /** {@inheritDoc} */
  750.         public void init(final double t0, final double[] y0, final double t) {
  751.             try {

  753.                 detector.init(getCompleteState(t0, y0), stateMapper.mapDoubleToDate(t));
  754.                 this.lastT = Double.NaN;
  755.                 this.lastG = Double.NaN;

  757.             } catch (OrekitException oe) {
  758.                 throw new OrekitExceptionWrapper(oe);
  759.             }
  760.         }

  762.         /** {@inheritDoc} */
  763.         public double g(final double t, final double[] y) {
  764.             try {
  765.                 if (!Precision.equals(lastT, t, 1)) {
  766.                     lastT = t;
  767.                     lastG = detector.g(getCompleteState(t, y));
  768.                 }
  769.                 return lastG;
  770.             } catch (OrekitException oe) {
  771.                 throw new OrekitExceptionWrapper(oe);
  772.             }
  773.         }

  775.         /** {@inheritDoc} */
  776.         public Action eventOccurred(final double t, final double[] y, final boolean increasing) {
  777.             try {

  779.                 final SpacecraftState state = getCompleteState(t, y);
  780.                 final EventHandler.Action whatNext;
  781.                 if (detector instanceof AbstractReconfigurableDetector) {
  782.                     @SuppressWarnings("unchecked")
  783.                     final EventHandler<T> handler = ((AbstractReconfigurableDetector<T>) detector).getHandler();
  784.                     whatNext = handler.eventOccurred(state, detector, increasing);
  785.                 } else {
  786.                     @SuppressWarnings("deprecation")
  787.                     final EventDetector.Action a = detector.eventOccurred(state, increasing);
  788.                     whatNext = AbstractReconfigurableDetector.convert(a);
  789.                 }

  791.                 switch (whatNext) {
  792.                 case STOP :
  793.                     return Action.STOP;
  794.                 case RESET_STATE :
  795.                     return Action.RESET_STATE;
  796.                 case RESET_DERIVATIVES :
  797.                     return Action.RESET_DERIVATIVES;
  798.                 default :
  799.                     return Action.CONTINUE;
  800.                 }
  801.             } catch (OrekitException oe) {
  802.                 throw new OrekitExceptionWrapper(oe);
  803.             }
  804.         }

  806.         /** {@inheritDoc} */
  807.         public void resetState(final double t, final double[] y) {
  808.             try {
  809.                 final SpacecraftState oldState = getCompleteState(t, y);
  810.                 final SpacecraftState newState;
  811.                 if (detector instanceof AbstractReconfigurableDetector) {
  812.                     @SuppressWarnings("unchecked")
  813.                     final EventHandler<T> handler = ((AbstractReconfigurableDetector<T>) detector).getHandler();
  814.                     newState = handler.resetState(detector, oldState);
  815.                 } else {
  816.                     @SuppressWarnings("deprecation")
  817.                     final SpacecraftState s = detector.resetState(oldState);
  818.                     newState = s;
  819.                 }

  821.                 // main part
  822.                 stateMapper.mapStateToArray(newState, y);

  824.                 // secondary part
  825.                 final EquationsMapper[] em = mathODE.getSecondaryMappers();
  826.                 for (int i = 0; i < additionalEquations.size(); ++i) {
  827.                     final double[] secondary =
  828.                             newState.getAdditionalState(additionalEquations.get(i).getName());
  829.                     System.arraycopy(secondary, 0, y, em[i].getFirstIndex(), secondary.length);
  830.                 }

  832.             } catch (OrekitException oe) {
  833.                 throw new OrekitExceptionWrapper(oe);
  834.             }
  835.         }

  837.     }

  839.     /** Adapt an {@link org.orekit.propagation.sampling.OrekitStepHandler}
  840.      * to commons-math {@link StepHandler} interface.
  841.      * @author Luc Maisonobe
  842.      */
  843.     private class AdaptedStepHandler
  844.         implements OrekitStepInterpolator, StepHandler, ModeHandler {

  846.         /** Underlying handler. */
  847.         private final OrekitStepHandler handler;

  849.         /** Flag for handler . */
  850.         private boolean activate;

  852.         /** Build an instance.
  853.          * @param handler underlying handler to wrap
  854.          */
  855.         public AdaptedStepHandler(final OrekitStepHandler handler) {
  856.             this.handler = handler;
  857.         }

  859.         /** {@inheritDoc} */
  860.         public void initialize(final boolean activateHandlers) {
  861.             this.activate = activateHandlers;
  862.         }

  864.         /** {@inheritDoc} */
  865.         public void init(final double t0, final double[] y0, final double t) {
  866.             try {
  867.                 handler.init(getCompleteState(t0, y0), stateMapper.mapDoubleToDate(t));
  868.             } catch (OrekitException oe) {
  869.                 throw new OrekitExceptionWrapper(oe);
  870.             }
  871.         }

  873.         /** {@inheritDoc} */
  874.         public void handleStep(final StepInterpolator interpolator, final boolean isLast) {
  875.             try {
  876.                 mathInterpolator = interpolator;
  877.                 if (activate) {
  878.                     handler.handleStep(this, isLast);
  879.                 }
  880.             } catch (PropagationException pe) {
  881.                 throw new OrekitExceptionWrapper(pe);
  882.             }
  883.         }

  885.         /** Get the current grid date.
  886.          * @return current grid date
  887.          */
  888.         public AbsoluteDate getCurrentDate() {
  889.             return stateMapper.mapDoubleToDate(mathInterpolator.getCurrentTime());
  890.         }

  892.         /** Get the previous grid date.
  893.          * @return previous grid date
  894.          */
  895.         public AbsoluteDate getPreviousDate() {
  896.             return stateMapper.mapDoubleToDate(mathInterpolator.getPreviousTime());
  897.         }

  899.         /** Get the interpolated date.
  900.          * <p>If {@link #setInterpolatedDate(AbsoluteDate) setInterpolatedDate}
  901.          * has not been called, the date returned is the same as  {@link
  902.          * #getCurrentDate() getCurrentDate}.</p>
  903.          * @return interpolated date
  904.          * @see #setInterpolatedDate(AbsoluteDate)
  905.          * @see #getInterpolatedState()
  906.          */
  907.         public AbsoluteDate getInterpolatedDate() {
  908.             return stateMapper.mapDoubleToDate(mathInterpolator.getInterpolatedTime());
  909.         }

  911.         /** Set the interpolated date.
  912.          * <p>It is possible to set the interpolation date outside of the current
  913.          * step range, but accuracy will decrease as date is farther.</p>
  914.          * @param date interpolated date to set
  915.          * @see #getInterpolatedDate()
  916.          * @see #getInterpolatedState()
  917.          */
  918.         public void setInterpolatedDate(final AbsoluteDate date) {
  919.             mathInterpolator.setInterpolatedTime(stateMapper.mapDateToDouble(date));
  920.         }

  922.         /** Get the interpolated state.
  923.          * @return interpolated state at the current interpolation date
  924.          * @exception OrekitException if state cannot be interpolated or converted
  925.          * @see #getInterpolatedDate()
  926.          * @see #setInterpolatedDate(AbsoluteDate)
  927.          */
  928.         public SpacecraftState getInterpolatedState() throws OrekitException {
  929.             try {

  931.                 SpacecraftState s =
  932.                         stateMapper.mapArrayToState(mathInterpolator.getInterpolatedTime(),
  933.                                                     mathInterpolator.getInterpolatedState());
  934.                 s = updateAdditionalStates(s);
  935.                 for (int i = 0; i < additionalEquations.size(); ++i) {
  936.                     final double[] secondary = mathInterpolator.getInterpolatedSecondaryState(i);
  937.                     s = s.addAdditionalState(additionalEquations.get(i).getName(), secondary);
  938.                 }

  940.                 return s;

  942.             } catch (OrekitExceptionWrapper oew) {
  943.                 throw oew.getException();
  944.             }
  945.         }

  947.         /** Check is integration direction is forward in date.
  948.          * @return true if integration is forward in date
  949.          */
  950.         public boolean isForward() {
  951.             return mathInterpolator.isForward();
  952.         }

  954.     }

  956.     private class EphemerisModeHandler implements ModeHandler, StepHandler {

  958.         /** Underlying raw mathematical model. */
  959.         private ContinuousOutputModel model;

  961.         /** Generated ephemeris. */
  962.         private BoundedPropagator ephemeris;

  964.         /** Flag for handler . */
  965.         private boolean activate;

  967.         /** Creates a new instance of EphemerisModeHandler which must be
  968.          *  filled by the propagator.
  969.          */
  970.         public EphemerisModeHandler() {
  971.         }

  973.         /** {@inheritDoc} */
  974.         public void initialize(final boolean activateHandlers) {
  975.             this.activate = activateHandlers;
  976.             this.model    = new ContinuousOutputModel();

  978.             // ephemeris will be generated when last step is processed
  979.             this.ephemeris = null;

  981.         }

  983.         /** Get the generated ephemeris.
  984.          * @return a new instance of the generated ephemeris
  985.          */
  986.         public BoundedPropagator getEphemeris() {
  987.             return ephemeris;
  988.         }

  990.         /** {@inheritDoc} */
  991.         public void handleStep(final StepInterpolator interpolator, final boolean isLast)
  992.             throws OrekitExceptionWrapper {
  993.             try {
  994.                 if (activate) {
  995.                     model.handleStep(interpolator, isLast);
  996.                     if (isLast) {

  998.                         // set up the boundary dates
  999.                         final double tI = model.getInitialTime();
  1000.                         final double tF = model.getFinalTime();
  1001.                         final AbsoluteDate startDate = stateMapper.mapDoubleToDate(tI);
  1002.                         final AbsoluteDate minDate;
  1003.                         final AbsoluteDate maxDate;
  1004.                         if (tF < tI) {
  1005.                             minDate = stateMapper.mapDoubleToDate(tF);
  1006.                             maxDate = startDate;
  1007.                         } else {
  1008.                             minDate = startDate;
  1009.                             maxDate = stateMapper.mapDoubleToDate(tF);
  1010.                         }

  1012.                         // get the initial additional states that are not managed
  1013.                         final Map<String, double[]> unmanaged = new HashMap<String, double[]>();
  1014.                         for (final Map.Entry<String, double[]> initial : getInitialState().getAdditionalStates().entrySet()) {
  1015.                             if (!isAdditionalStateManaged(initial.getKey())) {
  1016.                                 // this additional state was in the initial state, but is unknown to the propagator
  1017.                                 // we simply copy its initial value as is
  1018.                                 unmanaged.put(initial.getKey(), initial.getValue());
  1019.                             }
  1020.                         }

  1022.                         // get the names of additional states managed by differential equations
  1023.                         final String[] names = new String[additionalEquations.size()];
  1024.                         for (int i = 0; i < names.length; ++i) {
  1025.                             names[i] = additionalEquations.get(i).getName();
  1026.                         }

  1028.                         // create the ephemeris
  1029.                         ephemeris = new IntegratedEphemeris(startDate, minDate, maxDate,
  1030.                                                             stateMapper, model, unmanaged,
  1031.                                                             getAdditionalStateProviders(), names);

  1033.                     }
  1034.                 }
  1035.             } catch (OrekitException oe) {
  1036.                 throw new OrekitExceptionWrapper(oe);
  1037.             }
  1038.         }

  1040.         /** {@inheritDoc} */
  1041.         public void init(final double t0, final double[] y0, final double t) {
  1042.             model.init(t0, y0, t);
  1043.         }

  1045.     }

  1047. }
Created with JaCoCo 0.6.3.201306030806