FieldAbstractAnalyticalPropagator.java

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

  18. import java.util.ArrayList;
  19. import java.util.Collection;
  20. import java.util.Collections;
  21. import java.util.Comparator;
  22. import java.util.List;
  23. import java.util.PriorityQueue;
  24. import java.util.Queue;

  25. import org.hipparchus.Field;
  26. import org.hipparchus.RealFieldElement;
  27. import org.hipparchus.exception.MathRuntimeException;
  28. import org.hipparchus.ode.events.Action;
  29. import org.hipparchus.util.FastMath;
  30. import org.orekit.attitudes.AttitudeProvider;
  31. import org.orekit.attitudes.FieldAttitude;
  32. import org.orekit.errors.OrekitException;
  33. import org.orekit.errors.OrekitInternalError;
  34. import org.orekit.frames.Frame;
  35. import org.orekit.orbits.FieldOrbit;
  36. import org.orekit.propagation.BoundedPropagator;
  37. import org.orekit.propagation.FieldAbstractPropagator;
  38. import org.orekit.propagation.FieldAdditionalStateProvider;
  39. import org.orekit.propagation.FieldBoundedPropagator;
  40. import org.orekit.propagation.FieldSpacecraftState;
  41. import org.orekit.propagation.events.FieldEventDetector;
  42. import org.orekit.propagation.events.FieldEventState;
  43. import org.orekit.propagation.events.FieldEventState.EventOccurrence;
  44. import org.orekit.propagation.sampling.FieldOrekitStepInterpolator;
  45. import org.orekit.time.FieldAbsoluteDate;
  46. import org.orekit.utils.FieldPVCoordinatesProvider;
  47. import org.orekit.utils.TimeStampedFieldPVCoordinates;

  48. /** Common handling of {@link org.orekit.propagation.FieldPropagator} methods for analytical propagators.
  49.  * <p>
  50.  * This abstract class allows to provide easily the full set of {@link
  51.  * org.orekit.propagation.FieldPropagator FieldPropagator} methods, including all propagation
  52.  * modes support and discrete events support for any simple propagation method. Only
  53.  * two methods must be implemented by derived classes: {@link #propagateOrbit(FieldAbsoluteDate)}
  54.  * and {@link #getMass(FieldAbsoluteDate)}. The first method should perform straightforward
  55.  * propagation starting from some internally stored initial state up to the specified target date.
  56.  * </p>
  57.  * @author Luc Maisonobe
  58.  */

  59. public abstract class FieldAbstractAnalyticalPropagator<T extends RealFieldElement<T>> extends FieldAbstractPropagator<T> {

  60.     /** Provider for attitude computation. */
  61.     private FieldPVCoordinatesProvider<T> pvProvider;

  62.     /** Start date of last propagation. */
  63.     private FieldAbsoluteDate<T> lastPropagationStart;

  64.     /** End date of last propagation. */
  65.     private FieldAbsoluteDate<T> lastPropagationEnd;

  66.     /** Initialization indicator of events states. */
  67.     private boolean statesInitialized;

  68.     /** Indicator for last step. */
  69.     private boolean isLastStep;

  70.     /** Event steps. */
  71.     private final Collection<FieldEventState<?, T>> eventsStates;

  72.     /** Build a new instance.
  73.      * @param attitudeProvider provider for attitude computation
  74.      * @param field field used as default
  75.      */
  76.     protected FieldAbstractAnalyticalPropagator(final Field<T> field, final AttitudeProvider attitudeProvider) {
  77.         super(field);
  78.         setAttitudeProvider(attitudeProvider);
  79.         pvProvider           = new FieldLocalPVProvider();
  80.         lastPropagationStart = FieldAbsoluteDate.getPastInfinity(field);
  81.         lastPropagationEnd   = FieldAbsoluteDate.getFutureInfinity(field);
  82.         statesInitialized    = false;
  83.         eventsStates         = new ArrayList<FieldEventState<?, T>>();
  84.     }

  85.     /** {@inheritDoc} */
  86.     public FieldBoundedPropagator<T> getGeneratedEphemeris() {
  87.         return new FieldBoundedPropagatorView(lastPropagationStart, lastPropagationEnd);
  88.     }

  89.     /** {@inheritDoc} */
  90.     public Collection<FieldEventDetector<T>> getEventsDetectors() {
  91.         final List<FieldEventDetector<T>> list = new ArrayList<FieldEventDetector<T>>();
  92.         for (final FieldEventState<?, T> state : eventsStates) {
  93.             list.add(state.getEventDetector());
  94.         }
  95.         return Collections.unmodifiableCollection(list);
  96.     }

  97.     /** {@inheritDoc} */
  98.     public void clearEventsDetectors() {
  99.         eventsStates.clear();
  100.     }
  101.     /** {@inheritDoc} */
  102.     public FieldSpacecraftState<T> propagate(final FieldAbsoluteDate<T> start, final FieldAbsoluteDate<T> target) {
  103.         try {

  104.             initializePropagation();

  105.             lastPropagationStart = start;

  106.             final T dt       = target.durationFrom(start);

  107.             final double epsilon  = FastMath.ulp(dt.getReal());

  108.             FieldSpacecraftState<T> state = updateAdditionalStates(basicPropagate(start));

  109.             // evaluate step size
  110.             final T stepSize;
  111.             if (getMode() == MASTER_MODE) {
  112.                 if (Double.isNaN(getFixedStepSize().getReal())) {
  113.                     stepSize = state.getKeplerianPeriod().divide(100).copySign(dt);
  114.                 } else {
  115.                     stepSize = getFixedStepSize().copySign(dt);
  116.                 }
  117.             } else {
  118.                 stepSize = dt;
  119.             }

  120.             // initialize event detectors
  121.             for (final FieldEventState<?, T> es : eventsStates) {
  122.                 es.init(state, target);
  123.             }

  124.             // initialize step handler
  125.             if (getStepHandler() != null) {
  126.                 getStepHandler().init(state, target);
  127.             }

  128.             // iterate over the propagation range
  129.             statesInitialized = false;
  130.             isLastStep = false;
  131.             do {

  132.                 // go ahead one step size
  133.                 final FieldSpacecraftState<T> previous = state;
  134.                 FieldAbsoluteDate<T> t = previous.getDate().shiftedBy(stepSize);
  135.                 if ((dt.getReal() == 0) || ((dt.getReal() > 0) ^ (t.compareTo(target) <= 0))) {
  136.                     // current step exceeds target
  137.                     t = target;
  138.                 }
  139.                 final FieldSpacecraftState<T> current = updateAdditionalStates(basicPropagate(t));
  140.                 final FieldBasicStepInterpolator interpolator = new FieldBasicStepInterpolator(dt.getReal() >= 0, previous, current);



  141.                 // accept the step, trigger events and step handlers
  142.                 state = acceptStep(interpolator, target, epsilon);
  143.             } while (!isLastStep);
  144.             // return the last computed state
  145.             lastPropagationEnd = state.getDate();
  146.             setStartDate(state.getDate());
  147.             return state;

  148.         } catch (MathRuntimeException mrte) {
  149.             throw OrekitException.unwrap(mrte);
  150.         }
  151.     }

  152.     /** Accept a step, triggering events and step handlers.
  153.      * @param interpolator interpolator for the current step
  154.      * @param target final propagation time
  155.      * @param epsilon threshold for end date detection
  156.      * @return state at the end of the step
  157.           * @exception MathRuntimeException if an event cannot be located
  158.      */
  159.     protected FieldSpacecraftState<T> acceptStep(final FieldBasicStepInterpolator interpolator,
  160.                                          final FieldAbsoluteDate<T> target, final double epsilon)
  161.         throws MathRuntimeException {

  162.         FieldSpacecraftState<T>       previous = interpolator.getPreviousState();
  163.         final FieldSpacecraftState<T> current  = interpolator.getCurrentState();
  164.         FieldBasicStepInterpolator restricted = interpolator;

  165.         // initialize the events states if needed
  166.         if (!statesInitialized) {

  167.             if (!eventsStates.isEmpty()) {
  168.                 // initialize the events states
  169.                 for (final FieldEventState<?, T> state : eventsStates) {
  170.                     state.reinitializeBegin(interpolator);
  171.                 }
  172.             }

  173.             statesInitialized = true;

  174.         }
  175.         // search for next events that may occur during the step
  176.         final int orderingSign = interpolator.isForward() ? +1 : -1;
  177.         final Queue<FieldEventState<?, T>> occurringEvents = new PriorityQueue<>(new Comparator<FieldEventState<?, T>>() {
  178.             /** {@inheritDoc} */
  179.             @Override
  180.             public int compare(final FieldEventState<?, T> es0, final FieldEventState<?, T> es1) {
  181.                 return orderingSign * es0.getEventDate().compareTo(es1.getEventDate());
  182.             }
  183.         });

  184.         boolean doneWithStep = false;
  185.         resetEvents:
  186.         do {

  187.             // Evaluate all event detectors for events
  188.             occurringEvents.clear();
  189.             for (final FieldEventState<?, T> state : eventsStates) {
  190.                 if (state.evaluateStep(interpolator)) {
  191.                     // the event occurs during the current step
  192.                     occurringEvents.add(state);
  193.                 }
  194.             }


  195.             do {
  196.                 eventLoop:
  197.                 while (!occurringEvents.isEmpty()) {
  198.                     // handle the chronologically first event
  199.                     final FieldEventState<?, T> currentEvent = occurringEvents.poll();

  200.                     // get state at event time
  201.                     FieldSpacecraftState<T> eventState = restricted.getInterpolatedState(currentEvent.getEventDate());
  202.                     // try to advance all event states to current time
  203.                     for (final FieldEventState<?, T> state : eventsStates) {
  204.                         if (state != currentEvent && state.tryAdvance(eventState, interpolator)) {
  205.                             // we need to handle another event first
  206.                             // remove event we just updated to prevent heap corruption
  207.                             occurringEvents.remove(state);
  208.                             // add it back to update its position in the heap
  209.                             occurringEvents.add(state);
  210.                             // re-queue the event we were processing
  211.                             occurringEvents.add(currentEvent);
  212.                             continue eventLoop;
  213.                         }
  214.                     }
  215.                     // all event detectors agree we can advance to the current event time
  216.                     final EventOccurrence<T> occurrence = currentEvent.doEvent(eventState);
  217.                     final Action action = occurrence.getAction();
  218.                     isLastStep = action == Action.STOP;
  219.                     if (isLastStep) {
  220.                         // ensure the event is after the root if it is returned STOP
  221.                         // this lets the user integrate to a STOP event and then restart
  222.                         // integration from the same time.
  223.                         eventState = interpolator.getInterpolatedState(occurrence.getStopDate());
  224.                         restricted = new FieldBasicStepInterpolator(restricted.isForward(), previous, eventState);
  225.                     }
  226.                     // handle the first part of the step, up to the event
  227.                     if (getStepHandler() != null) {
  228.                         getStepHandler().handleStep(restricted, isLastStep);
  229.                     }

  230.                     if (isLastStep) {
  231.                         // the event asked to stop integration
  232.                         return eventState;
  233.                     }

  234.                     if (action == Action.RESET_DERIVATIVES || action == Action.RESET_STATE) {
  235.                         // some event handler has triggered changes that
  236.                         // invalidate the derivatives, we need to recompute them
  237.                         final FieldSpacecraftState<T> resetState = occurrence.getNewState();
  238.                         if (resetState != null) {
  239.                             resetIntermediateState(resetState, interpolator.isForward());
  240.                             return resetState;
  241.                         }
  242.                     }
  243.                     // at this point action == Action.CONTINUE or Action.RESET_EVENTS

  244.                     // prepare handling of the remaining part of the step
  245.                     previous = eventState;
  246.                     restricted = new FieldBasicStepInterpolator(restricted.isForward(), eventState, current);

  247.                     if (action == Action.RESET_EVENTS) {
  248.                         continue resetEvents;
  249.                     }

  250.                     // at this pint action == Action.CONTINUE
  251.                     // check if the same event occurs again in the remaining part of the step
  252.                     if (currentEvent.evaluateStep(restricted)) {
  253.                         // the event occurs during the current step
  254.                         occurringEvents.add(currentEvent);
  255.                     }

  256.                 }

  257.                 // last part of the step, after the last event
  258.                 // may be a new event here if the last event modified the g function of
  259.                 // another event detector.
  260.                 for (final FieldEventState<?, T> state : eventsStates) {
  261.                     if (state.tryAdvance(current, interpolator)) {
  262.                         occurringEvents.add(state);
  263.                     }
  264.                 }

  265.             } while (!occurringEvents.isEmpty());

  266.             doneWithStep = true;
  267.         } while (!doneWithStep);

  268.         final T remaining = target.durationFrom(current.getDate());
  269.         if (interpolator.isForward()) {
  270.             isLastStep = remaining.getReal() <  epsilon;
  271.         } else {
  272.             isLastStep = remaining.getReal() > -epsilon;
  273.         }

  274.         // handle the remaining part of the step, after all events if any
  275.         if (getStepHandler() != null) {
  276.             getStepHandler().handleStep(interpolator, isLastStep);
  277.         }
  278.         return current;

  279.     }

  280.     /** Get the mass.
  281.      * @param date target date for the orbit
  282.      * @return mass mass
  283.      */
  284.     protected abstract T getMass(FieldAbsoluteDate<T> date);

  285.     /** Get PV coordinates provider.
  286.      * @return PV coordinates provider
  287.      */
  288.     public FieldPVCoordinatesProvider<T> getPvProvider() {
  289.         return pvProvider;
  290.     }

  291.     /** {@inheritDoc} */
  292.     public <D extends FieldEventDetector<T>> void addEventDetector(final D detector) {
  293.         eventsStates.add(new FieldEventState<D, T>(detector));
  294.     }

  295.     /** Reset an intermediate state.
  296.      * @param state new intermediate state to consider
  297.      * @param forward if true, the intermediate state is valid for
  298.      * propagations after itself
  299.      */
  300.     protected abstract void resetIntermediateState(FieldSpacecraftState<T> state, boolean forward);

  301.     /** Extrapolate an orbit up to a specific target date.
  302.      * @param date target date for the orbit
  303.      * @return extrapolated parameters
  304.      */
  305.     protected abstract FieldOrbit<T> propagateOrbit(FieldAbsoluteDate<T> date);

  306.     /** Propagate an orbit without any fancy features.
  307.      * <p>This method is similar in spirit to the {@link #propagate} method,
  308.      * except that it does <strong>not</strong> call any handler during
  309.      * propagation, nor any discrete events, not additional states. It always
  310.      * stop exactly at the specified date.</p>
  311.      * @param date target date for propagation
  312.      * @return state at specified date
  313.      */
  314.     protected FieldSpacecraftState<T> basicPropagate(final FieldAbsoluteDate<T> date) {
  315.         try {

  316.             // evaluate orbit
  317.             final FieldOrbit<T> orbit = propagateOrbit(date);

  318.             // evaluate attitude
  319.             final FieldAttitude<T> attitude =
  320.                 getAttitudeProvider().getAttitude(pvProvider, date, orbit.getFrame());

  321.             // build raw state
  322.             return new FieldSpacecraftState<>(orbit, attitude, getMass(date));

  323.         } catch (OrekitException oe) {
  324.             throw new OrekitException(oe);
  325.         }
  326.     }

  327.     /** Internal FieldPVCoordinatesProvider<T> for attitude computation. */
  328.     private class FieldLocalPVProvider implements FieldPVCoordinatesProvider<T> {

  329.         /** {@inheritDoc} */
  330.         public TimeStampedFieldPVCoordinates<T> getPVCoordinates(final FieldAbsoluteDate<T> date, final Frame frame) {
  331.             return propagateOrbit(date).getPVCoordinates(frame);
  332.         }

  333.     }

  334.     /** {@link BoundedPropagator} view of the instance. */
  335.     private class FieldBoundedPropagatorView extends FieldAbstractAnalyticalPropagator<T>
  336.         implements FieldBoundedPropagator<T> {

  337.         /** Min date. */
  338.         private final FieldAbsoluteDate<T> minDate;

  339.         /** Max date. */
  340.         private final FieldAbsoluteDate<T> maxDate;

  341.         /** Simple constructor.
  342.          * @param startDate start date of the propagation
  343.          * @param endDate end date of the propagation
  344.          */
  345.         FieldBoundedPropagatorView(final FieldAbsoluteDate<T> startDate, final FieldAbsoluteDate<T> endDate) {
  346.             super(startDate.durationFrom(endDate).getField(), FieldAbstractAnalyticalPropagator.this.getAttitudeProvider());
  347.             if (startDate.compareTo(endDate) <= 0) {
  348.                 minDate = startDate;
  349.                 maxDate = endDate;
  350.             } else {
  351.                 minDate = endDate;
  352.                 maxDate = startDate;
  353.             }

  354.             try {
  355.                 // copy the same additional state providers as the original propagator
  356.                 for (FieldAdditionalStateProvider<T> provider : FieldAbstractAnalyticalPropagator.this.getAdditionalStateProviders()) {
  357.                     addAdditionalStateProvider(provider);
  358.                 }
  359.             } catch (OrekitException oe) {
  360.                 // as the providers are already compatible with each other,
  361.                 // this should never happen
  362.                 throw new OrekitInternalError(null);
  363.             }

  364.         }

  365.         /** {@inheritDoc} */
  366.         public FieldAbsoluteDate<T> getMinDate() {
  367.             return minDate;
  368.         }

  369.         /** {@inheritDoc} */
  370.         public FieldAbsoluteDate<T> getMaxDate() {
  371.             return maxDate;
  372.         }

  373.         /** {@inheritDoc} */
  374.         protected FieldOrbit<T> propagateOrbit(final FieldAbsoluteDate<T> target) {
  375.             return FieldAbstractAnalyticalPropagator.this.propagateOrbit(target);
  376.         }

  377.         /** {@inheritDoc} */
  378.         public T getMass(final FieldAbsoluteDate<T> date) {
  379.             return FieldAbstractAnalyticalPropagator.this.getMass(date);
  380.         }

  381.         /** {@inheritDoc} */
  382.         public TimeStampedFieldPVCoordinates<T> getPVCoordinates(final FieldAbsoluteDate<T> date, final Frame frame) {
  383.             return propagate(date).getPVCoordinates(frame);
  384.         }

  385.         /** {@inheritDoc} */
  386.         public void resetInitialState(final FieldSpacecraftState<T> state) {
  387.             FieldAbstractAnalyticalPropagator.this.resetInitialState(state);
  388.         }

  389.         /** {@inheritDoc} */
  390.         protected void resetIntermediateState(final FieldSpacecraftState<T> state, final boolean forward) {
  391.             FieldAbstractAnalyticalPropagator.this.resetIntermediateState(state, forward);
  392.         }

  393.         /** {@inheritDoc} */
  394.         public FieldSpacecraftState<T> getInitialState() {
  395.             return FieldAbstractAnalyticalPropagator.this.getInitialState();
  396.         }

  397.         /** {@inheritDoc} */
  398.         public Frame getFrame() {
  399.             return FieldAbstractAnalyticalPropagator.this.getFrame();
  400.         }
  401.     }


  402.     /** Internal class for local propagation. */
  403.     private class FieldBasicStepInterpolator implements FieldOrekitStepInterpolator<T> {

  404.         /** Previous state. */
  405.         private final FieldSpacecraftState<T> previousState;

  406.         /** Current state. */
  407.         private final FieldSpacecraftState<T> currentState;

  408.         /** Forward propagation indicator. */
  409.         private final boolean forward;

  410.         /** Simple constructor.
  411.          * @param isForward integration direction indicator
  412.          * @param previousState start of the step
  413.          * @param currentState end of the step
  414.          */
  415.         FieldBasicStepInterpolator(final boolean isForward,
  416.                               final FieldSpacecraftState<T> previousState,
  417.                               final FieldSpacecraftState<T> currentState) {
  418.             this.forward             = isForward;
  419.             this.previousState   = previousState;
  420.             this.currentState    = currentState;
  421.         }

  422.         /** {@inheritDoc} */
  423.         public FieldSpacecraftState<T> getPreviousState() {
  424.             return previousState;
  425.         }

  426.         /** {@inheritDoc} */
  427.         public FieldSpacecraftState<T> getCurrentState() {
  428.             return currentState;
  429.         }

  430.         /** {@inheritDoc} */
  431.         public FieldSpacecraftState<T> getInterpolatedState(final FieldAbsoluteDate<T> date) {

  432.             // compute the basic spacecraft state
  433.             final FieldSpacecraftState<T> basicState = basicPropagate(date);

  434.             // add the additional states
  435.             return updateAdditionalStates(basicState);

  436.         }

  437.         /** {@inheritDoc} */
  438.         public boolean isForward() {
  439.             return forward;
  440.         }

  441.     }

  442. }