Search in sources :

Example 6 with BracketingNthOrderBrentSolver

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

the class L2TransformProvider method getL2.

/**
 * Compute the coordinates of the L2 point.
 * @param primaryToSecondary relative position of secondary body with respect to primary body
 * @return coordinates of the L2 point given in frame: primaryBody.getInertiallyOrientedFrame()
 * @throws OrekitException if some frame specific error occurs at .getTransformTo()
 */
private Vector3D getL2(final Vector3D primaryToSecondary) throws OrekitException {
    // mass ratio
    final double massRatio = secondaryBody.getGM() / primaryBody.getGM();
    // Approximate position of L2 point, valid when m2 << m1
    final double bigR = primaryToSecondary.getNorm();
    final double baseR = bigR * (FastMath.cbrt(massRatio / 3) + 1);
    // Accurate position of L2 point, by solving the L2 equilibrium equation
    final UnivariateFunction l2Equation = r -> {
        final double rminusbigR = r - bigR;
        final double lhs1 = 1.0 / (r * r);
        final double lhs2 = massRatio / (rminusbigR * rminusbigR);
        final double rhs1 = 1.0 / (bigR * bigR);
        final double rhs2 = (1 + massRatio) * rminusbigR * rhs1 / bigR;
        return (lhs1 + lhs2) - (rhs1 + rhs2);
    };
    final double[] searchInterval = UnivariateSolverUtils.bracket(l2Equation, baseR, 0, 2 * 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, l2Equation, searchInterval[0], searchInterval[1], AllowedSolution.ANY_SIDE);
    // L2 point is built
    return new Vector3D(r / bigR, primaryToSecondary);
}
Also used : FieldBracketingNthOrderBrentSolver(org.hipparchus.analysis.solvers.FieldBracketingNthOrderBrentSolver) AllowedSolution(org.hipparchus.analysis.solvers.AllowedSolution) UnivariateSolverUtils(org.hipparchus.analysis.solvers.UnivariateSolverUtils) BracketingNthOrderBrentSolver(org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver) Vector3D(org.hipparchus.geometry.euclidean.threed.Vector3D) UnivariateFunction(org.hipparchus.analysis.UnivariateFunction) FieldRotation(org.hipparchus.geometry.euclidean.threed.FieldRotation) Rotation(org.hipparchus.geometry.euclidean.threed.Rotation) FieldPVCoordinates(org.orekit.utils.FieldPVCoordinates) PVCoordinates(org.orekit.utils.PVCoordinates) CelestialBody(org.orekit.bodies.CelestialBody) FieldAbsoluteDate(org.orekit.time.FieldAbsoluteDate) Field(org.hipparchus.Field) RealFieldUnivariateFunction(org.hipparchus.analysis.RealFieldUnivariateFunction) OrekitException(org.orekit.errors.OrekitException) RealFieldElement(org.hipparchus.RealFieldElement) FieldVector3D(org.hipparchus.geometry.euclidean.threed.FieldVector3D) FastMath(org.hipparchus.util.FastMath) AbsoluteDate(org.orekit.time.AbsoluteDate) UnivariateFunction(org.hipparchus.analysis.UnivariateFunction) RealFieldUnivariateFunction(org.hipparchus.analysis.RealFieldUnivariateFunction) Vector3D(org.hipparchus.geometry.euclidean.threed.Vector3D) FieldVector3D(org.hipparchus.geometry.euclidean.threed.FieldVector3D) FieldBracketingNthOrderBrentSolver(org.hipparchus.analysis.solvers.FieldBracketingNthOrderBrentSolver) BracketingNthOrderBrentSolver(org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver)

Example 7 with BracketingNthOrderBrentSolver

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

the class TopocentricFrame method computeLimitVisibilityPoint.

/**
 * Compute the limit visibility point for a satellite in a given direction.
 * <p>
 * This method can be used to compute visibility circles around ground stations
 * for example, using a simple loop on azimuth, with either a fixed elevation
 * or an elevation that depends on azimuth to take ground masks into account.
 * </p>
 * @param radius satellite distance to Earth center
 * @param azimuth pointing azimuth from station
 * @param elevation pointing elevation from station
 * @return limit visibility point for the satellite
 * @throws OrekitException if point cannot be found
 */
