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