Examples with BracketingNthOrderBrentSolver org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver used on opensource projects

Example 1 with BracketingNthOrderBrentSolver

use of org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver in project Orekit by CS-SI.

the class TurnAroundRangeMeasurementCreator method handleStep.

/**
 * Function handling the steps of the propagator
 * A turn-around measurement needs 2 stations, a master and a slave
 * The measurement is a signal:
 * - Emitted from the master ground station
 * - Reflected on the spacecraft
 * - Reflected on the slave ground station
 * - Reflected on the spacecraft again
 * - Received on the master ground station
 * Its value is the elapsed time between emission and reception
 * divided by 2c were c is the speed of light.
 *
 * The path of the signal is divided into 2 legs:
 *  - The 1st leg goes from emission by the master station to reception by the slave station
 *  - The 2nd leg goes from emission by the slave station to reception by the master station
 *
 * The spacecraft state date should, after a few iterations of the estimation process, be
 * set to the date of arrival/departure of the signal to/from the slave station.
 * It is guaranteed by implementation of the estimated measurement.
 * This is done to avoid big shifts in time to compute the transit states.
 * See TurnAroundRange.java for more
 * Thus the spacecraft date is the date when the 1st leg of the path ends and the 2nd leg begins
 */
public void handleStep(final SpacecraftState currentState, final boolean isLast) throws OrekitException {
    try {
        for (Map.Entry<GroundStation, GroundStation> entry : context.TARstations.entrySet()) {
            final GroundStation masterStation = entry.getKey();
            final GroundStation slaveStation = entry.getValue();
            final AbsoluteDate date = currentState.getDate();
            final Frame inertial = currentState.getFrame();
            final Vector3D position = currentState.toTransform().getInverse().transformPosition(antennaPhaseCenter);
            // Create a TAR measurement only if elevation for both stations is higher than elevationMin°
            if ((masterStation.getBaseFrame().getElevation(position, inertial, date) > FastMath.toRadians(30.0)) && (slaveStation.getBaseFrame().getElevation(position, inertial, date) > FastMath.toRadians(30.0))) {
                // The solver used
                final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
                // Spacecraft date t = date of arrival/departure of the signal to/from from the slave station
                // Slave station position in inertial frame at t
                final Vector3D slaveStationPosition = slaveStation.getOffsetToInertial(inertial, date).transformPosition(Vector3D.ZERO);
                // Downlink time of flight to slave station
                // The date of arrival/departure of the signal to/from the slave station is known and
                // equal to spacecraft date t.
                // Therefore we can use the function "downlinkTimeOfFlight" from GroundStation class
                // final double slaveTauD = slaveStation.downlinkTimeOfFlight(currentState, date);
                final double slaveTauD = solver.solve(1000, new UnivariateFunction() {

                    public double value(final double x) throws OrekitExceptionWrapper {
                        final SpacecraftState transitState = currentState.shiftedBy(-x);
                        final double d = Vector3D.distance(transitState.toTransform().getInverse().transformPosition(antennaPhaseCenter), slaveStationPosition);
                        return d - x * Constants.SPEED_OF_LIGHT;
                    }
                }, -1.0, 1.0);
                // Uplink time of flight from slave station
                // A solver is used to know where the satellite is when it receives the signal
                // back from the slave station
                final double slaveTauU = solver.solve(1000, new UnivariateFunction() {

                    public double value(final double x) throws OrekitExceptionWrapper {
                        final SpacecraftState transitState = currentState.shiftedBy(+x);
                        final double d = Vector3D.distance(transitState.toTransform().getInverse().transformPosition(antennaPhaseCenter), slaveStationPosition);
                        return d - x * Constants.SPEED_OF_LIGHT;
                    }
                }, -1.0, 1.0);
                // Find the position of the master station at signal departure and arrival
                // ----
                // Transit state position & date for the 1st leg of the signal path
                final SpacecraftState S1 = currentState.shiftedBy(-slaveTauD);
                final Vector3D P1 = S1.toTransform().getInverse().transformPosition(antennaPhaseCenter);
                final AbsoluteDate T1 = date.shiftedBy(-slaveTauD);
                // Transit state position & date for the 2nd leg of the signal path
                final Vector3D P2 = currentState.shiftedBy(+slaveTauU).toTransform().getInverse().transformPosition(antennaPhaseCenter);
                final AbsoluteDate T2 = date.shiftedBy(+slaveTauU);
                // Master station downlink delay - from P2 to master station
                // We use a solver to know where the master station is when it receives
                // the signal back from the satellite on the 2nd leg of the path
                final double masterTauD = solver.solve(1000, new UnivariateFunction() {

                    public double value(final double x) throws OrekitExceptionWrapper {
                        try {
                            final Transform t = masterStation.getOffsetToInertial(inertial, T2.shiftedBy(+x));
                            final double d = Vector3D.distance(P2, t.transformPosition(Vector3D.ZERO));
                            return d - x * Constants.SPEED_OF_LIGHT;
                        } catch (OrekitException oe) {
                            throw new OrekitExceptionWrapper(oe);
                        }
                    }
                }, -1.0, 1.0);
                final AbsoluteDate masterReceptionDate = T2.shiftedBy(+masterTauD);
                final TimeStampedPVCoordinates masterStationAtReception = masterStation.getOffsetToInertial(inertial, masterReceptionDate).transformPVCoordinates(new TimeStampedPVCoordinates(masterReceptionDate, PVCoordinates.ZERO));
                // Master station uplink delay - from master station to P1
                // Here the state date is known. Thus we can use the function "signalTimeOfFlight"
                // of the AbstractMeasurement class
                final double masterTauU = AbstractMeasurement.signalTimeOfFlight(masterStationAtReception, P1, T1);
                final AbsoluteDate masterEmissionDate = T1.shiftedBy(-masterTauU);
                final Vector3D masterStationAtEmission = masterStation.getOffsetToInertial(inertial, masterEmissionDate).transformPosition(Vector3D.ZERO);
                // Uplink/downlink distance from/to slave station
                final double slaveDownLinkDistance = Vector3D.distance(P1, slaveStationPosition);
                final double slaveUpLinkDistance = Vector3D.distance(P2, slaveStationPosition);
                // Uplink/downlink distance from/to master station
                final double masterUpLinkDistance = Vector3D.distance(P1, masterStationAtEmission);
                final double masterDownLinkDistance = Vector3D.distance(P2, masterStationAtReception.getPosition());
                addMeasurement(new TurnAroundRange(masterStation, slaveStation, masterReceptionDate, 0.5 * (masterUpLinkDistance + slaveDownLinkDistance + slaveUpLinkDistance + masterDownLinkDistance), 1.0, 10));
            }
        }
    } catch (OrekitExceptionWrapper oew) {
        throw new OrekitException(oew.getException());
    } catch (OrekitException oe) {
        throw new OrekitException(oe);
    }
}