public GeodeticPoint computeLimitVisibilityPoint(final double radius, final double azimuth, final double elevation) throws OrekitException {
    try {
        // convergence threshold on point position: 1mm
        final double deltaP = 0.001;
        final UnivariateSolver solver = new BracketingNthOrderBrentSolver(deltaP / Constants.WGS84_EARTH_EQUATORIAL_RADIUS, deltaP, deltaP, 5);
        // find the distance such that a point in the specified direction and at the solved-for
        // distance is exactly at the specified radius
        final double distance = solver.solve(1000, new UnivariateFunction() {

            /**
             * {@inheritDoc}
             */
            public double value(final double x) {
                try {
                    final GeodeticPoint gp = pointAtDistance(azimuth, elevation, x);
                    return parentShape.transform(gp).getNorm() - radius;
                } catch (OrekitException oe) {
                    throw new OrekitExceptionWrapper(oe);
                }
            }
        }, 0, 2 * radius);
        // return the limit point
        return pointAtDistance(azimuth, elevation, distance);
    } catch (MathRuntimeException mrte) {
        throw new OrekitException(mrte);
    } catch (OrekitExceptionWrapper lwe) {
        throw lwe.getException();
    }
}
Also used : OrekitExceptionWrapper(org.orekit.errors.OrekitExceptionWrapper) MathRuntimeException(org.hipparchus.exception.MathRuntimeException) UnivariateFunction(org.hipparchus.analysis.UnivariateFunction) UnivariateSolver(org.hipparchus.analysis.solvers.UnivariateSolver) OrekitException(org.orekit.errors.OrekitException) GeodeticPoint(org.orekit.bodies.GeodeticPoint) BracketingNthOrderBrentSolver(org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver)

Example 8 with BracketingNthOrderBrentSolver

use of org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver 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;
    }
}
Also used : BracketingNthOrderBrentSolver(org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver) UnivariateFunction(org.hipparchus.analysis.UnivariateFunction) MathRuntimeException(org.hipparchus.exception.MathRuntimeException) OrekitInternalError(org.orekit.errors.OrekitInternalError) OrekitExceptionWrapper(org.orekit.errors.OrekitExceptionWrapper) Interval(org.hipparchus.analysis.solvers.BracketedUnivariateSolver.Interval) Serializable(java.io.Serializable) Precision(org.hipparchus.util.Precision) EventHandler(org.orekit.propagation.events.handlers.EventHandler) OrekitException(org.orekit.errors.OrekitException) SpacecraftState(org.orekit.propagation.SpacecraftState) OrekitStepInterpolator(org.orekit.propagation.sampling.OrekitStepInterpolator) BracketedUnivariateSolver(org.hipparchus.analysis.solvers.BracketedUnivariateSolver) FastMath(org.hipparchus.util.FastMath) AbsoluteDate(org.orekit.time.AbsoluteDate) OrekitExceptionWrapper(org.orekit.errors.OrekitExceptionWrapper) UnivariateFunction(org.hipparchus.analysis.UnivariateFunction) OrekitException(org.orekit.errors.OrekitException) BracketingNthOrderBrentSolver(org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver) AbsoluteDate(org.orekit.time.AbsoluteDate) Interval(org.hipparchus.analysis.solvers.BracketedUnivariateSolver.Interval)

Example 9 with BracketingNthOrderBrentSolver

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

the class AngularAzElMeasurementCreator method handleStep.

public void handleStep(final SpacecraftState currentState, final boolean isLast) throws OrekitException {
    for (final GroundStation station : context.stations) {
        final AbsoluteDate date = currentState.getDate();
        final Frame inertial = currentState.getFrame();
        final Vector3D position = currentState.getPVCoordinates().getPosition();
        if (station.getBaseFrame().getElevation(position, inertial, date) > FastMath.toRadians(30.0)) {
            final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
            final double downLinkDelay = solver.solve(1000, new UnivariateFunction() {

                public double value(final double x) throws OrekitExceptionWrapper {
                    try {
                        final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(x));
                        final double d = Vector3D.distance(position, t.transformPosition(Vector3D.ZERO));
                        return d - x * Constants.SPEED_OF_LIGHT;
                    } catch (OrekitException oe) {
                        throw new OrekitExceptionWrapper(oe);
                    }
                }
            }, -1.0, 1.0);
            // Satellite position at signal departure
            final Vector3D satelliteAtDeparture = currentState.shiftedBy(-downLinkDelay).getPVCoordinates().getPosition();
            // Initialize measurement
            final double[] angular = new double[2];
            final double[] sigma = { 1.0, 1.0 };
            final double[] baseweight = { 10.0, 10.0 };
            // Compute measurement
            // Elevation
            angular[1] = station.getBaseFrame().getElevation(satelliteAtDeparture, currentState.getFrame(), currentState.getDate());
            // Azimuth
            angular[0] = station.getBaseFrame().getAzimuth(satelliteAtDeparture, currentState.getFrame(), currentState.getDate());
            addMeasurement(new AngularAzEl(station, date, angular, sigma, baseweight));
        }
    }
}
Also used : Frame(org.orekit.frames.Frame) OrekitExceptionWrapper(org.orekit.errors.OrekitExceptionWrapper) UnivariateFunction(org.hipparchus.analysis.UnivariateFunction) UnivariateSolver(org.hipparchus.analysis.solvers.UnivariateSolver) AbsoluteDate(org.orekit.time.AbsoluteDate) BracketingNthOrderBrentSolver(org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver) Vector3D(org.hipparchus.geometry.euclidean.threed.Vector3D) OrekitException(org.orekit.errors.OrekitException) Transform(org.orekit.frames.Transform)

