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 */
17
18 /*
19 * This is not the original file distributed by the Apache Software Foundation
20 * It has been modified by the Hipparchus project
21 */
22
23 package org.hipparchus.ode.events;
24
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;
36
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 {
49
50 /** Event detector.
51 * @since 3.0
52 */
53 private final ODEEventDetector detector;
54
55 /** Event solver.
56 * @since 3.0
57 */
58 private final BracketedUnivariateSolver<UnivariateFunction> solver;
59
60 /** Event handler. */
61 private final ODEEventHandler handler;
62
63 /** Time of the previous call to g. */
64 private double lastT;
65
66 /** Value from the previous call to g. */
67 private double lastG;
68
69 /** Time at the beginning of the step. */
70 private double t0;
71
72 /** Value of the events handler at the beginning of the step. */
73 private double g0;
74
75 /** Sign of g0. */
76 private boolean g0Positive;
77
78 /** Indicator of event expected during the step. */
79 private boolean pendingEvent;
80
81 /** Occurrence time of the pending event. */
82 private double pendingEventTime;
83
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;
89
90 /** Time after the current event. */
91 private double afterEvent;
92
93 /** Value of the g function after the current event. */
94 private double afterG;
95
96 /** The earliest time considered for events. */
97 private double earliestTimeConsidered;
98
99 /** Integration direction. */
100 private boolean forward;
101
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;
107
108 /** Simple constructor.
109 * @param detector event detector
110 * @since 3.0
111 */
112 public DetectorBasedEventState(final ODEEventDetector detector) {
113
114 this.detector = detector;
115 this.solver = detector.getSolver();
116 this.handler = detector.getHandler();
117
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 }
129
130 /** Get the underlying event detector.
131 * @return underlying event detector
132 * @since 3.0
133 */
134 public ODEEventDetector getEventDetector() {
135 return detector;
136 }
137
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 }
145
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 }
159
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 {
167
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.
184
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;
198
199 }
200
201 /** {@inheritDoc} */
202 @Override
203 public boolean evaluateStep(final ODEStateInterpolator interpolator)
204 throws MathIllegalArgumentException, MathIllegalStateException {
205
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 }
217
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)) {
223
224 // evaluate handler value at the end of the substep
225 final double tb = sb.getTime();
226 final double gb = g(sb);
227
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 }
239
240 }
241
242 // no event during the whole step
243 pendingEvent = false;
244 pendingEventTime = Double.NaN;
245 return false;
246
247 }
248
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);
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 }
271
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));
292
293 final int maxIterationCount = detector.getMaxIterationCount();
294 final UnivariateFunction f = t -> g(interpolator.getInterpolatedState(t));
295
296 // prepare loop below
297 double loopT = ta;
298 double loopG = ga;
299
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;
307
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 }
344
345 }
346 }
347
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 }
403
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;
418
419 // check increasing set correctly
420 check(afterG > 0 == increasing);
421 check(increasing == gb >= ga);
422
423 return true;
424 }
425
426 }
427
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 }
439
440 /** {@inheritDoc} */
441 @Override
442 public double getEventTime() {
443 return pendingEvent ?
444 pendingEventTime :
445 (forward ? Double.POSITIVE_INFINITY : Double.NEGATIVE_INFINITY);
446 }
447
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));
467
468 final boolean meFirst;
469
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;
477
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 }
491
492 if (!meFirst) {
493 // advance t0 to the current time so we can't find events that occur before t
494 t0 = t;
495 }
496
497 return meFirst;
498 }
499
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);
506
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 }
526
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 }
552
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 }
564
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 }
576
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 }
588
589 }