Example 2 with BracketingNthOrderBrentSolver

use of org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver in project Orekit by CS-SI.

the class Phasing method findLatitudeCrossing.

/**
 * Find the state at which the reference latitude is crossed.
 * @param latitude latitude to search for
 * @param guessDate guess date for the crossing
 * @param endDate maximal date not to overtake
 * @param shift shift value used to evaluate the latitude function bracketing around the guess date
 * @param maxShift maximum value that the shift value can take
 * @param propagator propagator used
 * @return state at latitude crossing time
 * @throws OrekitException if state cannot be propagated
 * @throws MathRuntimeException if latitude cannot be bracketed in the search interval
 */
private SpacecraftState findLatitudeCrossing(final double latitude, final AbsoluteDate guessDate, final AbsoluteDate endDate, final double shift, final double maxShift, final Propagator propagator) throws OrekitException, MathRuntimeException {
    // function evaluating to 0 at latitude crossings
    final UnivariateFunction latitudeFunction = new UnivariateFunction() {

        /**
         * {@inheritDoc}
         */
        public double value(double x) {
            try {
                final SpacecraftState state = propagator.propagate(guessDate.shiftedBy(x));
                final Vector3D position = state.getPVCoordinates(earth.getBodyFrame()).getPosition();
                final GeodeticPoint point = earth.transform(position, earth.getBodyFrame(), state.getDate());
                return point.getLatitude() - latitude;
            } catch (OrekitException oe) {
                throw new RuntimeException(oe);
            }
        }
    };
    // try to bracket the encounter
    double span;
    if (guessDate.shiftedBy(shift).compareTo(endDate) > 0) {
        // Take a 1e-3 security margin
        span = endDate.durationFrom(guessDate) - 1e-3;
    } else {
        span = shift;
    }
    while (!UnivariateSolverUtils.isBracketing(latitudeFunction, -span, span)) {
        if (2 * span > maxShift) {
            // let the Hipparchus exception be thrown
            UnivariateSolverUtils.verifyBracketing(latitudeFunction, -span, span);
        } else if (guessDate.shiftedBy(2 * span).compareTo(endDate) > 0) {
            // Out of range :
            return null;
        }
        // expand the search interval
        span *= 2;
    }
    // find the encounter in the bracketed interval
    final BaseUnivariateSolver<UnivariateFunction> solver = new BracketingNthOrderBrentSolver(0.1, 5);
    final double dt = solver.solve(1000, latitudeFunction, -span, span);
    return propagator.propagate(guessDate.shiftedBy(dt));
}

Example 3 with BracketingNthOrderBrentSolver

use of org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver 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;
    }
}

Example 4 with BracketingNthOrderBrentSolver

use of org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver in project Orekit by CS-SI.

