use of org.hipparchus.analysis.solvers.BracketedUnivariateSolver.Interval in project Orekit by CS-SI.
the class FieldEventState method findRoot.
/**
* Find a root in a bracketing interval.
*
* <p> When calling this method one of the following must be true. Either ga == 0, gb
* == 0, (ga < 0 and gb > 0), or (ga > 0 and gb < 0).
*
* @param interpolator that covers the interval.
* @param ta earliest possible time for root.
* @param ga g(ta).
* @param tb latest possible time for root.
* @param gb g(tb).
* @return if a zero crossing was found.
* @throws OrekitException if the event detector throws one
*/
private boolean findRoot(final FieldOrekitStepInterpolator<T> interpolator, final FieldAbsoluteDate<T> ta, final T ga, final FieldAbsoluteDate<T> tb, final T gb) throws OrekitException {
final T zero = ga.getField().getZero();
// check there appears to be a root in [ta, tb]
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));
final T convergence = detector.getThreshold();
final int maxIterationCount = detector.getMaxIterationCount();
final BracketedUnivariateSolver<UnivariateFunction> solver = new BracketingNthOrderBrentSolver(0, convergence.getReal(), 0, 5);
// event time, just at or before the actual root.
FieldAbsoluteDate<T> beforeRootT = null;
T beforeRootG = zero.add(Double.NaN);
// time on the other side of the root.
// Initialized the the loop below executes once.
FieldAbsoluteDate<T> afterRootT = ta;
T afterRootG = zero;
// the ga == 0.0 case is handled by the loop below
if (ta.equals(tb)) {
// both non-zero but times are the same. Probably due to reset state
beforeRootT = ta;
beforeRootG = ga;
afterRootT = shiftedBy(beforeRootT, convergence);
afterRootG = g(interpolator.getInterpolatedState(afterRootT));
} else if (ga.getReal() != 0.0 && gb.getReal() == 0.0) {
// hard: ga != 0.0 and gb == 0.0
// look past gb by up to convergence to find next sign
// throw an exception if g(t) = 0.0 in [tb, tb + convergence]
beforeRootT = tb;
beforeRootG = gb;
afterRootT = shiftedBy(beforeRootT, convergence);
afterRootG = g(interpolator.getInterpolatedState(afterRootT));
} else if (ga.getReal() != 0.0) {
final T newGa = g(interpolator.getInterpolatedState(ta));
if (ga.getReal() > 0 != newGa.getReal() > 0) {
// both non-zero, step sign change at ta, possibly due to reset state
beforeRootT = ta;
beforeRootG = newGa;
afterRootT = minTime(shiftedBy(beforeRootT, convergence), tb);
afterRootG = g(interpolator.getInterpolatedState(afterRootT));
}
}
// loop to skip through "fake" roots, i.e. where g(t) = g'(t) = 0.0
// executed once if we didn't hit a special case above
FieldAbsoluteDate<T> loopT = ta;
T loopG = ga;
while ((afterRootG.getReal() == 0.0 || afterRootG.getReal() > 0.0 == g0Positive) && strictlyAfter(afterRootT, tb)) {
if (loopG.getReal() == 0.0) {
// ga == 0.0 and gb may or may not be 0.0
// handle the root at ta first
beforeRootT = loopT;
beforeRootG = loopG;
afterRootT = minTime(shiftedBy(beforeRootT, convergence), tb);
afterRootG = g(interpolator.getInterpolatedState(afterRootT));
} else {
// both non-zero, the usual case, use a root finder.
try {
// time zero for evaluating the function f. Needs to be final
final FieldAbsoluteDate<T> fT0 = loopT;
final UnivariateFunction f = dt -> {
try {
return g(interpolator.getInterpolatedState(fT0.shiftedBy(dt))).getReal();
} catch (OrekitException oe) {
throw new OrekitExceptionWrapper(oe);
}
};
// tb as a double for use in f
final T tbDouble = tb.durationFrom(fT0);
if (forward) {
final Interval interval = solver.solveInterval(maxIterationCount, f, 0, tbDouble.getReal());
beforeRootT = fT0.shiftedBy(interval.getLeftAbscissa());
beforeRootG = zero.add(interval.getLeftValue());
afterRootT = fT0.shiftedBy(interval.getRightAbscissa());
afterRootG = zero.add(interval.getRightValue());
} else {
final Interval interval = solver.solveInterval(maxIterationCount, f, tbDouble.getReal(), 0);
beforeRootT = fT0.shiftedBy(interval.getRightAbscissa());
beforeRootG = zero.add(interval.getRightValue());
afterRootT = fT0.shiftedBy(interval.getLeftAbscissa());
afterRootG = zero.add(interval.getLeftValue());
}
} catch (OrekitExceptionWrapper oew) {
throw oew.getException();
}
}
// assume tolerance is 1 ulp
if (beforeRootT.equals(afterRootT)) {
afterRootT = nextAfter(afterRootT);
afterRootG = g(interpolator.getInterpolatedState(afterRootT));
}
// check loop is making some progress
check((forward && afterRootT.compareTo(beforeRootT) > 0) || (!forward && afterRootT.compareTo(beforeRootT) < 0));
// setup next iteration
loopT = afterRootT;
loopG = afterRootG;
}
// figure out the result of root finding, and return accordingly
if (afterRootG.getReal() == 0.0 || afterRootG.getReal() > 0.0 == g0Positive) {
// loop gave up and didn't find any crossing within this step
return false;
} else {
// real crossing
check(beforeRootT != null && !Double.isNaN(beforeRootG.getReal()));
// variation direction, with respect to the integration direction
increasing = !g0Positive;
pendingEventTime = beforeRootT;
stopTime = beforeRootG.getReal() == 0.0 ? beforeRootT : afterRootT;
pendingEvent = true;
afterEvent = afterRootT;
afterG = afterRootG;
// check increasing set correctly
check(afterG.getReal() > 0 == increasing);
check(increasing == gb.getReal() >= ga.getReal());
return true;
}
}
use of org.hipparchus.analysis.solvers.BracketedUnivariateSolver.Interval in project Orekit by CS-SI.
the class EventState method findRoot.
/**
* Find a root in a bracketing interval.
*
* <p> When calling this method one of the following must be true. Either ga == 0, gb
* == 0, (ga < 0 and gb > 0), or (ga > 0 and gb < 0).
*
* @param interpolator that covers the interval.
* @param ta earliest possible time for root.
* @param ga g(ta).
* @param tb latest possible time for root.
* @param gb g(tb).
* @return if a zero crossing was found.
* @throws OrekitException if the event detector throws one
*/
private boolean findRoot(final OrekitStepInterpolator interpolator, final AbsoluteDate ta, final double ga, final AbsoluteDate tb, final double gb) throws OrekitException {
// check there appears to be a root in [ta, tb]
check(ga == 0.0 || gb == 0.0 || (ga > 0.0 && gb < 0.0) || (ga < 0.0 && gb > 0.0));
final double convergence = detector.getThreshold();
final int maxIterationCount = detector.getMaxIterationCount();
final BracketedUnivariateSolver<UnivariateFunction> solver = new BracketingNthOrderBrentSolver(0, convergence, 0, 5);
// event time, just at or before the actual root.
AbsoluteDate beforeRootT = null;
double beforeRootG = Double.NaN;
// time on the other side of the root.
// Initialized the the loop below executes once.
AbsoluteDate afterRootT = ta;
double afterRootG = 0.0;
// the ga == 0.0 case is handled by the loop below
if (ta.equals(tb)) {
// both non-zero but times are the same. Probably due to reset state
beforeRootT = ta;
beforeRootG = ga;
afterRootT = shiftedBy(beforeRootT, convergence);
afterRootG = g(interpolator.getInterpolatedState(afterRootT));
} else if (ga != 0.0 && gb == 0.0) {
// hard: ga != 0.0 and gb == 0.0
// look past gb by up to convergence to find next sign
// throw an exception if g(t) = 0.0 in [tb, tb + convergence]
beforeRootT = tb;
beforeRootG = gb;
afterRootT = shiftedBy(beforeRootT, convergence);
afterRootG = g(interpolator.getInterpolatedState(afterRootT));
} else if (ga != 0.0) {
final double newGa = g(interpolator.getInterpolatedState(ta));
if (ga > 0 != newGa > 0) {
// both non-zero, step sign change at ta, possibly due to reset state
beforeRootT = ta;
beforeRootG = newGa;
afterRootT = minTime(shiftedBy(beforeRootT, convergence), tb);
afterRootG = g(interpolator.getInterpolatedState(afterRootT));
}
}
// loop to skip through "fake" roots, i.e. where g(t) = g'(t) = 0.0
// executed once if we didn't hit a special case above
AbsoluteDate loopT = ta;
double loopG = ga;
while ((afterRootG == 0.0 || afterRootG > 0.0 == g0Positive) && strictlyAfter(afterRootT, tb)) {
if (loopG == 0.0) {
// ga == 0.0 and gb may or may not be 0.0
// handle the root at ta first
beforeRootT = loopT;
beforeRootG = loopG;
afterRootT = minTime(shiftedBy(beforeRootT, convergence), tb);
afterRootG = g(interpolator.getInterpolatedState(afterRootT));
} else {
// both non-zero, the usual case, use a root finder.
try {
// time zero for evaluating the function f. Needs to be final
final AbsoluteDate fT0 = loopT;
final UnivariateFunction f = dt -> {
try {
return g(interpolator.getInterpolatedState(fT0.shiftedBy(dt)));
} catch (OrekitException oe) {
throw new OrekitExceptionWrapper(oe);
}
};
// tb as a double for use in f
final double tbDouble = tb.durationFrom(fT0);
if (forward) {
final Interval interval = solver.solveInterval(maxIterationCount, f, 0, tbDouble);
beforeRootT = fT0.shiftedBy(interval.getLeftAbscissa());
beforeRootG = interval.getLeftValue();
afterRootT = fT0.shiftedBy(interval.getRightAbscissa());
afterRootG = interval.getRightValue();
} else {
final Interval interval = solver.solveInterval(maxIterationCount, f, tbDouble, 0);
beforeRootT = fT0.shiftedBy(interval.getRightAbscissa());
beforeRootG = interval.getRightValue();
afterRootT = fT0.shiftedBy(interval.getLeftAbscissa());
afterRootG = interval.getLeftValue();
}
} catch (OrekitExceptionWrapper oew) {
throw oew.getException();
}
}
// assume tolerance is 1 ulp
if (beforeRootT.equals(afterRootT)) {
afterRootT = nextAfter(afterRootT);
afterRootG = g(interpolator.getInterpolatedState(afterRootT));
}
// check loop is making some progress
check((forward && afterRootT.compareTo(beforeRootT) > 0) || (!forward && afterRootT.compareTo(beforeRootT) < 0));
// setup next iteration
loopT = afterRootT;
loopG = afterRootG;
}
// figure out the result of root finding, and return accordingly
if (afterRootG == 0.0 || afterRootG > 0.0 == g0Positive) {
// loop gave up and didn't find any crossing within this step
return false;
} else {
// real crossing
check(beforeRootT != null && !Double.isNaN(beforeRootG));
// variation direction, with respect to the integration direction
increasing = !g0Positive;
pendingEventTime = beforeRootT;
stopTime = beforeRootG == 0.0 ? beforeRootT : afterRootT;
pendingEvent = true;
afterEvent = afterRootT;
afterG = afterRootG;
// check increasing set correctly
check(afterG > 0 == increasing);
check(increasing == gb >= ga);
return true;
}
}
Aggregations