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