TopocentricFrame.java

  1. /* Copyright 2002-2025 CS GROUP
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS 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.frames;

  19. import org.hipparchus.Field;
  20. import org.hipparchus.CalculusFieldElement;
  21. import org.hipparchus.analysis.UnivariateFunction;
  22. import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
  23. import org.hipparchus.analysis.solvers.UnivariateSolver;
  24. import org.hipparchus.exception.MathRuntimeException;
  25. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  26. import org.hipparchus.geometry.euclidean.threed.Rotation;
  27. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  28. import org.hipparchus.util.FastMath;
  29. import org.hipparchus.util.FieldSinCos;
  30. import org.hipparchus.util.MathUtils;
  31. import org.hipparchus.util.SinCos;
  32. import org.orekit.bodies.BodyShape;
  33. import org.orekit.bodies.FieldGeodeticPoint;
  34. import org.orekit.bodies.GeodeticPoint;
  35. import org.orekit.errors.OrekitException;
  36. import org.orekit.time.AbsoluteDate;
  37. import org.orekit.time.FieldAbsoluteDate;
  38. import org.orekit.utils.Constants;
  39. import org.orekit.utils.ExtendedPositionProvider;
  40. import org.orekit.utils.FieldPVCoordinates;
  41. import org.orekit.utils.FieldTrackingCoordinates;
  42. import org.orekit.utils.PVCoordinates;
  43. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  44. import org.orekit.utils.TimeStampedPVCoordinates;
  45. import org.orekit.utils.TrackingCoordinates;


  48. /** Topocentric frame.
  49.  * <p>Frame associated to a position near the surface of a body shape.</p>
  50.  * <p>
  51.  * The origin of the frame is at the defining {@link GeodeticPoint geodetic point}
  52.  * location, and the right-handed canonical trihedra is:
  53.  * </p>
  54.  * <ul>
  55.  *   <li>X axis in the local horizontal plane (normal to zenith direction) and
  56.  *   following the local parallel towards East</li>
  57.  *   <li>Y axis in the horizontal plane (normal to zenith direction) and
  58.  *   following the local meridian towards North</li>
  59.  *   <li>Z axis towards Zenith direction</li>
  60.  * </ul>
  61.  * @author V&eacute;ronique Pommier-Maurussane
  62.  */
  63. public class TopocentricFrame extends Frame implements ExtendedPositionProvider {

  65.     /** Body shape on which the local point is defined. */
  66.     private final BodyShape parentShape;

  68.     /** Geodetic point where the topocentric frame is defined. */
  69.     private final GeodeticPoint point;

  71.     /** Cartesian point where the topocentric frame is defined.
  72.      * @since 12.0
  73.      */
  74.     private final Vector3D cartesianPoint;

  76.     /** Simple constructor.
  77.      * @param parentShape body shape on which the local point is defined
  78.      * @param point local surface point where topocentric frame is defined
  79.      * @param name the string representation
  80.      */
  81.     public TopocentricFrame(final BodyShape parentShape, final GeodeticPoint point,
  82.                             final String name) {

  84.         super(parentShape.getBodyFrame(),
  85.                 new Transform(AbsoluteDate.ARBITRARY_EPOCH,
  86.                         new Transform(AbsoluteDate.ARBITRARY_EPOCH,
  87.                                 parentShape.transform(point).negate()),
  88.                         new Transform(AbsoluteDate.ARBITRARY_EPOCH,
  89.                                 new Rotation(point.getEast(), point.getZenith(),
  90.                                         Vector3D.PLUS_I, Vector3D.PLUS_K),
  91.                                 Vector3D.ZERO)),
  92.                 name, false);
  93.         this.parentShape    = parentShape;
  94.         this.point          = point;
  95.         this.cartesianPoint = getTransformProvider().
  96.                 getStaticTransform(AbsoluteDate.ARBITRARY_EPOCH).
  97.                 getInverse().
  98.                 transformPosition(Vector3D.ZERO);
  99.     }

  101.     /** Get the body shape on which the local point is defined.
  102.      * @return body shape on which the local point is defined
  103.      */
  104.     public BodyShape getParentShape() {
  105.         return parentShape;
  106.     }

  108.     /** Get the surface point defining the origin of the frame.
  109.      * @return surface point defining the origin of the frame
  110.      */
  111.     public GeodeticPoint getPoint() {
  112.         return point;
  113.     }

  115.     /** Get the surface point defining the origin of the frame.
  116.      * @return surface point defining the origin of the frame in body frame
  117.      * @since 12.0
  118.      */
  119.     public Vector3D getCartesianPoint() {
  120.         return cartesianPoint;
  121.     }

  123.     /** Get the surface point defining the origin of the frame.
  124.      * @param <T> type of the elements
  125.      * @param field of the elements
  126.      * @return surface point defining the origin of the frame
  127.      * @since 9.3
  128.      */
  129.     public <T extends CalculusFieldElement<T>> FieldGeodeticPoint<T> getPoint(final Field<T> field) {
  130.         final T zero = field.getZero();
  131.         return new FieldGeodeticPoint<>(zero.newInstance(point.getLatitude()),
  132.                 zero.newInstance(point.getLongitude()),
  133.                 zero.newInstance(point.getAltitude()));
  134.     }

  136.     /** Get the zenith direction of topocentric frame, expressed in parent shape frame.
  137.      * <p>The zenith direction is defined as the normal to local horizontal plane.</p>
  138.      * @return unit vector in the zenith direction
  139.      * @see #getNadir()
  140.      */
  141.     public Vector3D getZenith() {
  142.         return point.getZenith();
  143.     }

  145.     /** Get the nadir direction of topocentric frame, expressed in parent shape frame.
  146.      * <p>The nadir direction is the opposite of zenith direction.</p>
  147.      * @return unit vector in the nadir direction
  148.      * @see #getZenith()
  149.      */
  150.     public Vector3D getNadir() {
  151.         return point.getNadir();
  152.     }

  154.     /** Get the north direction of topocentric frame, expressed in parent shape frame.
  155.      * <p>The north direction is defined in the horizontal plane
  156.      * (normal to zenith direction) and following the local meridian.</p>
  157.      * @return unit vector in the north direction
  158.      * @see #getSouth()
  159.      */
  160.     public Vector3D getNorth() {
  161.         return point.getNorth();
  162.     }

  164.     /** Get the south direction of topocentric frame, expressed in parent shape frame.
  165.      * <p>The south direction is the opposite of north direction.</p>
  166.      * @return unit vector in the south direction
  167.      * @see #getNorth()
  168.      */
  169.     public Vector3D getSouth() {
  170.         return point.getSouth();
  171.     }

  173.     /** Get the east direction of topocentric frame, expressed in parent shape frame.
  174.      * <p>The east direction is defined in the horizontal plane
  175.      * in order to complete direct triangle (east, north, zenith).</p>
  176.      * @return unit vector in the east direction
  177.      * @see #getWest()
  178.      */
  179.     public Vector3D getEast() {
  180.         return point.getEast();
  181.     }

  183.     /** Get the west direction of topocentric frame, expressed in parent shape frame.
  184.      * <p>The west direction is the opposite of east direction.</p>
  185.      * @return unit vector in the west direction
  186.      * @see #getEast()
  187.      */
  188.     public Vector3D getWest() {
  189.         return point.getWest();
  190.     }

  192.     /** Get the tracking coordinates of a point with regards to the local point.
  193.      * @param extPoint point for which elevation shall be computed
  194.      * @param frame frame in which the point is defined
  195.      * @param date computation date
  196.      * @return tracking coordinates of the point
  197.      * @since 12.0
  198.      */
  199.     public TrackingCoordinates getTrackingCoordinates(final Vector3D extPoint, final Frame frame,
  200.                                                       final AbsoluteDate date) {

  202.         // transform given point from given frame to topocentric frame
  203.         final Vector3D extPointTopo = transformPoint(extPoint, frame, date);

  205.         final double azimuth = computeAzimuthFromTopoPoint(extPointTopo);

  207.         return new TrackingCoordinates(azimuth, extPointTopo.getDelta(), extPointTopo.getNorm());

  209.     }

  211.     /** Get the tracking coordinates of a point with regards to the local point.
  212.      * @param <T> type of the field elements
  213.      * @param extPoint point for which elevation shall be computed
  214.      * @param frame frame in which the point is defined
  215.      * @param date computation date
  216.      * @return tracking coordinates of the point
  217.      * @since 12.0
  218.      */
  219.     public <T extends CalculusFieldElement<T>> FieldTrackingCoordinates<T> getTrackingCoordinates(final FieldVector3D<T> extPoint,
  220.                                                                                                   final Frame frame,
  221.                                                                                                   final FieldAbsoluteDate<T> date) {

  223.         // Transform given point from given frame to topocentric frame
  224.         final FieldVector3D<T> extPointTopo = transformPoint(extPoint, frame, date);

  226.         final T azimuth = computeAzimuthFromTopoPoint(extPointTopo);

  228.         return new FieldTrackingCoordinates<>(azimuth, extPointTopo.getDelta(), extPointTopo.getNorm());

  230.     }

  232.     /** Get the elevation of a point with regards to the local point.
  233.      * <p>The elevation is the angle between the local horizontal and
  234.      * the direction from local point to given point.</p>
  235.      * @param extPoint point for which elevation shall be computed
  236.      * @param frame frame in which the point is defined
  237.      * @param date computation date
  238.      * @return elevation of the point
  239.      */
  240.     public double getElevation(final Vector3D extPoint, final Frame frame,
  241.                                final AbsoluteDate date) {

  243.         // Transform given point from given frame to topocentric frame
  244.         final Vector3D extPointTopo = transformPoint(extPoint, frame, date);

  246.         return extPointTopo.getDelta();
  247.     }

  249.     /** Get the elevation of a point with regards to the local point.
  250.      * <p>The elevation is the angle between the local horizontal and
  251.      * the direction from local point to given point.</p>
  252.      * @param <T> type of the elements
  253.      * @param extPoint point for which elevation shall be computed
  254.      * @param frame frame in which the point is defined
  255.      * @param date computation date
  256.      * @return elevation of the point
  257.      * @since 9.3
  258.      */
  259.     public <T extends CalculusFieldElement<T>> T getElevation(final FieldVector3D<T> extPoint, final Frame frame,
  260.                                                               final FieldAbsoluteDate<T> date) {

  262.         // Transform given point from given frame to topocentric frame
  263.         final FieldVector3D<T> extPointTopo = transformPoint(extPoint, frame, date);

  265.         return extPointTopo.getDelta();
  266.     }

  268.     /** Get the azimuth of a point with regards to the topocentric frame center point.
  269.      * <p>The azimuth is the angle between the North direction at local point and
  270.      * the projection in local horizontal plane of the direction from local point
  271.      * to given point. Azimuth angles are counted clockwise, i.e positive towards the East.</p>
  272.      * @param extPoint point for which elevation shall be computed
  273.      * @param frame frame in which the point is defined
  274.      * @param date computation date
  275.      * @return azimuth of the point
  276.      */
  277.     public double getAzimuth(final Vector3D extPoint, final Frame frame,
  278.                              final AbsoluteDate date) {

  280.         // Transform given point from given frame to topocentric frame
  281.         final Vector3D extPointTopo = transformPoint(extPoint, frame, date);

  283.         return computeAzimuthFromTopoPoint(extPointTopo);

  285.     }

  287.     /** Get the azimuth of a point with regards to the topocentric frame center point.
  288.      * <p>The azimuth is the angle between the North direction at local point and
  289.      * the projection in local horizontal plane of the direction from local point
  290.      * to given point. Azimuth angles are counted clockwise, i.e positive towards the East.</p>
  291.      * @param <T> type of the elements
  292.      * @param extPoint point for which elevation shall be computed
  293.      * @param frame frame in which the point is defined
  294.      * @param date computation date
  295.      * @return azimuth of the point
  296.      * @since 9.3
  297.      */
  298.     public <T extends CalculusFieldElement<T>> T getAzimuth(final FieldVector3D<T> extPoint, final Frame frame,
  299.                                                             final FieldAbsoluteDate<T> date) {

  301.         // Transform given point from given frame to topocentric frame
  302.         final FieldVector3D<T> extPointTopo = transformPoint(extPoint, frame, date);

  304.         return computeAzimuthFromTopoPoint(extPointTopo);

  306.     }

  308.     /** Get the range of a point with regards to the topocentric frame center point.
  309.      * @param extPoint point for which range shall be computed
  310.      * @param frame frame in which the point is defined
  311.      * @param date computation date
  312.      * @return range (distance) of the point
  313.      */
  314.     public double getRange(final Vector3D extPoint, final Frame frame,
  315.                            final AbsoluteDate date) {

  317.         // Transform given point from given frame to topocentric frame
  318.         final Vector3D extPointTopo = transformPoint(extPoint, frame, date);

  320.         return extPointTopo.getNorm();

  322.     }

  324.     /** Get the range of a point with regards to the topocentric frame center point.
  325.      * @param <T> type of the elements
  326.      * @param extPoint point for which range shall be computed
  327.      * @param frame frame in which the point is defined
  328.      * @param date computation date
  329.      * @return range (distance) of the point
  330.      * @since 9.3
  331.      */
  332.     public <T extends CalculusFieldElement<T>> T getRange(final FieldVector3D<T> extPoint, final Frame frame,
  333.                                                           final FieldAbsoluteDate<T> date) {

  335.         // Transform given point from given frame to topocentric frame
  336.         final FieldVector3D<T> extPointTopo = transformPoint(extPoint, frame, date);

  338.         return extPointTopo.getNorm();

  340.     }

  342.     /** Get the range rate of a point with regards to the topocentric frame center point.
  343.      * @param extPV point/velocity for which range rate shall be computed
  344.      * @param frame frame in which the point is defined
  345.      * @param date computation date
  346.      * @return range rate of the point (positive if point departs from frame)
  347.      */
  348.     public double getRangeRate(final PVCoordinates extPV, final Frame frame,
  349.                                final AbsoluteDate date) {

  351.         // Transform given point from given frame to topocentric frame
  352.         final KinematicTransform t = frame.getKinematicTransformTo(this, date);
  353.         final PVCoordinates extPVTopo = t.transformOnlyPV(extPV);

  355.         // Compute range rate (doppler) : relative rate along the line of sight
  356.         return Vector3D.dotProduct(extPVTopo.getPosition(), extPVTopo.getVelocity()) /
  357.                 extPVTopo.getPosition().getNorm();

  359.     }

  361.     /** Get the range rate of a point with regards to the topocentric frame center point.
  362.      * @param <T> type of the elements
  363.      * @param extPV point/velocity for which range rate shall be computed
  364.      * @param frame frame in which the point is defined
  365.      * @param date computation date
  366.      * @return range rate of the point (positive if point departs from frame)
  367.      * @since 9.3
  368.      */
  369.     public <T extends CalculusFieldElement<T>> T getRangeRate(final FieldPVCoordinates<T> extPV, final Frame frame,
  370.                                                               final FieldAbsoluteDate<T> date) {

  372.         // Transform given point from given frame to topocentric frame
  373.         final FieldKinematicTransform<T> t = frame.getKinematicTransformTo(this, date);
  374.         final FieldPVCoordinates<T> extPVTopo = t.transformOnlyPV(extPV);

  376.         // Compute range rate (doppler) : relative rate along the line of sight
  377.         return FieldVector3D.dotProduct(extPVTopo.getPosition(), extPVTopo.getVelocity()).divide(
  378.                 extPVTopo.getPosition().getNorm());

  380.     }

  382.     /**
  383.      * Compute the limit visibility point for a satellite in a given direction.
  384.      * <p>
  385.      * This method can be used to compute visibility circles around ground stations
  386.      * for example, using a simple loop on azimuth, with either a fixed elevation
  387.      * or an elevation that depends on azimuth to take ground masks into account.
  388.      * </p>
  389.      * @param radius satellite distance to Earth center
  390.      * @param azimuth pointing azimuth from station
  391.      * @param elevation pointing elevation from station
  392.      * @return limit visibility point for the satellite
  393.      */
  394.     public GeodeticPoint computeLimitVisibilityPoint(final double radius,
  395.                                                      final double azimuth, final double elevation) {
  396.         try {
  397.             // convergence threshold on point position: 1mm
  398.             final double deltaP = 0.001;
  399.             final UnivariateSolver solver =
  400.                     new BracketingNthOrderBrentSolver(deltaP / Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
  401.                             deltaP, deltaP, 5);

  403.             // find the distance such that a point in the specified direction and at the solved-for
  404.             // distance is exactly at the specified radius
  405.             final double distance = solver.solve(1000, new UnivariateFunction() {
  406.                 /** {@inheritDoc} */
  407.                 public double value(final double x) {
  408.                     final GeodeticPoint gp = pointAtDistance(azimuth, elevation, x);
  409.                     return parentShape.transform(gp).getNorm() - radius;
  410.                 }
  411.             }, 0, 2 * radius);

  413.             // return the limit point
  414.             return pointAtDistance(azimuth, elevation, distance);

  416.         } catch (MathRuntimeException mrte) {
  417.             throw new OrekitException(mrte);
  418.         }
  419.     }

  421.     /** Compute the point observed from the station at some specified distance.
  422.      * @param azimuth pointing azimuth from station
  423.      * @param elevation pointing elevation from station
  424.      * @param distance distance to station
  425.      * @return observed point
  426.      */
  427.     public GeodeticPoint pointAtDistance(final double azimuth, final double elevation,
  428.                                          final double distance) {
  429.         final SinCos scAz  = FastMath.sinCos(azimuth);
  430.         final SinCos scEl  = FastMath.sinCos(elevation);
  431.         final Vector3D  observed = new Vector3D(distance * scEl.cos() * scAz.sin(),
  432.                 distance * scEl.cos() * scAz.cos(),
  433.                 distance * scEl.sin());
  434.         return parentShape.transform(observed, this, AbsoluteDate.ARBITRARY_EPOCH);
  435.     }

  437.     /** {@inheritDoc} */
  438.     @Override
  439.     public Vector3D getPosition(final AbsoluteDate date, final Frame frame) {
  440.         return getStaticTransformTo(frame, date).transformPosition(Vector3D.ZERO);
  441.     }

  443.     /** {@inheritDoc} */
  444.     @Override
  445.     public <T extends CalculusFieldElement<T>> FieldVector3D<T> getPosition(final FieldAbsoluteDate<T> date,
  446.                                                                             final Frame frame) {
  447.         return getStaticTransformTo(frame, date).transformPosition(Vector3D.ZERO);
  448.     }

  450.     /** {@inheritDoc} */
  451.     @Override
  452.     public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame frame) {
  453.         return getTransformTo(frame, date).transformPVCoordinates(new TimeStampedPVCoordinates(date,
  454.                 Vector3D.ZERO,
  455.                 Vector3D.ZERO,
  456.                 Vector3D.ZERO));
  457.     }

  459.     /** {@inheritDoc} */
  460.     @Override
  461.     public <T extends CalculusFieldElement<T>> TimeStampedFieldPVCoordinates<T> getPVCoordinates(final FieldAbsoluteDate<T> date,
  462.                                                                                                  final Frame frame) {
  463.         final FieldVector3D<T> zero = FieldVector3D.getZero(date.getField());
  464.         return getTransformTo(frame, date).transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(date, zero, zero, zero));
  465.     }

  467.     /** Get the topocentric position from {@link TrackingCoordinates}.
  468.      * @param coords The coordinates that are to be converted.
  469.      * @return The topocentric coordinates.
  470.      * @since 12.1
  471.      */
  472.     public static Vector3D getTopocentricPosition(final TrackingCoordinates coords) {
  473.         return getTopocentricPosition(coords.getAzimuth(), coords.getElevation(), coords.getRange());
  474.     }

  476.     /** Get the topocentric position from {@link FieldTrackingCoordinates}.
  477.      * @param coords The coordinates that are to be converted.
  478.      * @param <T> Type of the field coordinates.
  479.      * @return The topocentric coordinates.
  480.      * @since 12.1
  481.      */
  482.     public static <T extends CalculusFieldElement<T>> FieldVector3D<T> getTopocentricPosition(final FieldTrackingCoordinates<T> coords) {
  483.         return getTopocentricPosition(coords.getAzimuth(), coords.getElevation(), coords.getRange());
  484.     }

  486.     /**
  487.      * Gets the topocentric position from a set of az/el/ra coordinates.
  488.      * @param azimuth the angle of rotation around the vertical axis, going East.
  489.      * @param elevation the elevation angle from the local horizon.
  490.      * @param range the distance from the goedetic position.
  491.      * @return the topocentric position.
  492.      * @since 12.1
  493.      */
  494.     private static Vector3D getTopocentricPosition(final double azimuth, final double elevation, final double range) {
  495.         final SinCos sinCosAz = FastMath.sinCos(azimuth);
  496.         final SinCos sinCosEL = FastMath.sinCos(elevation);
  497.         return new Vector3D(range * sinCosEL.cos() * sinCosAz.sin(), range * sinCosEL.cos() * sinCosAz.cos(), range * sinCosEL.sin());
  498.     }

  500.     /**
  501.      * Gets the topocentric position from a set of az/el/ra coordinates.
  502.      * @param azimuth the angle of rotation around the vertical axis, going East.
  503.      * @param elevation the elevation angle from the local horizon.
  504.      * @param range the distance from the geodetic position.
  505.      * @return the topocentric position.
  506.      * @param <T> the type of the az/el/ra coordinates.
  507.      * @since 12.1
  508.      */
  509.     private static <T extends CalculusFieldElement<T>> FieldVector3D<T> getTopocentricPosition(final T azimuth, final T elevation, final T range) {
  510.         final FieldSinCos<T> sinCosAz = FastMath.sinCos(azimuth);
  511.         final FieldSinCos<T> sinCosEl = FastMath.sinCos(elevation);
  512.         return new FieldVector3D<>(
  513.                 range.multiply(sinCosEl.cos()).multiply(sinCosAz.sin()),
  514.                 range.multiply(sinCosEl.cos()).multiply(sinCosAz.cos()),
  515.                 range.multiply(sinCosEl.sin())
  516.         );
  517.     }

  519.     /** Transform point in topocentric frame.
  520.      * @param extPoint point
  521.      * @param date current date
  522.      * @param frame the frame where to define the position
  523.      * @return transformed point in topocentric frame
  524.      */
  525.     private Vector3D transformPoint(final Vector3D extPoint, final Frame frame, final AbsoluteDate date) {
  526.         final StaticTransform t = frame.getStaticTransformTo(this, date);
  527.         return t.transformPosition(extPoint);
  528.     }

  530.     /** Transform point in topocentric frame.
  531.      * @param <T> type of the field elements
  532.      * @param extPoint point
  533.      * @param date current date
  534.      * @param frame the frame where to define the position
  535.      * @return transformed point in topocentric frame
  536.      */
  537.     private <T extends CalculusFieldElement<T>> FieldVector3D<T> transformPoint(final FieldVector3D<T> extPoint,
  538.                                                                                 final Frame frame,
  539.                                                                                 final FieldAbsoluteDate<T> date) {
  540.         final FieldStaticTransform<T> t = frame.getStaticTransformTo(this, date);
  541.         return t.transformPosition(extPoint);
  542.     }

  544.     /** Compute azimuth from topocentric point.
  545.      * @param extPointTopo topocentric point
  546.      * @return azimuth
  547.      */
  548.     private double computeAzimuthFromTopoPoint(final Vector3D extPointTopo) {
  549.         final double azimuth = FastMath.atan2(extPointTopo.getX(), extPointTopo.getY());
  550.         if (azimuth < 0.0) {
  551.             return azimuth + MathUtils.TWO_PI;
  552.         } else {
  553.             return azimuth;
  554.         }
  555.     }

  557.     /** Compute azimuth from topocentric point.
  558.      * @param <T> type of the field elements
  559.      * @param extPointTopo topocentric point
  560.      * @return azimuth
  561.      */
  562.     private <T extends CalculusFieldElement<T>> T computeAzimuthFromTopoPoint(final FieldVector3D<T> extPointTopo) {
  563.         final T azimuth = FastMath.atan2(extPointTopo.getX(), extPointTopo.getY());
  564.         if (azimuth.getReal() < 0.0) {
  565.             return azimuth.add(MathUtils.TWO_PI);
  566.         } else {
  567.             return azimuth;
  568.         }
  569.     }

  571. }
Created with JaCoCo 0.8.12.202403310830