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.CalculusFieldElement;
26 import org.hipparchus.analysis.CalculusFieldUnivariateFunction;
27 import org.hipparchus.analysis.solvers.BracketedRealFieldUnivariateSolver;
28 import org.hipparchus.analysis.solvers.BracketedRealFieldUnivariateSolver.Interval;
29 import org.hipparchus.exception.MathIllegalArgumentException;
30 import org.hipparchus.exception.MathIllegalStateException;
31 import org.hipparchus.exception.MathRuntimeException;
32 import org.hipparchus.ode.FieldODEState;
33 import org.hipparchus.ode.FieldODEStateAndDerivative;
34 import org.hipparchus.ode.LocalizedODEFormats;
35 import org.hipparchus.ode.sampling.FieldODEStateInterpolator;
36 import org.hipparchus.util.FastMath;
37
38 /** This class handles the state for one {@link FieldODEEventHandler
39 * event handler} during integration steps.
40 *
41 * <p>Each time the integrator proposes a step, the event handler
42 * switching function should be checked. This class handles the state
43 * of one handler during one integration step, with references to the
44 * state at the end of the preceding step. This information is used to
45 * decide if the handler should trigger an event or not during the
46 * proposed step.</p>
47 *
48 * @param <T> the type of the field elements
49 */
50 public class FieldDetectorBasedEventState<T extends CalculusFieldElement<T>> implements FieldEventState<T> {
51
52 /** Event detector.
53 * @since 3.0
54 */
55 private final FieldODEEventDetector<T> detector;
56
57 /** Event solver.
58 * @since 3.0
59 */
60 private final BracketedRealFieldUnivariateSolver<T> solver;
61
62 /** Event handler. */
63 private final FieldODEEventHandler<T> handler;
64
65 /** Time of the previous call to g. */
66 private T lastT;
67
68 /** Value from the previous call to g. */
69 private T lastG;
70
71 /** Time at the beginning of the step. */
72 private T t0;
73
74 /** Value of the events handler at the beginning of the step. */
75 private T g0;
76
77 /** Sign of g0. */
78 private boolean g0Positive;
79
80 /** Indicator of event expected during the step. */
81 private boolean pendingEvent;
82
83 /** Occurrence time of the pending event. */
84 private T pendingEventTime;
85
86 /**
87 * Time to stop propagation if the event is a stop event. Used to enable stopping at
88 * an event and then restarting after that event.
89 */
90 private T stopTime;
91
92 /** Time after the current event. */
93 private T afterEvent;
94
95 /** Value of the g function after the current event. */
96 private T afterG;
97
98 /** The earliest time considered for events. */
99 private T earliestTimeConsidered;
100
101 /** Integration direction. */
102 private boolean forward;
103
104 /** Variation direction around pending event.
105 * (this is considered with respect to the integration direction)
106 */
107 private boolean increasing;
108
109 /** Simple constructor.
110 * @param detector event detector
111 * @since 3.0
112 */
113 public FieldDetectorBasedEventState(final FieldODEEventDetector<T> detector) {
114
115 this.detector = detector;
116 this.solver = detector.getSolver();
117 this.handler = detector.getHandler();
118
119 // some dummy values ...
120 t0 = null;
121 g0 = null;
122 g0Positive = true;
123 pendingEvent = false;
124 pendingEventTime = null;
125 increasing = true;
126 earliestTimeConsidered = null;
127 afterEvent = null;
128 afterG = null;
129
130 }
131
132 /** Get the underlying event detector.
133 * @return underlying event detector
134 * @since 3.0
135 */
136 public FieldODEEventDetector<T> getEventDetector() {
137 return detector;
138 }
139
140 /** Initialize event handler at the start of an integration.
141 * <p>
142 * This method is called once at the start of the integration. It
143 * may be used by the event handler to initialize some internal data
144 * if needed.
145 * </p>
146 * @param s0 initial state
147 * @param t target time for the integration
148 *
149 */
150 @Override
151 public void init(final FieldODEStateAndDerivative<T> s0, final T t) {
152 detector.init(s0, t);
153 lastT = t.getField().getZero().newInstance(Double.NEGATIVE_INFINITY);
154 lastG = null;
155 }
156
157 /** Compute the value of the switching function.
158 * This function must be continuous (at least in its roots neighborhood),
159 * as the integrator will need to find its roots to locate the events.
160 * @param s the current state information: date, kinematics, attitude
161 * @return value of the switching function
162 */
163 private T g(final FieldODEStateAndDerivative<T> s) {
164 if (!s.getTime().subtract(lastT).isZero()) {
165 lastG = detector.g(s);
166 lastT = s.getTime();
167 }
168 return lastG;
169 }
170
171 /** Reinitialize the beginning of the step.
172 * @param interpolator valid for the current step
173 * @exception MathIllegalStateException if the interpolator throws one because
174 * the number of functions evaluations is exceeded
175 */
176 public void reinitializeBegin(final FieldODEStateInterpolator<T> interpolator)
177 throws MathIllegalStateException {
178
179 forward = interpolator.isForward();
180 final FieldODEStateAndDerivative<T> s0 = interpolator.getPreviousState();
181 t0 = s0.getTime();
182 g0 = g(s0);
183 while (g0.isZero()) {
184 // excerpt from MATH-421 issue:
185 // If an ODE solver is setup with a FieldODEEventHandler that return STOP
186 // when the even is triggered, the integrator stops (which is exactly
187 // the expected behavior). If however the user wants to restart the
188 // solver from the final state reached at the event with the same
189 // configuration (expecting the event to be triggered again at a
190 // later time), then the integrator may fail to start. It can get stuck
191 // at the previous event. The use case for the bug MATH-421 is fairly
192 // general, so events occurring exactly at start in the first step should
193 // be ignored.
194
195 // extremely rare case: there is a zero EXACTLY at interval start
196 // we will use the sign slightly after step beginning to force ignoring this zero
197 T tStart = t0.add(solver.getAbsoluteAccuracy().multiply(forward ? 0.5 : -0.5));
198 if (tStart.equals(t0)) {
199 tStart = nextAfter(t0);
200 }
201 t0 = tStart;
202 g0 = g(interpolator.getInterpolatedState(tStart));
203 }
204 g0Positive = g0.getReal() > 0;
205 // "last" event was increasing
206 increasing = g0Positive;
207
208 }
209
210 /** Evaluate the impact of the proposed step on the event handler.
211 * @param interpolator step interpolator for the proposed step
212 * @return true if the event handler triggers an event before
213 * the end of the proposed step
214 * @exception MathIllegalStateException if the interpolator throws one because
215 * the number of functions evaluations is exceeded
216 * @exception MathIllegalArgumentException if the event cannot be bracketed
217 */
218 @Override
219 public boolean evaluateStep(final FieldODEStateInterpolator<T> interpolator)
220 throws MathIllegalArgumentException, MathIllegalStateException {
221
222 forward = interpolator.isForward();
223 final FieldODEStateAndDerivative<T> s0 = interpolator.getPreviousState();
224 final FieldODEStateAndDerivative<T> s1 = interpolator.getCurrentState();
225 final T t1 = s1.getTime();
226 final T dt = t1.subtract(t0);
227 if (dt.abs().subtract(solver.getAbsoluteAccuracy()).getReal() < 0) {
228 // we cannot do anything on such a small step, don't trigger any events
229 pendingEvent = false;
230 pendingEventTime = null;
231 return false;
232 }
233
234 T ta = t0;
235 T ga = g0;
236 for (FieldODEStateAndDerivative<T>sb = nextCheck(s0, s1, interpolator);
237 sb != null;
238 sb = nextCheck(sb, s1, interpolator)) {
239
240 // evaluate handler value at the end of the substep
241 final T tb = sb.getTime();
242 final T gb = g(sb);
243
244 // check events occurrence
245 if (gb.getReal() == 0.0 || (g0Positive ^ (gb.getReal() > 0))) {
246 // there is a sign change: an event is expected during this step
247 if (findRoot(interpolator, ta, ga, tb, gb)) {
248 return true;
249 }
250 } else {
251 // no sign change: there is no event for now
252 ta = tb;
253 ga = gb;
254 }
255
256 }
257
258 // no event during the whole step
259 pendingEvent = false;
260 pendingEventTime = null;
261 return false;
262
263 }
264
265 /** Estimate next state to check.
266 * @param done state already checked
267 * @param target target state towards which we are checking
268 * @param interpolator step interpolator for the proposed step
269 * @return intermediate state to check, or exactly {@code null}
270 * if we already have {@code done == target}
271 * @since 3.0
272 */
273 private FieldODEStateAndDerivative<T> nextCheck(final FieldODEStateAndDerivative<T> done, final FieldODEStateAndDerivative<T> target,
274 final FieldODEStateInterpolator<T> interpolator) {
275 if (done == target) {
276 // we have already reached target
277 return null;
278 } else {
279 // we have to select some intermediate state
280 // attempting to split the remaining time in an integer number of checks
281 final T dt = target.getTime().subtract(done.getTime());
282 final double maxCheck = detector.getMaxCheckInterval().currentInterval(done);
283 final int n = FastMath.max(1, (int) FastMath.ceil(dt.abs().divide(maxCheck).getReal()));
284 return n == 1 ? target : interpolator.getInterpolatedState(done.getTime().add(dt.divide(n)));
285 }
286 }
287
288 /**
289 * Find a root in a bracketing interval.
290 *
291 * <p> When calling this method one of the following must be true. Either ga == 0, gb
292 * == 0, (ga < 0 and gb > 0), or (ga > 0 and gb < 0).
293 *
294 * @param interpolator that covers the interval.
295 * @param ta earliest possible time for root.
296 * @param ga g(ta).
297 * @param tb latest possible time for root.
298 * @param gb g(tb).
299 * @return if a zero crossing was found.
300 */
301 private boolean findRoot(final FieldODEStateInterpolator<T> interpolator,
302 final T ta,
303 final T ga,
304 final T tb,
305 final T gb) {
306 // check there appears to be a root in [ta, tb]
307 check(ga.getReal() == 0.0 || gb.getReal() == 0.0 ||
308 (ga.getReal() > 0.0 && gb.getReal() < 0.0) ||
309 (ga.getReal() < 0.0 && gb.getReal() > 0.0));
310
311 final int maxIterationCount = detector.getMaxIterationCount();
312 final CalculusFieldUnivariateFunction<T> f = t -> g(interpolator.getInterpolatedState(t));
313
314 // prepare loop below
315 T loopT = ta;
316 T loopG = ga;
317
318 // event time, just at or before the actual root.
319 T beforeRootT = null;
320 T beforeRootG = null;
321 // time on the other side of the root.
322 // Initialized the the loop below executes once.
323 T afterRootT = ta;
324 T afterRootG = ta.getField().getZero();
325
326 // check for some conditions that the root finders don't like
327 // these conditions cannot not happen in the loop below
328 // the ga == 0.0 case is handled by the loop below
329 if (ta.getReal() == tb.getReal()) {
330 // both non-zero but times are the same. Probably due to reset state
331 beforeRootT = ta;
332 beforeRootG = ga;
333 afterRootT = shiftedBy(beforeRootT, solver.getAbsoluteAccuracy());
334 afterRootG = f.value(afterRootT);
335 } else if (!ga.isZero() && gb.isZero()) {
336 // hard: ga != 0.0 and gb == 0.0
337 // look past gb by up to convergence to find next sign
338 // throw an exception if g(t) = 0.0 in [tb, tb + convergence]
339 beforeRootT = tb;
340 beforeRootG = gb;
341 afterRootT = shiftedBy(beforeRootT, solver.getAbsoluteAccuracy());
342 afterRootG = f.value(afterRootT);
343 } else if (!ga.isZero()) {
344 final T newGa = f.value(ta);
345 if (ga.getReal() > 0 != newGa.getReal() > 0) {
346 // both non-zero, step sign change at ta, possibly due to reset state
347 final T nextT = minTime(shiftedBy(ta, solver.getAbsoluteAccuracy()), tb);
348 final T nextG = f.value(nextT);
349 if (nextG.getReal() > 0.0 == g0Positive) {
350 // the sign change between ga and new Ga just moved the root less than one convergence
351 // threshold later, we are still in a regular search for another root before tb,
352 // we just need to fix the bracketing interval
353 // (see issue https://github.com/Hipparchus-Math/hipparchus/issues/184)
354 loopT = nextT;
355 loopG = nextG;
356 } else {
357 beforeRootT = ta;
358 beforeRootG = newGa;
359 afterRootT = nextT;
360 afterRootG = nextG;
361 }
362 }
363 }
364
365 // loop to skip through "fake" roots, i.e. where g(t) = g'(t) = 0.0
366 // executed once if we didn't hit a special case above
367 while ((afterRootG.isZero() || afterRootG.getReal() > 0.0 == g0Positive) &&
368 strictlyAfter(afterRootT, tb)) {
369 if (loopG.getReal() == 0.0) {
370 // ga == 0.0 and gb may or may not be 0.0
371 // handle the root at ta first
372 beforeRootT = loopT;
373 beforeRootG = loopG;
374 afterRootT = minTime(shiftedBy(beforeRootT, solver.getAbsoluteAccuracy()), tb);
375 afterRootG = f.value(afterRootT);
376 } else {
377 // both non-zero, the usual case, use a root finder.
378 if (forward) {
379 try {
380 final Interval<T> interval =
381 solver.solveInterval(maxIterationCount, f, loopT, tb);
382 beforeRootT = interval.getLeftAbscissa();
383 beforeRootG = interval.getLeftValue();
384 afterRootT = interval.getRightAbscissa();
385 afterRootG = interval.getRightValue();
386 // CHECKSTYLE: stop IllegalCatch check
387 } catch (RuntimeException e) { // NOPMD
388 // CHECKSTYLE: resume IllegalCatch check
389 throw new MathIllegalStateException(e, LocalizedODEFormats.FIND_ROOT,
390 detector, loopT.getReal(), loopG.getReal(),
391 tb.getReal(), gb.getReal(),
392 lastT.getReal(), lastG.getReal());
393 }
394 } else {
395 try {
396 final Interval<T> interval =
397 solver.solveInterval(maxIterationCount, f, tb, loopT);
398 beforeRootT = interval.getRightAbscissa();
399 beforeRootG = interval.getRightValue();
400 afterRootT = interval.getLeftAbscissa();
401 afterRootG = interval.getLeftValue();
402 // CHECKSTYLE: stop IllegalCatch check
403 } catch (RuntimeException e) { // NOPMD
404 // CHECKSTYLE: resume IllegalCatch check
405 throw new MathIllegalStateException(e, LocalizedODEFormats.FIND_ROOT,
406 detector, tb.getReal(), gb.getReal(),
407 loopT.getReal(), loopG.getReal(),
408 lastT.getReal(), lastG.getReal());
409 }
410 }
411 }
412 // tolerance is set to less than 1 ulp
413 // assume tolerance is 1 ulp
414 if (beforeRootT == afterRootT) {
415 afterRootT = nextAfter(afterRootT);
416 afterRootG = f.value(afterRootT);
417 }
418 // check loop is making some progress
419 check((forward && afterRootT.getReal() > beforeRootT.getReal()) ||
420 (!forward && afterRootT.getReal() < beforeRootT.getReal()));
421 // setup next iteration
422 loopT = afterRootT;
423 loopG = afterRootG;
424 }
425
426 // figure out the result of root finding, and return accordingly
427 if (afterRootG.getReal() == 0.0 || afterRootG.getReal() > 0.0 == g0Positive) {
428 // loop gave up and didn't find any crossing within this step
429 return false;
430 } else {
431 // real crossing
432 check(beforeRootT != null && beforeRootG != null);
433 // variation direction, with respect to the integration direction
434 increasing = !g0Positive;
435 pendingEventTime = beforeRootT;
436 stopTime = beforeRootG.isZero() ? beforeRootT : afterRootT;
437 pendingEvent = true;
438 afterEvent = afterRootT;
439 afterG = afterRootG;
440
441 // check increasing set correctly
442 check(afterG.getReal() > 0 == increasing);
443 check(increasing == gb.getReal() >= ga.getReal());
444
445 return true;
446 }
447 }
448
449 /**
450 * Try to accept the current history up to the given time.
451 *
452 * <p> It is not necessary to call this method before calling {@link
453 * #doEvent(FieldODEStateAndDerivative)} with the same state. It is necessary to call this
454 * method before you call {@link #doEvent(FieldODEStateAndDerivative)} on some other event
455 * detector.
456 *
457 * @param state to try to accept.
458 * @param interpolator to use to find the new root, if any.
459 * @return if the event detector has an event it has not detected before that is on or
460 * before the same time as {@code state}. In other words {@code false} means continue
461 * on while {@code true} means stop and handle my event first.
462 */
463 public boolean tryAdvance(final FieldODEStateAndDerivative<T> state,
464 final FieldODEStateInterpolator<T> interpolator) {
465 final T t = state.getTime();
466 // check this is only called before a pending event.
467 check(!pendingEvent || !strictlyAfter(pendingEventTime, t));
468
469 final boolean meFirst;
470
471 // just found an event and we know the next time we want to search again
472 if (earliestTimeConsidered != null && strictlyAfter(t, earliestTimeConsidered)) {
473 meFirst = false;
474 } else {
475 // check g function to see if there is a new event
476 final T g = g(state);
477 final boolean positive = g.getReal() > 0;
478
479 if (positive == g0Positive) {
480 // g function has expected sign
481 g0 = g; // g0Positive is the same
482 meFirst = false;
483 } else {
484 // found a root we didn't expect -> find precise location
485 final T oldPendingEventTime = pendingEventTime;
486 final boolean foundRoot = findRoot(interpolator, t0, g0, t, g);
487 // make sure the new root is not the same as the old root, if one exists
488 meFirst = foundRoot && !pendingEventTime.equals(oldPendingEventTime);
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 /**
501 * Notify the user's listener of the event. The event occurs wholly within this method
502 * call including a call to {@link FieldODEEventHandler#resetState(FieldODEEventDetector,
503 * FieldODEStateAndDerivative)} if necessary.
504 *
505 * @param state the state at the time of the event. This must be at the same time as
506 * the current value of {@link #getEventTime()}.
507 * @return the user's requested action and the new state if the action is {@link
508 * Action#RESET_STATE}. Otherwise the new state is {@code state}. The stop time
509 * indicates what time propagation should stop if the action is {@link Action#STOP}.
510 * This guarantees the integration will stop on or after the root, so that integration
511 * may be restarted safely.
512 */
513 @Override
514 public FieldEventOccurrence<T> doEvent(final FieldODEStateAndDerivative<T> state) {
515 // check event is pending and is at the same time
516 check(pendingEvent);
517 check(state.getTime() == this.pendingEventTime);
518
519 final Action action = handler.eventOccurred(state, detector, increasing == forward);
520 final FieldODEState<T> newState;
521 if (action == Action.RESET_STATE) {
522 newState = handler.resetState(detector, state);
523 } else {
524 newState = state;
525 }
526 // clear pending event
527 pendingEvent = false;
528 pendingEventTime = null;
529 // setup for next search
530 earliestTimeConsidered = afterEvent;
531 t0 = afterEvent;
532 g0 = afterG;
533 g0Positive = increasing;
534 // check g0Positive set correctly
535 check(g0.isZero() || g0Positive == (g0.getReal() > 0));
536 return new FieldEventOccurrence<>(action, newState, stopTime);
537 }
538
539 /**
540 * Check the ordering of two times.
541 *
542 * @param t1 the first time.
543 * @param t2 the second time.
544 * @return true if {@code t2} is strictly after {@code t1} in the propagation
545 * direction.
546 */
547 private boolean strictlyAfter(final T t1, final T t2) {
548 return forward ? t1.getReal() < t2.getReal() : t2.getReal() < t1.getReal();
549 }
550
551 /**
552 * Get the next number after the given number in the current propagation direction.
553 *
554 * <p> Assumes T has the same precision as a double.
555 *
556 * @param t input time
557 * @return t +/- 1 ulp depending on the direction.
558 */
559 private T nextAfter(final T t) {
560 // direction
561 final int sign = forward ? 1 : -1;
562 final double ulp = FastMath.ulp(t.getReal());
563 return t.add(sign * ulp);
564 }
565
566 /**
567 * Same as keyword assert, but throw a {@link MathRuntimeException}.
568 *
569 * @param condition to check
570 * @throws MathRuntimeException if {@code condition} is false.
571 */
572 private void check(final boolean condition) throws MathRuntimeException {
573 if (!condition) {
574 throw MathRuntimeException.createInternalError();
575 }
576 }
577
578 /**
579 * Get the time that happens first along the current propagation direction: {@link
580 * #forward}.
581 *
582 * @param a first time
583 * @param b second time
584 * @return min(a, b) if forward, else max (a, b)
585 */
586 private T minTime(final T a, final T b) {
587 return forward ? FastMath.min(a, b) : FastMath.max(a, b);
588 }
589
590 /**
591 * Shift a time value along the current integration direction: {@link #forward}.
592 *
593 * @param t the time to shift.
594 * @param delta the amount to shift.
595 * @return t + delta if forward, else t - delta. If the result has to be rounded it
596 * will be rounded to be before the true value of t + delta.
597 */
598 private T shiftedBy(final T t, final T delta) {
599 if (forward) {
600 final T ret = t.add(delta);
601 if (ret.subtract(t).getReal() > delta.getReal()) {
602 // nextDown(ret)
603 return ret.subtract(FastMath.ulp(ret.getReal()));
604 } else {
605 return ret;
606 }
607 } else {
608 final T ret = t.subtract(delta);
609 if (t.subtract(ret).getReal() > delta.getReal()) {
610 // nextUp(ret)
611 return ret.add(FastMath.ulp(ret.getReal()));
612 } else {
613 return ret;
614 }
615 }
616 }
617
618 /** Get the occurrence time of the event triggered in the current step.
619 * @return occurrence time of the event triggered in the current
620 * step or infinity if no events are triggered
621 */
622 @Override
623 public T getEventTime() {
624 return pendingEvent ?
625 pendingEventTime :
626 t0.getField().getZero().add(forward ? Double.POSITIVE_INFINITY : Double.NEGATIVE_INFINITY);
627 }
628
629 }