the class Geoid method getIntersectionPoint.

/**
 * {@inheritDoc}
 *
 * <p> The intersection point is computed using a line search along the
 * specified line. This is accurate when the geoid is slowly varying.
 */
@Override
public GeodeticPoint getIntersectionPoint(final Line lineInFrame, final Vector3D closeInFrame, final Frame frame, final AbsoluteDate date) throws OrekitException {
    /*
         * It is assumed that the geoid is slowly varying over it's entire
         * surface. Therefore there will one local intersection.
         */
    // transform to body frame
    final Frame bodyFrame = this.getBodyFrame();
    final Transform frameToBody = frame.getTransformTo(bodyFrame, date);
    final Vector3D close = frameToBody.transformPosition(closeInFrame);
    final Line lineInBodyFrame = frameToBody.transformLine(lineInFrame);
    // set the line's direction so the solved for value is always positive
    final Line line;
    if (lineInBodyFrame.getAbscissa(close) < 0) {
        line = lineInBodyFrame.revert();
    } else {
        line = lineInBodyFrame;
    }
    final ReferenceEllipsoid ellipsoid = this.getEllipsoid();
    // calculate end points
    // distance from line to center of earth, squared
    final double d2 = line.pointAt(0.0).getNormSq();
    // the minimum abscissa, squared
    final double n = ellipsoid.getPolarRadius() + MIN_UNDULATION;
    final double minAbscissa2 = n * n - d2;
    // smaller end point of the interval = 0.0 or intersection with
    // min_undulation sphere
    final double lowPoint = FastMath.sqrt(FastMath.max(minAbscissa2, 0.0));
    // the maximum abscissa, squared
    final double x = ellipsoid.getEquatorialRadius() + MAX_UNDULATION;
    final double maxAbscissa2 = x * x - d2;
    // larger end point of the interval
    final double highPoint = FastMath.sqrt(maxAbscissa2);
    // line search function
    final UnivariateFunction heightFunction = new UnivariateFunction() {

        @Override
        public double value(final double x) {
            try {
                final GeodeticPoint geodetic = transform(line.pointAt(x), bodyFrame, date);
                return geodetic.getAltitude();
            } catch (OrekitException e) {
                // due to frame transform -> re-throw
                throw new RuntimeException(e);
            }
        }
    };
    // compute answer
    if (maxAbscissa2 < 0) {
        // ray does not pierce bounding sphere -> no possible intersection
        return null;
    }
    // solve line search problem to find the intersection
    final UnivariateSolver solver = new BracketingNthOrderBrentSolver();
    try {
        final double abscissa = solver.solve(MAX_EVALUATIONS, heightFunction, lowPoint, highPoint);
        // return intersection point
        return this.transform(line.pointAt(abscissa), bodyFrame, date);
    } catch (MathRuntimeException e) {
        // no intersection
        return null;
    }
}

Example 5 with BracketingNthOrderBrentSolver

use of org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver in project Orekit by CS-SI.

the class L1TransformProvider method getL1.

/**
 * Compute the coordinates of the L1 point.
 * @param primaryToSecondary relative position of secondary body with respect to primary body
 * @return coordinates of the L1 point given in frame: primaryBody.getInertiallyOrientedFrame()
 * @throws OrekitException if some frame specific error occurs at .getTransformTo()
 */
private Vector3D getL1(final Vector3D primaryToSecondary) throws OrekitException {
    // mass ratio
    final double massRatio = secondaryBody.getGM() / primaryBody.getGM();
    // Approximate position of L1 point, valid when m2 << m1
    final double bigR = primaryToSecondary.getNorm();
    final double baseR = bigR * (1 - FastMath.cbrt(massRatio / 3));
    // Accurate position of L1 point, by solving the L1 equilibrium equation
    final UnivariateFunction l1Equation = r -> {
        final double bigrminusR = bigR - r;
        final double lhs = 1.0 / (r * r);
        final double rhs1 = massRatio / (bigrminusR * bigrminusR);
        final double rhs2 = 1.0 / (bigR * bigR);
        final double rhs3 = (1 + massRatio) * bigrminusR * rhs2 / bigR;
        return lhs - (rhs1 + rhs2 - rhs3);
    };
    final double[] searchInterval = UnivariateSolverUtils.bracket(l1Equation, baseR, 0, bigR, 0.01 * bigR, 1, MAX_EVALUATIONS);
    final BracketingNthOrderBrentSolver solver = new BracketingNthOrderBrentSolver(RELATIVE_ACCURACY, ABSOLUTE_ACCURACY, FUNCTION_ACCURACY, MAX_ORDER);
    final double r = solver.solve(MAX_EVALUATIONS, l1Equation, searchInterval[0], searchInterval[1], AllowedSolution.ANY_SIDE);
    // L1 point is built
    return new Vector3D(r / bigR, primaryToSecondary);
}