EventState.java

  1. /*
  2.  * Licensed to the Apache Software Foundation (ASF) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * The ASF 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.events;

  19. import org.hipparchus.analysis.UnivariateFunction;
  20. import org.hipparchus.analysis.solvers.BracketedUnivariateSolver;
  21. import org.hipparchus.analysis.solvers.BracketedUnivariateSolver.Interval;
  22. import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
  23. import org.hipparchus.exception.MathRuntimeException;
  24. import org.hipparchus.ode.events.Action;
  25. import org.hipparchus.util.FastMath;
  26. import org.hipparchus.util.Precision;
  27. import org.orekit.errors.OrekitInternalError;
  28. import org.orekit.propagation.SpacecraftState;
  29. import org.orekit.propagation.sampling.OrekitStepInterpolator;
  30. import org.orekit.time.AbsoluteDate;

  32. /** This class handles the state for one {@link EventDetector
  33.  * event detector} during integration steps.
  34.  *
  35.  * <p>This class is heavily based on the class with the same name from the
  36.  * Hipparchus library. The changes performed consist in replacing
  37.  * raw types (double and double arrays) with space dynamics types
  38.  * ({@link AbsoluteDate}, {@link SpacecraftState}).</p>
  39.  * <p>Each time the propagator proposes a step, the event detector
  40.  * should be checked. This class handles the state of one detector
  41.  * during one propagation step, with references to the state at the
  42.  * end of the preceding step. This information is used to determine if
  43.  * the detector should trigger an event or not during the proposed
  44.  * step (and hence the step should be reduced to ensure the event
  45.  * occurs at a bound rather than inside the step).</p>
  46.  * @author Luc Maisonobe
  47.  * @param <T> class type for the generic version
  48.  */
  49. public class EventState<T extends EventDetector> {

  51.     /** Event detector. */
  52.     private T detector;

  54.     /** Time of the previous call to g. */
  55.     private AbsoluteDate lastT;

  57.     /** Value from the previous call to g. */
  58.     private double lastG;

  60.     /** Time at the beginning of the step. */
  61.     private AbsoluteDate t0;

  63.     /** Value of the event detector at the beginning of the step. */
  64.     private double g0;

  66.     /** Simulated sign of g0 (we cheat when crossing events). */
  67.     private boolean g0Positive;

  69.     /** Indicator of event expected during the step. */
  70.     private boolean pendingEvent;

  72.     /** Occurrence time of the pending event. */
  73.     private AbsoluteDate pendingEventTime;

  75.     /**
  76.      * Time to stop propagation if the event is a stop event. Used to enable stopping at
  77.      * an event and then restarting after that event.
  78.      */
  79.     private AbsoluteDate stopTime;

  81.     /** Time after the current event. */
  82.     private AbsoluteDate afterEvent;

  84.     /** Value of the g function after the current event. */
  85.     private double afterG;

  87.     /** The earliest time considered for events. */
  88.     private AbsoluteDate earliestTimeConsidered;

  90.     /** Integration direction. */
  91.     private boolean forward;

  93.     /** Variation direction around pending event.
  94.      *  (this is considered with respect to the integration direction)
  95.      */
  96.     private boolean increasing;

  98.     /** Simple constructor.
  99.      * @param detector monitored event detector
  100.      */
  101.     public EventState(final T detector) {
  102.         this.detector     = detector;

  104.         // some dummy values ...
  105.         lastT                  = AbsoluteDate.PAST_INFINITY;
  106.         lastG                  = Double.NaN;
  107.         t0                     = null;
  108.         g0                     = Double.NaN;
  109.         g0Positive             = true;
  110.         pendingEvent           = false;
  111.         pendingEventTime       = null;
  112.         stopTime               = null;
  113.         increasing             = true;
  114.         earliestTimeConsidered = null;
  115.         afterEvent             = null;
  116.         afterG                 = Double.NaN;

  118.     }

  120.     /** Get the underlying event detector.
  121.      * @return underlying event detector
  122.      */
  123.     public T getEventDetector() {
  124.         return detector;
  125.     }

  127.     /** Initialize event handler at the start of a propagation.
  128.      * <p>
  129.      * This method is called once at the start of the propagation. It
  130.      * may be used by the event handler to initialize some internal data
  131.      * if needed.
  132.      * </p>
  133.      * @param s0 initial state
  134.      * @param t target time for the integration
  135.      *
  136.      */
  137.     public void init(final SpacecraftState s0,
  138.                      final AbsoluteDate t) {
  139.         detector.init(s0, t);
  140.         lastT = AbsoluteDate.PAST_INFINITY;
  141.         lastG = Double.NaN;
  142.     }

  144.     /** Compute the value of the switching function.
  145.      * This function must be continuous (at least in its roots neighborhood),
  146.      * as the integrator will need to find its roots to locate the events.
  147.      * @param s the current state information: date, kinematics, attitude
  148.      * @return value of the switching function
  149.      */
  150.     private double g(final SpacecraftState s) {
  151.         if (!s.getDate().equals(lastT)) {
  152.             lastT = s.getDate();
  153.             lastG = detector.g(s);
  154.         }
  155.         return lastG;
  156.     }

  158.     /** Reinitialize the beginning of the step.
  159.      * @param interpolator interpolator valid for the current step
  160.      */
  161.     public void reinitializeBegin(final OrekitStepInterpolator interpolator) {
  162.         forward = interpolator.isForward();
  163.         final SpacecraftState s0 = interpolator.getPreviousState();
  164.         this.t0 = s0.getDate();
  165.         g0 = g(s0);
  166.         while (g0 == 0) {
  167.             // extremely rare case: there is a zero EXACTLY at interval start
  168.             // we will use the sign slightly after step beginning to force ignoring this zero
  169.             // try moving forward by half a convergence interval
  170.             final double dt = (forward ? 0.5 : -0.5) * detector.getThreshold();
  171.             AbsoluteDate startDate = t0.shiftedBy(dt);
  172.             // if convergence is too small move an ulp
  173.             if (t0.equals(startDate)) {
  174.                 startDate = nextAfter(startDate);
  175.             }
  176.             t0 = startDate;
  177.             g0 = g(interpolator.getInterpolatedState(t0));
  178.         }
  179.         g0Positive = g0 > 0;
  180.         // "last" event was increasing
  181.         increasing = g0Positive;
  182.     }

  184.     /** Evaluate the impact of the proposed step on the event detector.
  185.      * @param interpolator step interpolator for the proposed step
  186.      * @return true if the event detector triggers an event before
  187.      * the end of the proposed step (this implies the step should be
  188.      * rejected)
  189.      * @exception MathRuntimeException if an event cannot be located
  190.      */
  191.     public boolean evaluateStep(final OrekitStepInterpolator interpolator)
  192.         throws MathRuntimeException {

  194.         forward = interpolator.isForward();
  195.         final SpacecraftState s1 = interpolator.getCurrentState();
  196.         final AbsoluteDate t1 = s1.getDate();
  197.         final double dt = t1.durationFrom(t0);
  198.         if (FastMath.abs(dt) < detector.getThreshold()) {
  199.             // we cannot do anything on such a small step, don't trigger any events
  200.             return false;
  201.         }
  202.         // number of points to check in the current step
  203.         final int n = FastMath.max(1, (int) FastMath.ceil(FastMath.abs(dt) / detector.getMaxCheckInterval()));
  204.         final double h = dt / n;


  207.         AbsoluteDate ta = t0;
  208.         double ga = g0;
  209.         for (int i = 0; i < n; ++i) {

  211.             // evaluate handler value at the end of the substep
  212.             final AbsoluteDate tb = (i == n - 1) ? t1 : t0.shiftedBy((i + 1) * h);
  213.             final double gb = g(interpolator.getInterpolatedState(tb));

  215.             // check events occurrence
  216.             if (gb == 0.0 || (g0Positive ^ (gb > 0))) {
  217.                 // there is a sign change: an event is expected during this step
  218.                 if (findRoot(interpolator, ta, ga, tb, gb)) {
  219.                     return true;
  220.                 }
  221.             } else {
  222.                 // no sign change: there is no event for now
  223.                 ta = tb;
  224.                 ga = gb;
  225.             }

  227.         }

  229.         // no event during the whole step
  230.         pendingEvent     = false;
  231.         pendingEventTime = null;
  232.         return false;

  234.     }

  236.     /**
  237.      * Find a root in a bracketing interval.
  238.      *
  239.      * <p> When calling this method one of the following must be true. Either ga == 0, gb
  240.      * == 0, (ga < 0  and gb > 0), or (ga > 0 and gb < 0).
  241.      *
  242.      * @param interpolator that covers the interval.
  243.      * @param ta           earliest possible time for root.
  244.      * @param ga           g(ta).
  245.      * @param tb           latest possible time for root.
  246.      * @param gb           g(tb).
  247.      * @return if a zero crossing was found.
  248.      */
  249.     private boolean findRoot(final OrekitStepInterpolator interpolator,
  250.                              final AbsoluteDate ta, final double ga,
  251.                              final AbsoluteDate tb, final double gb) {
  252.         // check there appears to be a root in [ta, tb]
  253.         check(ga == 0.0 || gb == 0.0 || (ga > 0.0 && gb < 0.0) || (ga < 0.0 && gb > 0.0));

  255.         final double convergence = detector.getThreshold();
  256.         final int maxIterationCount = detector.getMaxIterationCount();
  257.         final BracketedUnivariateSolver<UnivariateFunction> solver =
  258.                 new BracketingNthOrderBrentSolver(0, convergence, 0, 5);

  260.         // event time, just at or before the actual root.
  261.         AbsoluteDate beforeRootT = null;
  262.         double beforeRootG = Double.NaN;
  263.         // time on the other side of the root.
  264.         // Initialized the the loop below executes once.
  265.         AbsoluteDate afterRootT = ta;
  266.         double afterRootG = 0.0;

  268.         // check for some conditions that the root finders don't like
  269.         // these conditions cannot not happen in the loop below
  270.         // the ga == 0.0 case is handled by the loop below
  271.         if (ta.equals(tb)) {
  272.             // both non-zero but times are the same. Probably due to reset state
  273.             beforeRootT = ta;
  274.             beforeRootG = ga;
  275.             afterRootT = shiftedBy(beforeRootT, convergence);
  276.             afterRootG = g(interpolator.getInterpolatedState(afterRootT));
  277.         } else if (ga != 0.0 && gb == 0.0) {
  278.             // hard: ga != 0.0 and gb == 0.0
  279.             // look past gb by up to convergence to find next sign
  280.             // throw an exception if g(t) = 0.0 in [tb, tb + convergence]
  281.             beforeRootT = tb;
  282.             beforeRootG = gb;
  283.             afterRootT = shiftedBy(beforeRootT, convergence);
  284.             afterRootG = g(interpolator.getInterpolatedState(afterRootT));
  285.         } else if (ga != 0.0) {
  286.             final double newGa = g(interpolator.getInterpolatedState(ta));
  287.             if (ga > 0 != newGa > 0) {
  288.                 // both non-zero, step sign change at ta, possibly due to reset state
  289.                 beforeRootT = ta;
  290.                 beforeRootG = newGa;
  291.                 afterRootT = minTime(shiftedBy(beforeRootT, convergence), tb);
  292.                 afterRootG = g(interpolator.getInterpolatedState(afterRootT));
  293.             }
  294.         }

  296.         // loop to skip through "fake" roots, i.e. where g(t) = g'(t) = 0.0
  297.         // executed once if we didn't hit a special case above
  298.         AbsoluteDate loopT = ta;
  299.         double loopG = ga;
  300.         while ((afterRootG == 0.0 || afterRootG > 0.0 == g0Positive) &&
  301.                 strictlyAfter(afterRootT, tb)) {
  302.             if (loopG == 0.0) {
  303.                 // ga == 0.0 and gb may or may not be 0.0
  304.                 // handle the root at ta first
  305.                 beforeRootT = loopT;
  306.                 beforeRootG = loopG;
  307.                 afterRootT = minTime(shiftedBy(beforeRootT, convergence), tb);
  308.                 afterRootG = g(interpolator.getInterpolatedState(afterRootT));
  309.             } else {
  310.                 // both non-zero, the usual case, use a root finder.
  311.                 // time zero for evaluating the function f. Needs to be final
  312.                 final AbsoluteDate fT0 = loopT;
  313.                 final UnivariateFunction f = dt -> {
  314.                     return g(interpolator.getInterpolatedState(fT0.shiftedBy(dt)));
  315.                 };
  316.                 // tb as a double for use in f
  317.                 final double tbDouble = tb.durationFrom(fT0);
  318.                 if (forward) {
  319.                     final Interval interval =
  320.                             solver.solveInterval(maxIterationCount, f, 0, tbDouble);
  321.                     beforeRootT = fT0.shiftedBy(interval.getLeftAbscissa());
  322.                     beforeRootG = interval.getLeftValue();
  323.                     afterRootT = fT0.shiftedBy(interval.getRightAbscissa());
  324.                     afterRootG = interval.getRightValue();
  325.                 } else {
  326.                     final Interval interval =
  327.                             solver.solveInterval(maxIterationCount, f, tbDouble, 0);
  328.                     beforeRootT = fT0.shiftedBy(interval.getRightAbscissa());
  329.                     beforeRootG = interval.getRightValue();
  330.                     afterRootT = fT0.shiftedBy(interval.getLeftAbscissa());
  331.                     afterRootG = interval.getLeftValue();
  332.                 }
  333.             }
  334.             // tolerance is set to less than 1 ulp
  335.             // assume tolerance is 1 ulp
  336.             if (beforeRootT.equals(afterRootT)) {
  337.                 afterRootT = nextAfter(afterRootT);
  338.                 afterRootG = g(interpolator.getInterpolatedState(afterRootT));
  339.             }
  340.             // check loop is making some progress
  341.             check((forward && afterRootT.compareTo(beforeRootT) > 0) ||
  342.                   (!forward && afterRootT.compareTo(beforeRootT) < 0));
  343.             // setup next iteration
  344.             loopT = afterRootT;
  345.             loopG = afterRootG;
  346.         }

  348.         // figure out the result of root finding, and return accordingly
  349.         if (afterRootG == 0.0 || afterRootG > 0.0 == g0Positive) {
  350.             // loop gave up and didn't find any crossing within this step
  351.             return false;
  352.         } else {
  353.             // real crossing
  354.             check(beforeRootT != null && !Double.isNaN(beforeRootG));
  355.             // variation direction, with respect to the integration direction
  356.             increasing = !g0Positive;
  357.             pendingEventTime = beforeRootT;
  358.             stopTime = beforeRootG == 0.0 ? beforeRootT : afterRootT;
  359.             pendingEvent = true;
  360.             afterEvent = afterRootT;
  361.             afterG = afterRootG;

  363.             // check increasing set correctly
  364.             check(afterG > 0 == increasing);
  365.             check(increasing == gb >= ga);

  367.             return true;
  368.         }

  370.     }

  372.     /**
  373.      * Get the next number after the given number in the current propagation direction.
  374.      *
  375.      * @param t input time
  376.      * @return t +/- 1 ulp depending on the direction.
  377.      */
  378.     private AbsoluteDate nextAfter(final AbsoluteDate t) {
  379.         return t.shiftedBy(forward ? +Precision.EPSILON : -Precision.EPSILON);
  380.     }

  382.     /** Get the occurrence time of the event triggered in the current
  383.      * step.
  384.      * @return occurrence time of the event triggered in the current
  385.      * step.
  386.      */
  387.     public AbsoluteDate getEventDate() {
  388.         return pendingEventTime;
  389.     }

  391.     /**
  392.      * Try to accept the current history up to the given time.
  393.      *
  394.      * <p> It is not necessary to call this method before calling {@link
  395.      * #doEvent(SpacecraftState)} with the same state. It is necessary to call this
  396.      * method before you call {@link #doEvent(SpacecraftState)} on some other event
  397.      * detector.
  398.      *
  399.      * @param state        to try to accept.
  400.      * @param interpolator to use to find the new root, if any.
  401.      * @return if the event detector has an event it has not detected before that is on or
  402.      * before the same time as {@code state}. In other words {@code false} means continue
  403.      * on while {@code true} means stop and handle my event first.
  404.      */
  405.     public boolean tryAdvance(final SpacecraftState state,
  406.                               final OrekitStepInterpolator interpolator) {
  407.         final AbsoluteDate t = state.getDate();
  408.         // check this is only called before a pending event.
  409.         check(!pendingEvent || !strictlyAfter(pendingEventTime, t));

  411.         final boolean meFirst;

  413.         if (strictlyAfter(t, earliestTimeConsidered)) {
  414.             // just found an event and we know the next time we want to search again
  415.             meFirst = false;
  416.         } else {
  417.             // check g function to see if there is a new event
  418.             final double g = g(state);
  419.             final boolean positive = g > 0;

  421.             if (positive == g0Positive) {
  422.                 // g function has expected sign
  423.                 g0 = g; // g0Positive is the same
  424.                 meFirst = false;
  425.             } else {
  426.                 // found a root we didn't expect -> find precise location
  427.                 final AbsoluteDate oldPendingEventTime = pendingEventTime;
  428.                 final boolean foundRoot = findRoot(interpolator, t0, g0, t, g);
  429.                 // make sure the new root is not the same as the old root, if one exists
  430.                 meFirst = foundRoot && !pendingEventTime.equals(oldPendingEventTime);
  431.             }
  432.         }

  434.         if (!meFirst) {
  435.             // advance t0 to the current time so we can't find events that occur before t
  436.             t0 = t;
  437.         }

  439.         return meFirst;
  440.     }

  442.     /**
  443.      * Notify the user's listener of the event. The event occurs wholly within this method
  444.      * call including a call to {@link EventDetector#resetState(SpacecraftState)}
  445.      * if necessary.
  446.      *
  447.      * @param state the state at the time of the event. This must be at the same time as
  448.      *              the current value of {@link #getEventDate()}.
  449.      * @return the user's requested action and the new state if the action is {@link
  450.      * Action#RESET_STATE Action.RESET_STATE}.
  451.      * Otherwise the new state is {@code state}. The stop time indicates what time propagation
  452.      * should stop if the action is {@link Action#STOP Action.STOP}.
  453.      * This guarantees the integration will stop on or after the root, so that integration
  454.      * may be restarted safely.
  455.      */
  456.     public EventOccurrence doEvent(final SpacecraftState state) {
  457.         // check event is pending and is at the same time
  458.         check(pendingEvent);
  459.         check(state.getDate().equals(this.pendingEventTime));

  461.         final Action action = detector.eventOccurred(state, increasing == forward);
  462.         final SpacecraftState newState;
  463.         if (action == Action.RESET_STATE) {
  464.             newState = detector.resetState(state);
  465.         } else {
  466.             newState = state;
  467.         }
  468.         // clear pending event
  469.         pendingEvent     = false;
  470.         pendingEventTime = null;
  471.         // setup for next search
  472.         earliestTimeConsidered = afterEvent;
  473.         t0 = afterEvent;
  474.         g0 = afterG;
  475.         g0Positive = increasing;
  476.         // check g0Positive set correctly
  477.         check(g0 == 0.0 || g0Positive == (g0 > 0));
  478.         return new EventOccurrence(action, newState, stopTime);
  479.     }

  481.     /**
  482.      * Shift a time value along the current integration direction: {@link #forward}.
  483.      *
  484.      * @param t     the time to shift.
  485.      * @param delta the amount to shift.
  486.      * @return t + delta if forward, else t - delta. If the result has to be rounded it
  487.      * will be rounded to be before the true value of t + delta.
  488.      */
  489.     private AbsoluteDate shiftedBy(final AbsoluteDate t, final double delta) {
  490.         if (forward) {
  491.             final AbsoluteDate ret = t.shiftedBy(delta);
  492.             if (ret.durationFrom(t) > delta) {
  493.                 return ret.shiftedBy(-Precision.EPSILON);
  494.             } else {
  495.                 return ret;
  496.             }
  497.         } else {
  498.             final AbsoluteDate ret = t.shiftedBy(-delta);
  499.             if (t.durationFrom(ret) > delta) {
  500.                 return ret.shiftedBy(+Precision.EPSILON);
  501.             } else {
  502.                 return ret;
  503.             }
  504.         }
  505.     }

  507.     /**
  508.      * Get the time that happens first along the current propagation direction: {@link
  509.      * #forward}.
  510.      *
  511.      * @param a first time
  512.      * @param b second time
  513.      * @return min(a, b) if forward, else max (a, b)
  514.      */
  515.     private AbsoluteDate minTime(final AbsoluteDate a, final AbsoluteDate b) {
  516.         return (forward ^ (a.compareTo(b) > 0)) ? a : b;
  517.     }

  519.     /**
  520.      * Check the ordering of two times.
  521.      *
  522.      * @param t1 the first time.
  523.      * @param t2 the second time.
  524.      * @return true if {@code t2} is strictly after {@code t1} in the propagation
  525.      * direction.
  526.      */
  527.     private boolean strictlyAfter(final AbsoluteDate t1, final AbsoluteDate t2) {
  528.         if (t1 == null || t2 == null) {
  529.             return false;
  530.         } else {
  531.             return forward ? t1.compareTo(t2) < 0 : t2.compareTo(t1) < 0;
  532.         }
  533.     }

  535.     /**
  536.      * Same as keyword assert, but throw a {@link MathRuntimeException}.
  537.      *
  538.      * @param condition to check
  539.      * @throws MathRuntimeException if {@code condition} is false.
  540.      */
  541.     private void check(final boolean condition) throws MathRuntimeException {
  542.         if (!condition) {
  543.             throw new OrekitInternalError(null);
  544.         }
  545.     }

  547.     /**
  548.      * Class to hold the data related to an event occurrence that is needed to decide how
  549.      * to modify integration.
  550.      */
  551.     public static class EventOccurrence {

  553.         /** User requested action. */
  554.         private final Action action;
  555.         /** New state for a reset action. */
  556.         private final SpacecraftState newState;
  557.         /** The time to stop propagation if the action is a stop event. */
  558.         private final AbsoluteDate stopDate;

  560.         /**
  561.          * Create a new occurrence of an event.
  562.          *
  563.          * @param action   the user requested action.
  564.          * @param newState for a reset event. Should be the current state unless the
  565.          *                 action is {@link Action#RESET_STATE}.
  566.          * @param stopDate to stop propagation if the action is {@link Action#STOP}. Used
  567.          *                 to move the stop time to just after the root.
  568.          */
  569.         EventOccurrence(final Action action,
  570.                         final SpacecraftState newState,
  571.                         final AbsoluteDate stopDate) {
  572.             this.action = action;
  573.             this.newState = newState;
  574.             this.stopDate = stopDate;
  575.         }

  577.         /**
  578.          * Get the user requested action.
  579.          *
  580.          * @return the action.
  581.          */
  582.         public Action getAction() {
  583.             return action;
  584.         }

  586.         /**
  587.          * Get the new state for a reset action.
  588.          *
  589.          * @return the new state.
  590.          */
  591.         public SpacecraftState getNewState() {
  592.             return newState;
  593.         }

  595.         /**
  596.          * Get the new time for a stop action.
  597.          *
  598.          * @return when to stop propagation.
  599.          */
  600.         public AbsoluteDate getStopDate() {
  601.             return stopDate;
  602.         }

  604.     }

  606. }
Created with JaCoCo 0.8.6.202009150832