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.FieldPVCoordinates;
  40. import org.orekit.utils.FieldTrackingCoordinates;
  41. import org.orekit.utils.PVCoordinates;
  42. import org.orekit.utils.PVCoordinatesProvider;
  43. import org.orekit.utils.TimeStampedPVCoordinates;
  44. import org.orekit.utils.TrackingCoordinates;


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  204.         final double azimuth = computeAzimuthFromTopoPoint(extPointTopo);

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

  208.     }

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

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

  225.         final T azimuth = computeAzimuthFromTopoPoint(extPointTopo);

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

  229.     }

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

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

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

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

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

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

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

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

  282.         return computeAzimuthFromTopoPoint(extPointTopo);

  284.     }

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

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

  303.         return computeAzimuthFromTopoPoint(extPointTopo);

  305.     }

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

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

  319.         return extPointTopo.getNorm();

  321.     }

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

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

  337.         return extPointTopo.getNorm();

  339.     }

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

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

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

  358.     }

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

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

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

  379.     }

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

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

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

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

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

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

  442.     /** Get the {@link PVCoordinates} of the topocentric frame origin in the selected frame.
  443.      * @param date current date
  444.      * @param frame the frame where to define the position
  445.      * @return position/velocity of the topocentric frame origin (m and m/s)
  446.      */
  447.     public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame frame) {
  448.         return getTransformTo(frame, date).transformPVCoordinates(new TimeStampedPVCoordinates(date,
  449.                 Vector3D.ZERO,
  450.                 Vector3D.ZERO,
  451.                 Vector3D.ZERO));
  452.     }

  454.     /** Get the topocentric position from {@link TrackingCoordinates}.
  455.      * @param coords The coordinates that are to be converted.
  456.      * @return The topocentric coordinates.
  457.      * @since 12.1
  458.      */
  459.     public static Vector3D getTopocentricPosition(final TrackingCoordinates coords) {
  460.         return getTopocentricPosition(coords.getAzimuth(), coords.getElevation(), coords.getRange());
  461.     }

  463.     /** Get the topocentric position from {@link FieldTrackingCoordinates}.
  464.      * @param coords The coordinates that are to be converted.
  465.      * @param <T> Type of the field coordinates.
  466.      * @return The topocentric coordinates.
  467.      * @since 12.1
  468.      */
  469.     public static <T extends CalculusFieldElement<T>> FieldVector3D<T> getTopocentricPosition(final FieldTrackingCoordinates<T> coords) {
  470.         return getTopocentricPosition(coords.getAzimuth(), coords.getElevation(), coords.getRange());
  471.     }

  473.     /**
  474.      * Gets the topocentric position from a set of az/el/ra coordinates.
  475.      * @param azimuth the angle of rotation around the vertical axis, going East.
  476.      * @param elevation the elevation angle from the local horizon.
  477.      * @param range the distance from the goedetic position.
  478.      * @return the topocentric position.
  479.      * @since 12.1
  480.      */
  481.     private static Vector3D getTopocentricPosition(final double azimuth, final double elevation, final double range) {
  482.         final SinCos sinCosAz = FastMath.sinCos(azimuth);
  483.         final SinCos sinCosEL = FastMath.sinCos(elevation);
  484.         return new Vector3D(range * sinCosEL.cos() * sinCosAz.sin(), range * sinCosEL.cos() * sinCosAz.cos(), range * sinCosEL.sin());
  485.     }

  487.     /**
  488.      * Gets the topocentric position from a set of az/el/ra coordinates.
  489.      * @param azimuth the angle of rotation around the vertical axis, going East.
  490.      * @param elevation the elevation angle from the local horizon.
  491.      * @param range the distance from the geodetic position.
  492.      * @return the topocentric position.
  493.      * @param <T> the type of the az/el/ra coordinates.
  494.      * @since 12.1
  495.      */
  496.     private static <T extends CalculusFieldElement<T>> FieldVector3D<T> getTopocentricPosition(final T azimuth, final T elevation, final T range) {
  497.         final FieldSinCos<T> sinCosAz = FastMath.sinCos(azimuth);
  498.         final FieldSinCos<T> sinCosEl = FastMath.sinCos(elevation);
  499.         return new FieldVector3D<>(
  500.                 range.multiply(sinCosEl.cos()).multiply(sinCosAz.sin()),
  501.                 range.multiply(sinCosEl.cos()).multiply(sinCosAz.cos()),
  502.                 range.multiply(sinCosEl.sin())
  503.         );
  504.     }

  506.     /** Transform point in topocentric frame.
  507.      * @param extPoint point
  508.      * @param date current date
  509.      * @param frame the frame where to define the position
  510.      * @return transformed point in topocentric frame
  511.      */
  512.     private Vector3D transformPoint(final Vector3D extPoint, final Frame frame, final AbsoluteDate date) {
  513.         final StaticTransform t = frame.getStaticTransformTo(this, date);
  514.         return t.transformPosition(extPoint);
  515.     }

  517.     /** Transform point in topocentric frame.
  518.      * @param <T> type of the field elements
  519.      * @param extPoint point
  520.      * @param date current date
  521.      * @param frame the frame where to define the position
  522.      * @return transformed point in topocentric frame
  523.      */
  524.     private <T extends CalculusFieldElement<T>> FieldVector3D<T> transformPoint(final FieldVector3D<T> extPoint,
  525.                                                                                 final Frame frame,
  526.                                                                                 final FieldAbsoluteDate<T> date) {
  527.         final FieldStaticTransform<T> t = frame.getStaticTransformTo(this, date);
  528.         return t.transformPosition(extPoint);
  529.     }

  531.     /** Compute azimuth from topocentric point.
  532.      * @param extPointTopo topocentric point
  533.      * @return azimuth
  534.      */
  535.     private double computeAzimuthFromTopoPoint(final Vector3D extPointTopo) {
  536.         final double azimuth = FastMath.atan2(extPointTopo.getX(), extPointTopo.getY());
  537.         if (azimuth < 0.0) {
  538.             return azimuth + MathUtils.TWO_PI;
  539.         } else {
  540.             return azimuth;
  541.         }
  542.     }

  544.     /** Compute azimuth from topocentric point.
  545.      * @param <T> type of the field elements
  546.      * @param extPointTopo topocentric point
  547.      * @return azimuth
  548.      */
  549.     private <T extends CalculusFieldElement<T>> T computeAzimuthFromTopoPoint(final FieldVector3D<T> extPointTopo) {
  550.         final T azimuth = FastMath.atan2(extPointTopo.getX(), extPointTopo.getY());
  551.         if (azimuth.getReal() < 0.0) {
  552.             return azimuth.add(MathUtils.TWO_PI);
  553.         } else {
  554.             return azimuth;
  555.         }
  556.     }

  558. }
Created with JaCoCo 0.8.12.202403310830