DetectorBasedEventState.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.  *      https://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.  */

  18. /*
  19.  * This is not the original file distributed by the Apache Software Foundation
  20.  * It has been modified by the Hipparchus project
  21.  */

  23. package org.hipparchus.ode.events;

  25. import org.hipparchus.analysis.UnivariateFunction;
  26. import org.hipparchus.analysis.solvers.BracketedUnivariateSolver;
  27. import org.hipparchus.analysis.solvers.BracketedUnivariateSolver.Interval;
  28. import org.hipparchus.exception.MathIllegalArgumentException;
  29. import org.hipparchus.exception.MathIllegalStateException;
  30. import org.hipparchus.exception.MathRuntimeException;
  31. import org.hipparchus.ode.LocalizedODEFormats;
  32. import org.hipparchus.ode.ODEState;
  33. import org.hipparchus.ode.ODEStateAndDerivative;
  34. import org.hipparchus.ode.sampling.ODEStateInterpolator;
  35. import org.hipparchus.util.FastMath;

  37. /** This class handles the state for one {@link ODEEventHandler
  38.  * event handler} during integration steps.
  39.  *
  40.  * <p>Each time the integrator proposes a step, the event handler
  41.  * switching function should be checked. This class handles the state
  42.  * of one handler during one integration step, with references to the
  43.  * state at the end of the preceding step. This information is used to
  44.  * decide if the handler should trigger an event or not during the
  45.  * proposed step.</p>
  46.  *
  47.  */
  48. public class DetectorBasedEventState implements EventState {

  50.     /** Event detector.
  51.      * @since 3.0
  52.      */
  53.     private final ODEEventDetector detector;

  55.     /** Event solver.
  56.      * @since 3.0
  57.      */
  58.     private final BracketedUnivariateSolver<UnivariateFunction> solver;

  60.     /** Event handler. */
  61.     private final ODEEventHandler handler;

  63.     /** Time of the previous call to g. */
  64.     private double lastT;

  66.     /** Value from the previous call to g. */
  67.     private double lastG;

  69.     /** Time at the beginning of the step. */
  70.     private double t0;

  72.     /** Value of the events handler at the beginning of the step. */
  73.     private double g0;

  75.     /** Sign of g0. */
  76.     private boolean g0Positive;

  78.     /** Indicator of event expected during the step. */
  79.     private boolean pendingEvent;

  81.     /** Occurrence time of the pending event. */
  82.     private double pendingEventTime;

  84.     /**
  85.      * Time to stop propagation if the event is a stop event. Used to enable stopping at
  86.      * an event and then restarting after that event.
  87.      */
  88.     private double stopTime;

  90.     /** Time after the current event. */
  91.     private double afterEvent;

  93.     /** Value of the g function after the current event. */
  94.     private double afterG;

  96.     /** The earliest time considered for events. */
  97.     private double earliestTimeConsidered;

  99.     /** Integration direction. */
  100.     private boolean forward;

  102.     /**
  103.      * Direction of g(t) in the propagation direction for the pending event, or if there
  104.      * is no pending event the direction of the previous event.
  105.      */
  106.     private boolean increasing;

  108.     /** Simple constructor.
  109.      * @param detector event detector
  110.      * @since 3.0
  111.      */
  112.     public DetectorBasedEventState(final ODEEventDetector detector) {

  114.         this.detector     = detector;
  115.         this.solver       = detector.getSolver();
  116.         this.handler      = detector.getHandler();

  118.         // some dummy values ...
  119.         t0                = Double.NaN;
  120.         g0                = Double.NaN;
  121.         g0Positive        = true;
  122.         pendingEvent      = false;
  123.         pendingEventTime  = Double.NaN;
  124.         increasing        = true;
  125.         earliestTimeConsidered = Double.NaN;
  126.         afterEvent = Double.NaN;
  127.         afterG = Double.NaN;
  128.     }

  130.     /** Get the underlying event detector.
  131.      * @return underlying event detector
  132.      * @since 3.0
  133.      */
  134.     public ODEEventDetector getEventDetector() {
  135.         return detector;
  136.     }

  138.     /** {@inheritDoc} */
  139.     @Override
  140.     public void init(final ODEStateAndDerivative s0, final double t) {
  141.         detector.init(s0, t);
  142.         lastT = Double.NEGATIVE_INFINITY;
  143.         lastG = Double.NaN;
  144.     }

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

  160.     /** Reinitialize the beginning of the step.
  161.      * @param interpolator valid for the current step
  162.      * @exception MathIllegalStateException if the interpolator throws one because
  163.      * the number of functions evaluations is exceeded
  164.      */
  165.     public void reinitializeBegin(final ODEStateInterpolator interpolator)
  166.         throws MathIllegalStateException {

  168.         forward = interpolator.isForward();
  169.         final ODEStateAndDerivative s0 = interpolator.getPreviousState();
  170.         t0 = s0.getTime();
  171.         g0 = g(s0);
  172.         while (g0 == 0) {
  173.             // excerpt from MATH-421 issue:
  174.             // If an ODE solver is setup with an ODEEventHandler that return STOP
  175.             // when the even is triggered, the integrator stops (which is exactly
  176.             // the expected behavior). If however the user wants to restart the
  177.             // solver from the final state reached at the event with the same
  178.             // configuration (expecting the event to be triggered again at a
  179.             // later time), then the integrator may fail to start. It can get stuck
  180.             // at the previous event. The use case for the bug MATH-421 is fairly
  181.             // general, so events occurring exactly at start in the first step should
  182.             // be ignored. Some g functions may be zero for multiple adjacent values of t
  183.             // so keep skipping roots while g(t) is zero.

  185.             // extremely rare case: there is a zero EXACTLY at interval start
  186.             // we will use the sign slightly after step beginning to force ignoring this zero
  187.             double tStart = t0 + (forward ? 0.5 : -0.5) * solver.getAbsoluteAccuracy();
  188.             // check for case where tolerance is too small to make a difference
  189.             if (tStart == t0) {
  190.                 tStart = nextAfter(t0);
  191.             }
  192.             t0 = tStart;
  193.             g0 = g(interpolator.getInterpolatedState(tStart));
  194.         }
  195.         g0Positive = g0 > 0;
  196.         // "last" event was increasing
  197.         increasing = g0Positive;

  199.     }

  201.     /** {@inheritDoc} */
  202.     @Override
  203.     public boolean evaluateStep(final ODEStateInterpolator interpolator)
  204.             throws MathIllegalArgumentException, MathIllegalStateException {

  206.         forward = interpolator.isForward();
  207.         final ODEStateAndDerivative s0 = interpolator.getPreviousState();
  208.         final ODEStateAndDerivative s1 = interpolator.getCurrentState();
  209.         final double t1 = s1.getTime();
  210.         final double dt = t1 - t0;
  211.         if (FastMath.abs(dt) < solver.getAbsoluteAccuracy()) {
  212.             // we cannot do anything on such a small step, don't trigger any events
  213.             pendingEvent     = false;
  214.             pendingEventTime = Double.NaN;
  215.             return false;
  216.         }

  218.         double ta = t0;
  219.         double ga = g0;
  220.         for (ODEStateAndDerivative sb = nextCheck(s0, s1, interpolator);
  221.              sb != null;
  222.              sb = nextCheck(sb, s1, interpolator)) {

  224.             // evaluate handler value at the end of the substep
  225.             final double tb = sb.getTime();
  226.             final double gb = g(sb);

  228.             // check events occurrence
  229.             if (gb == 0.0 || (g0Positive ^ (gb > 0))) {
  230.                 // there is a sign change: an event is expected during this step
  231.                 if (findRoot(interpolator, ta, ga, tb, gb)) {
  232.                     return true;
  233.                 }
  234.             } else {
  235.                 // no sign change: there is no event for now
  236.                 ta = tb;
  237.                 ga = gb;
  238.             }

  240.         }

  242.         // no event during the whole step
  243.         pendingEvent     = false;
  244.         pendingEventTime = Double.NaN;
  245.         return false;

  247.     }

  249.     /** Estimate next state to check.
  250.      * @param done state already checked
  251.      * @param target target state towards which we are checking
  252.      * @param interpolator step interpolator for the proposed step
  253.      * @return intermediate state to check, or exactly {@code null}
  254.      * if we already have {@code done == target}
  255.      * @since 3.0
  256.      */
  257.     private ODEStateAndDerivative nextCheck(final ODEStateAndDerivative done, final ODEStateAndDerivative target,
  258.                                             final ODEStateInterpolator interpolator) {
  259.         if (done == target) {
  260.             // we have already reached target
  261.             return null;
  262.         } else {
  263.             // we have to select some intermediate state
  264.             // attempting to split the remaining time in an integer number of checks
  265.             final double dt       = target.getTime() - done.getTime();
  266.             final double maxCheck = detector.getMaxCheckInterval().currentInterval(done, dt >= 0.);
  267.             final int    n        = FastMath.max(1, (int) FastMath.ceil(FastMath.abs(dt) / maxCheck));
  268.             return n == 1 ? target : interpolator.getInterpolatedState(done.getTime() + dt / n);
  269.         }
  270.     }

  272.     /**
  273.      * Find a root in a bracketing interval.
  274.      *
  275.      * <p> When calling this method one of the following must be true. Either ga == 0, gb
  276.      * == 0, (ga < 0  and gb > 0), or (ga > 0 and gb < 0).
  277.      *
  278.      * @param interpolator that covers the interval.
  279.      * @param ta           earliest possible time for root.
  280.      * @param ga           g(ta).
  281.      * @param tb           latest possible time for root.
  282.      * @param gb           g(tb).
  283.      * @return if a zero crossing was found.
  284.      */
  285.     private boolean findRoot(final ODEStateInterpolator interpolator,
  286.                              final double ta,
  287.                              final double ga,
  288.                              final double tb,
  289.                              final double gb) {
  290.         // check there appears to be a root in [ta, tb]
  291.         check(ga == 0.0 || gb == 0.0 || (ga > 0.0 && gb < 0.0) || (ga < 0.0 && gb > 0.0));

  293.         final int maxIterationCount = detector.getMaxIterationCount();
  294.         final UnivariateFunction f = t -> g(interpolator.getInterpolatedState(t));

  296.         // prepare loop below
  297.         double loopT = ta;
  298.         double loopG = ga;

  300.         // event time, just at or before the actual root.
  301.         double beforeRootT = Double.NaN;
  302.         double beforeRootG = Double.NaN;
  303.         // time on the other side of the root.
  304.         // Initialized the the loop below executes once.
  305.         double afterRootT = ta;
  306.         double afterRootG = 0.0;

  308.         // check for some conditions that the root finders don't like
  309.         // these conditions cannot not happen in the loop below
  310.         // the ga == 0.0 case is handled by the loop below
  311.         if (ta == tb) {
  312.             // both non-zero but times are the same. Probably due to reset state
  313.             beforeRootT = ta;
  314.             beforeRootG = ga;
  315.             afterRootT = shiftedBy(beforeRootT, solver.getAbsoluteAccuracy());
  316.             afterRootG = f.value(afterRootT);
  317.         } else if (ga != 0.0 && gb == 0.0) {
  318.             // hard: ga != 0.0 and gb == 0.0
  319.             // look past gb by up to convergence to find next sign
  320.             // throw an exception if g(t) = 0.0 in [tb, tb + convergence]
  321.             beforeRootT = tb;
  322.             beforeRootG = gb;
  323.             afterRootT = shiftedBy(beforeRootT, solver.getAbsoluteAccuracy());
  324.             afterRootG = f.value(afterRootT);
  325.         } else if (ga != 0.0) {
  326.             final double newGa = f.value(ta);
  327.             if (ga > 0 != newGa > 0) {
  328.                 // both non-zero, step sign change at ta, possibly due to reset state
  329.                 final double nextT = minTime(shiftedBy(ta, solver.getAbsoluteAccuracy()), tb);
  330.                 final double nextG = f.value(nextT);
  331.                 if (nextG > 0.0 == g0Positive) {
  332.                     // the sign change between ga and new Ga just moved the root less than one convergence
  333.                     // threshold later, we are still in a regular search for another root before tb,
  334.                     // we just need to fix the bracketing interval
  335.                     // (see issue https://github.com/Hipparchus-Math/hipparchus/issues/184)
  336.                     loopT = nextT;
  337.                     loopG = nextG;
  338.                 } else {
  339.                     beforeRootT = ta;
  340.                     beforeRootG = newGa;
  341.                     afterRootT  = nextT;
  342.                     afterRootG  = nextG;
  343.                 }

  345.             }
  346.         }

  348.         // loop to skip through "fake" roots, i.e. where g(t) = g'(t) = 0.0
  349.         // executed once if we didn't hit a special case above
  350.         while ((afterRootG == 0.0 || afterRootG > 0.0 == g0Positive) &&
  351.                strictlyAfter(afterRootT, tb)) {
  352.             if (loopG == 0.0) {
  353.                 // ga == 0.0 and gb may or may not be 0.0
  354.                 // handle the root at ta first
  355.                 beforeRootT = loopT;
  356.                 beforeRootG = loopG;
  357.                 afterRootT = minTime(shiftedBy(beforeRootT, solver.getAbsoluteAccuracy()), tb);
  358.                 afterRootG = f.value(afterRootT);
  359.             } else {
  360.                 // both non-zero, the usual case, use a root finder.
  361.                 if (forward) {
  362.                     try {
  363.                         final Interval interval =
  364.                                         solver.solveInterval(maxIterationCount, f, loopT, tb);
  365.                         beforeRootT = interval.getLeftAbscissa();
  366.                         beforeRootG = interval.getLeftValue();
  367.                         afterRootT = interval.getRightAbscissa();
  368.                         afterRootG = interval.getRightValue();
  369.                         // CHECKSTYLE: stop IllegalCatch check
  370.                     } catch (RuntimeException e) { // NOPMD
  371.                         // CHECKSTYLE: resume IllegalCatch check
  372.                         throw new MathIllegalStateException(e, LocalizedODEFormats.FIND_ROOT,
  373.                                                             detector, loopT, loopG, tb, gb, lastT, lastG);
  374.                     }
  375.                 } else {
  376.                     try {
  377.                         final Interval interval =
  378.                                         solver.solveInterval(maxIterationCount, f, tb, loopT);
  379.                         beforeRootT = interval.getRightAbscissa();
  380.                         beforeRootG = interval.getRightValue();
  381.                         afterRootT = interval.getLeftAbscissa();
  382.                         afterRootG = interval.getLeftValue();
  383.                         // CHECKSTYLE: stop IllegalCatch check
  384.                     } catch (RuntimeException e) { // NOPMD
  385.                         // CHECKSTYLE: resume IllegalCatch check
  386.                         throw new MathIllegalStateException(e, LocalizedODEFormats.FIND_ROOT,
  387.                                                             detector, tb, gb, loopT, loopG, lastT, lastG);
  388.                     }
  389.                 }
  390.             }
  391.             // tolerance is set to less than 1 ulp
  392.             // assume tolerance is 1 ulp
  393.             if (beforeRootT == afterRootT) {
  394.                 afterRootT = nextAfter(afterRootT);
  395.                 afterRootG = f.value(afterRootT);
  396.             }
  397.             // check loop is making some progress
  398.             check((forward && afterRootT > beforeRootT) || (!forward && afterRootT < beforeRootT));
  399.             // setup next iteration
  400.             loopT = afterRootT;
  401.             loopG = afterRootG;
  402.         }

  404.         // figure out the result of root finding, and return accordingly
  405.         if (afterRootG == 0.0 || afterRootG > 0.0 == g0Positive) {
  406.             // loop gave up and didn't find any crossing within this step
  407.             return false;
  408.         } else {
  409.             // real crossing
  410.             check(!Double.isNaN(beforeRootT) && !Double.isNaN(beforeRootG));
  411.             // variation direction, with respect to the integration direction
  412.             increasing = !g0Positive;
  413.             pendingEventTime = beforeRootT;
  414.             stopTime = beforeRootG == 0.0 ? beforeRootT : afterRootT;
  415.             pendingEvent = true;
  416.             afterEvent = afterRootT;
  417.             afterG = afterRootG;

  419.             // check increasing set correctly
  420.             check(afterG > 0 == increasing);
  421.             check(increasing == gb >= ga);

  423.             return true;
  424.         }

  426.     }

  428.     /**
  429.      * Get the next number after the given number in the current propagation direction.
  430.      *
  431.      * @param t input time
  432.      * @return t +/- 1 ulp depending on the direction.
  433.      */
  434.     private double nextAfter(final double t) {
  435.         // direction
  436.         final double dir = forward ? Double.POSITIVE_INFINITY : Double.NEGATIVE_INFINITY;
  437.         return FastMath.nextAfter(t, dir);
  438.     }

  440.     /** {@inheritDoc} */
  441.     @Override
  442.     public double getEventTime() {
  443.         return pendingEvent ?
  444.                pendingEventTime :
  445.                (forward ? Double.POSITIVE_INFINITY : Double.NEGATIVE_INFINITY);
  446.     }

  448.     /**
  449.      * Try to accept the current history up to the given time.
  450.      *
  451.      * <p> It is not necessary to call this method before calling {@link
  452.      * #doEvent(ODEStateAndDerivative)} with the same state. It is necessary to call this
  453.      * method before you call {@link #doEvent(ODEStateAndDerivative)} on some other event
  454.      * detector.
  455.      *
  456.      * @param state        to try to accept.
  457.      * @param interpolator to use to find the new root, if any.
  458.      * @return if the event detector has an event it has not detected before that is on or
  459.      * before the same time as {@code state}. In other words {@code false} means continue
  460.      * on while {@code true} means stop and handle my event first.
  461.      */
  462.     public boolean tryAdvance(final ODEStateAndDerivative state,
  463.                               final ODEStateInterpolator interpolator) {
  464.         final double t = state.getTime();
  465.         // check this is only called before a pending event.
  466.         check(!pendingEvent || !strictlyAfter(pendingEventTime, t));

  468.         final boolean meFirst;

  470.         if (strictlyAfter(t, earliestTimeConsidered)) {
  471.             // just found an event and we know the next time we want to search again
  472.             meFirst = false;
  473.         } else {
  474.             // check g function to see if there is a new event
  475.             final double g = g(state);
  476.             final boolean positive = g > 0;

  478.             if (positive == g0Positive) {
  479.                 // g function has expected sign
  480.                 g0 = g; // g0Positive is the same
  481.                 meFirst = false;
  482.             } else {
  483.                 // found a root we didn't expect -> find precise location
  484.                 final double oldPendingEventTime = pendingEventTime;
  485.                 final boolean foundRoot = findRoot(interpolator, t0, g0, t, g);
  486.                 // make sure the new root is not the same as the old root, if one exists
  487.                 meFirst = foundRoot &&
  488.                           (Double.isNaN(oldPendingEventTime) || oldPendingEventTime != pendingEventTime);
  489.             }
  490.         }

  492.         if (!meFirst) {
  493.             // advance t0 to the current time so we can't find events that occur before t
  494.             t0 = t;
  495.         }

  497.         return meFirst;
  498.     }

  500.     /** {@inheritDoc} */
  501.     @Override
  502.     public EventOccurrence doEvent(final ODEStateAndDerivative state) {
  503.         // check event is pending and is at the same time
  504.         check(pendingEvent);
  505.         check(state.getTime() == this.pendingEventTime);

  507.         final Action action = handler.eventOccurred(state, detector, increasing == forward);
  508.         final ODEState newState;
  509.         if (action == Action.RESET_STATE) {
  510.             newState = handler.resetState(detector, state);
  511.         } else {
  512.             newState = state;
  513.         }
  514.         // clear pending event
  515.         pendingEvent = false;
  516.         pendingEventTime = Double.NaN;
  517.         // setup for next search
  518.         earliestTimeConsidered = afterEvent;
  519.         t0 = afterEvent;
  520.         g0 = afterG;
  521.         g0Positive = increasing;
  522.         // check g0Positive set correctly
  523.         check(g0 == 0.0 || g0Positive == (g0 > 0));
  524.         return new EventOccurrence(action, newState, stopTime);
  525.     }

  527.     /**
  528.      * Shift a time value along the current integration direction: {@link #forward}.
  529.      *
  530.      * @param t     the time to shift.
  531.      * @param delta the amount to shift.
  532.      * @return t + delta if forward, else t - delta. If the result has to be rounded it
  533.      * will be rounded to be before the true value of t + delta.
  534.      */
  535.     private double shiftedBy(final double t, final double delta) {
  536.         if (forward) {
  537.             final double ret = t + delta;
  538.             if (ret - t > delta) {
  539.                 return FastMath.nextDown(ret);
  540.             } else {
  541.                 return ret;
  542.             }
  543.         } else {
  544.             final double ret = t - delta;
  545.             if (t - ret > delta) {
  546.                 return FastMath.nextUp(ret);
  547.             } else {
  548.                 return ret;
  549.             }
  550.         }
  551.     }

  553.     /**
  554.      * Get the time that happens first along the current propagation direction: {@link
  555.      * #forward}.
  556.      *
  557.      * @param a first time
  558.      * @param b second time
  559.      * @return min(a, b) if forward, else max (a, b)
  560.      */
  561.     private double minTime(final double a, final double b) {
  562.         return forward ? FastMath.min(a, b) : FastMath.max(a, b);
  563.     }

  565.     /**
  566.      * Check the ordering of two times.
  567.      *
  568.      * @param t1 the first time.
  569.      * @param t2 the second time.
  570.      * @return true if {@code t2} is strictly after {@code t1} in the propagation
  571.      * direction.
  572.      */
  573.     private boolean strictlyAfter(final double t1, final double t2) {
  574.         return forward ? t1 < t2 : t2 < t1;
  575.     }

  577.     /**
  578.      * Same as keyword assert, but throw a {@link MathRuntimeException}.
  579.      *
  580.      * @param condition to check
  581.      * @throws MathRuntimeException if {@code condition} is false.
  582.      */
  583.     private void check(final boolean condition) throws MathRuntimeException {
  584.         if (!condition) {
  585.             throw MathRuntimeException.createInternalError();
  586.         }
  587.     }

  589. }
Created with JaCoCo 0.8.12.202403310830