Example 10 with BracketingNthOrderBrentSolver

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

the class AngularRaDecMeasurementCreator method handleStep.

public void handleStep(final SpacecraftState currentState, final boolean isLast) throws OrekitException {
    for (final GroundStation station : context.stations) {
        final AbsoluteDate date = currentState.getDate();
        final Frame inertial = currentState.getFrame();
        final Vector3D position = currentState.getPVCoordinates().getPosition();
        if (station.getBaseFrame().getElevation(position, inertial, date) > FastMath.toRadians(30.0)) {
            final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
            final double downLinkDelay = solver.solve(1000, new UnivariateFunction() {

                public double value(final double x) throws OrekitExceptionWrapper {
                    try {
                        final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(x));
                        final double d = Vector3D.distance(position, t.transformPosition(Vector3D.ZERO));
                        return d - x * Constants.SPEED_OF_LIGHT;
                    } catch (OrekitException oe) {
                        throw new OrekitExceptionWrapper(oe);
                    }
                }
            }, -1.0, 1.0);
            // Satellite position at signal departure
            final Vector3D satelliteAtDeparture = currentState.shiftedBy(-downLinkDelay).getPVCoordinates().getPosition();
            // Initialize measurement
            final double[] angular = new double[2];
            final double[] sigma = { 1.0, 1.0 };
            final double[] baseweight = { 10.0, 10.0 };
            // Inertial frame used
            final Frame inertialFrame = context.initialOrbit.getFrame();
            // Station position at signal arrival
            // Set the reference date of offset drivers arbitrarily to avoid exception
            station.getPrimeMeridianOffsetDriver().setReferenceDate(date);
            station.getPolarOffsetXDriver().setReferenceDate(date);
            station.getPolarOffsetYDriver().setReferenceDate(date);
            final Transform offsetToInertialArrival = station.getOffsetToInertial(inertialFrame, date);
            final Vector3D stationPArrival = offsetToInertialArrival.transformVector(Vector3D.ZERO);
            // Vector station position at signal arrival - satellite at signal departure
            // In inertial reference frame
            final Vector3D staSat = satelliteAtDeparture.subtract(stationPArrival);
            // Compute measurement
            // Right ascension
            final double baseRightAscension = staSat.getAlpha();
            angular[0] = MathUtils.normalizeAngle(baseRightAscension, 0.0);
            // Declination
            angular[1] = staSat.getDelta();
            addMeasurement(new AngularRaDec(station, inertialFrame, date, angular, sigma, baseweight));
        }
    }
}
Also used : Frame(org.orekit.frames.Frame) OrekitExceptionWrapper(org.orekit.errors.OrekitExceptionWrapper) UnivariateFunction(org.hipparchus.analysis.UnivariateFunction) UnivariateSolver(org.hipparchus.analysis.solvers.UnivariateSolver) AbsoluteDate(org.orekit.time.AbsoluteDate) BracketingNthOrderBrentSolver(org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver) Vector3D(org.hipparchus.geometry.euclidean.threed.Vector3D) OrekitException(org.orekit.errors.OrekitException) Transform(org.orekit.frames.Transform)

Aggregations

UnivariateFunction (org.hipparchus.analysis.UnivariateFunction)14 BracketingNthOrderBrentSolver (org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver)14 OrekitException (org.orekit.errors.OrekitException)14 Vector3D (org.hipparchus.geometry.euclidean.threed.Vector3D)11 OrekitExceptionWrapper (org.orekit.errors.OrekitExceptionWrapper)9 AbsoluteDate (org.orekit.time.AbsoluteDate)9 UnivariateSolver (org.hipparchus.analysis.solvers.UnivariateSolver)8 MathRuntimeException (org.hipparchus.exception.MathRuntimeException)6 Frame (org.orekit.frames.Frame)6 Transform (org.orekit.frames.Transform)6 FastMath (org.hipparchus.util.FastMath)4 GeodeticPoint (org.orekit.bodies.GeodeticPoint)4 SpacecraftState (org.orekit.propagation.SpacecraftState)4 Field (org.hipparchus.Field)3 RealFieldElement (org.hipparchus.RealFieldElement)3 RealFieldUnivariateFunction (org.hipparchus.analysis.RealFieldUnivariateFunction)3 FieldBracketingNthOrderBrentSolver (org.hipparchus.analysis.solvers.FieldBracketingNthOrderBrentSolver)3 FieldVector3D (org.hipparchus.geometry.euclidean.threed.FieldVector3D)3 FieldAbsoluteDate (org.orekit.time.FieldAbsoluteDate)3 PVCoordinates (org.orekit.utils.PVCoordinates)3