TopocentricFrame.java

  1. /* Copyright 2002-2019 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (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.RealFieldElement;
  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.MathUtils;
  30. import org.orekit.bodies.BodyShape;
  31. import org.orekit.bodies.FieldGeodeticPoint;
  32. import org.orekit.bodies.GeodeticPoint;
  33. import org.orekit.errors.OrekitException;
  34. import org.orekit.time.AbsoluteDate;
  35. import org.orekit.time.FieldAbsoluteDate;
  36. import org.orekit.utils.Constants;
  37. import org.orekit.utils.FieldPVCoordinates;
  38. import org.orekit.utils.PVCoordinates;
  39. import org.orekit.utils.PVCoordinatesProvider;
  40. import org.orekit.utils.TimeStampedPVCoordinates;


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

  60.     /** Serializable UID. */
  61.     private static final long serialVersionUID = -5997915708080966466L;

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

  66.     /** Point where the topocentric frame is defined. */
  67.     private final GeodeticPoint point;

  69.     /** Simple constructor.
  70.      * @param parentShape body shape on which the local point is defined
  71.      * @param point local surface point where topocentric frame is defined
  72.      * @param name the string representation
  73.      */
  74.     public TopocentricFrame(final BodyShape parentShape, final GeodeticPoint point,
  75.                             final String name) {

  77.         super(parentShape.getBodyFrame(),
  78.               new Transform(AbsoluteDate.J2000_EPOCH,
  79.                             new Transform(AbsoluteDate.J2000_EPOCH,
  80.                                           parentShape.transform(point).negate()),
  81.                             new Transform(AbsoluteDate.J2000_EPOCH,
  82.                                           new Rotation(point.getEast(), point.getZenith(),
  83.                                                        Vector3D.PLUS_I, Vector3D.PLUS_K),
  84.                                           Vector3D.ZERO)),
  85.               name, false);
  86.         this.parentShape = parentShape;
  87.         this.point = point;
  88.     }

  90.     /** Get the body shape on which the local point is defined.
  91.      * @return body shape on which the local point is defined
  92.      */
  93.     public BodyShape getParentShape() {
  94.         return parentShape;
  95.     }

  97.     /** Get the surface point defining the origin of the frame.
  98.      * @return surface point defining the origin of the frame
  99.      */
  100.     public GeodeticPoint getPoint() {
  101.         return point;
  102.     }

  104.     /** Get the surface point defining the origin of the frame.
  105.      * @param <T> tyoe of the elements
  106.      * @param field of the elements
  107.      * @return surface point defining the origin of the frame
  108.      * @since 9.3
  109.      */
  110.     public <T extends RealFieldElement<T>> FieldGeodeticPoint<T> getPoint(final Field<T> field) {
  111.         final T zero = field.getZero();
  112.         return new FieldGeodeticPoint<>(zero.add(point.getLatitude()),
  113.                                         zero.add(point.getLongitude()),
  114.                                         zero.add(point.getAltitude()));
  115.     }

  117.     /** Get the zenith direction of topocentric frame, expressed in parent shape frame.
  118.      * <p>The zenith direction is defined as the normal to local horizontal plane.</p>
  119.      * @return unit vector in the zenith direction
  120.      * @see #getNadir()
  121.      */
  122.     public Vector3D getZenith() {
  123.         return point.getZenith();
  124.     }

  126.     /** Get the nadir direction of topocentric frame, expressed in parent shape frame.
  127.      * <p>The nadir direction is the opposite of zenith direction.</p>
  128.      * @return unit vector in the nadir direction
  129.      * @see #getZenith()
  130.      */
  131.     public Vector3D getNadir() {
  132.         return point.getNadir();
  133.     }

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

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

  154.     /** Get the east direction of topocentric frame, expressed in parent shape frame.
  155.      * <p>The east direction is defined in the horizontal plane
  156.      * in order to complete direct triangle (east, north, zenith).</p>
  157.      * @return unit vector in the east direction
  158.      * @see #getWest()
  159.      */
  160.     public Vector3D getEast() {
  161.         return point.getEast();
  162.     }

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

  173.     /** Get the elevation of a point with regards to the local point.
  174.      * <p>The elevation is the angle between the local horizontal and
  175.      * the direction from local point to given point.</p>
  176.      * @param extPoint point for which elevation shall be computed
  177.      * @param frame frame in which the point is defined
  178.      * @param date computation date
  179.      * @return elevation of the point
  180.      */
  181.     public double getElevation(final Vector3D extPoint, final Frame frame,
  182.                                final AbsoluteDate date) {

  184.         // Transform given point from given frame to topocentric frame
  185.         final Transform t = frame.getTransformTo(this, date);
  186.         final Vector3D extPointTopo = t.transformPosition(extPoint);

  188.         // Elevation angle is PI/2 - angle between zenith and given point direction
  189.         return extPointTopo.getDelta();
  190.     }

  192.     /** Get the elevation of a point with regards to the local point.
  193.      * <p>The elevation is the angle between the local horizontal and
  194.      * the direction from local point to given point.</p>
  195.      * @param <T> type of the elements
  196.      * @param extPoint point for which elevation shall be computed
  197.      * @param frame frame in which the point is defined
  198.      * @param date computation date
  199.      * @return elevation of the point
  200.      * @since 9.3
  201.      */
  202.     public <T extends RealFieldElement<T>> T getElevation(final FieldVector3D<T> extPoint, final Frame frame,
  203.                                                           final FieldAbsoluteDate<T> date) {

  205.         // Transform given point from given frame to topocentric frame
  206.         final FieldTransform<T> t = frame.getTransformTo(this, date);
  207.         final FieldVector3D<T> extPointTopo = t.transformPosition(extPoint);

  209.         // Elevation angle is PI/2 - angle between zenith and given point direction
  210.         return extPointTopo.getDelta();
  211.     }

  213.     /** Get the azimuth of a point with regards to the topocentric frame center point.
  214.      * <p>The azimuth is the angle between the North direction at local point and
  215.      * the projection in local horizontal plane of the direction from local point
  216.      * to given point. Azimuth angles are counted clockwise, i.e positive towards the East.</p>
  217.      * @param extPoint point for which elevation shall be computed
  218.      * @param frame frame in which the point is defined
  219.      * @param date computation date
  220.      * @return azimuth of the point
  221.      */
  222.     public double getAzimuth(final Vector3D extPoint, final Frame frame,
  223.                              final AbsoluteDate date) {

  225.         // Transform given point from given frame to topocentric frame
  226.         final Transform t = getTransformTo(frame, date).getInverse();
  227.         final Vector3D extPointTopo = t.transformPosition(extPoint);

  229.         // Compute azimuth
  230.         double azimuth = FastMath.atan2(extPointTopo.getX(), extPointTopo.getY());
  231.         if (azimuth < 0.) {
  232.             azimuth += MathUtils.TWO_PI;
  233.         }
  234.         return azimuth;

  236.     }

  238.     /** Get the azimuth of a point with regards to the topocentric frame center point.
  239.      * <p>The azimuth is the angle between the North direction at local point and
  240.      * the projection in local horizontal plane of the direction from local point
  241.      * to given point. Azimuth angles are counted clockwise, i.e positive towards the East.</p>
  242.      * @param <T> type of the elements
  243.      * @param extPoint point for which elevation shall be computed
  244.      * @param frame frame in which the point is defined
  245.      * @param date computation date
  246.      * @return azimuth of the point
  247.      * @since 9.3
  248.      */
  249.     public <T extends RealFieldElement<T>> T getAzimuth(final FieldVector3D<T> extPoint, final Frame frame,
  250.                                                         final FieldAbsoluteDate<T> date) {

  252.         // Transform given point from given frame to topocentric frame
  253.         final FieldTransform<T> t = getTransformTo(frame, date).getInverse();
  254.         final FieldVector3D<T> extPointTopo = t.transformPosition(extPoint);

  256.         // Compute azimuth
  257.         T azimuth = FastMath.atan2(extPointTopo.getX(), extPointTopo.getY());
  258.         if (azimuth.getReal() < 0.) {
  259.             azimuth = azimuth.add(MathUtils.TWO_PI);
  260.         }
  261.         return azimuth;

  263.     }

  265.     /** Get the range of a point with regards to the topocentric frame center point.
  266.      * @param extPoint point for which range shall be computed
  267.      * @param frame frame in which the point is defined
  268.      * @param date computation date
  269.      * @return range (distance) of the point
  270.      */
  271.     public double getRange(final Vector3D extPoint, final Frame frame,
  272.                            final AbsoluteDate date) {

  274.         // Transform given point from given frame to topocentric frame
  275.         final Transform t = frame.getTransformTo(this, date);
  276.         final Vector3D extPointTopo = t.transformPosition(extPoint);

  278.         // Compute range
  279.         return extPointTopo.getNorm();

  281.     }

  283.     /** Get the range of a point with regards to the topocentric frame center point.
  284.      * @param <T> type of the elements
  285.      * @param extPoint point for which range shall be computed
  286.      * @param frame frame in which the point is defined
  287.      * @param date computation date
  288.      * @return range (distance) of the point
  289.      * @since 9.3
  290.      */
  291.     public <T extends RealFieldElement<T>> T getRange(final FieldVector3D<T> extPoint, final Frame frame,
  292.                                                       final FieldAbsoluteDate<T> date) {

  294.         // Transform given point from given frame to topocentric frame
  295.         final FieldTransform<T> t = frame.getTransformTo(this, date);
  296.         final FieldVector3D<T> extPointTopo = t.transformPosition(extPoint);

  298.         // Compute range
  299.         return extPointTopo.getNorm();

  301.     }

  303.     /** Get the range rate of a point with regards to the topocentric frame center point.
  304.      * @param extPV point/velocity for which range rate shall be computed
  305.      * @param frame frame in which the point is defined
  306.      * @param date computation date
  307.      * @return range rate of the point (positive if point departs from frame)
  308.      */
  309.     public double getRangeRate(final PVCoordinates extPV, final Frame frame,
  310.                                final AbsoluteDate date) {

  312.         // Transform given point from given frame to topocentric frame
  313.         final Transform t = frame.getTransformTo(this, date);
  314.         final PVCoordinates extPVTopo = t.transformPVCoordinates(extPV);

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

  320.     }

  322.     /** Get the range rate of a point with regards to the topocentric frame center point.
  323.      * @param <T> type of the elements
  324.      * @param extPV point/velocity for which range rate shall be computed
  325.      * @param frame frame in which the point is defined
  326.      * @param date computation date
  327.      * @return range rate of the point (positive if point departs from frame)
  328.      * @since 9.3
  329.      */
  330.     public <T extends RealFieldElement<T>> T getRangeRate(final FieldPVCoordinates<T> extPV, final Frame frame,
  331.                                                           final FieldAbsoluteDate<T> date) {

  333.         // Transform given point from given frame to topocentric frame
  334.         final FieldTransform<T> t = frame.getTransformTo(this, date);
  335.         final FieldPVCoordinates<T> extPVTopo = t.transformPVCoordinates(extPV);

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

  341.     }

  343.     /**
  344.      * Compute the limit visibility point for a satellite in a given direction.
  345.      * <p>
  346.      * This method can be used to compute visibility circles around ground stations
  347.      * for example, using a simple loop on azimuth, with either a fixed elevation
  348.      * or an elevation that depends on azimuth to take ground masks into account.
  349.      * </p>
  350.      * @param radius satellite distance to Earth center
  351.      * @param azimuth pointing azimuth from station
  352.      * @param elevation pointing elevation from station
  353.      * @return limit visibility point for the satellite
  354.      */
  355.     public GeodeticPoint computeLimitVisibilityPoint(final double radius,
  356.                                                      final double azimuth, final double elevation) {
  357.         try {
  358.             // convergence threshold on point position: 1mm
  359.             final double deltaP = 0.001;
  360.             final UnivariateSolver solver =
  361.                     new BracketingNthOrderBrentSolver(deltaP / Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
  362.                                                       deltaP, deltaP, 5);

  364.             // find the distance such that a point in the specified direction and at the solved-for
  365.             // distance is exactly at the specified radius
  366.             final double distance = solver.solve(1000, new UnivariateFunction() {
  367.                 /** {@inheritDoc} */
  368.                 public double value(final double x) {
  369.                     final GeodeticPoint gp = pointAtDistance(azimuth, elevation, x);
  370.                     return parentShape.transform(gp).getNorm() - radius;
  371.                 }
  372.             }, 0, 2 * radius);

  374.             // return the limit point
  375.             return pointAtDistance(azimuth, elevation, distance);

  377.         } catch (MathRuntimeException mrte) {
  378.             throw new OrekitException(mrte);
  379.         }
  380.     }

  382.     /** Compute the point observed from the station at some specified distance.
  383.      * @param azimuth pointing azimuth from station
  384.      * @param elevation pointing elevation from station
  385.      * @param distance distance to station
  386.      * @return observed point
  387.      */
  388.     public GeodeticPoint pointAtDistance(final double azimuth, final double elevation,
  389.                                          final double distance) {
  390.         final double cosAz = FastMath.cos(azimuth);
  391.         final double sinAz = FastMath.sin(azimuth);
  392.         final double cosEl = FastMath.cos(elevation);
  393.         final double sinEl = FastMath.sin(elevation);
  394.         final Vector3D  observed = new Vector3D(distance * cosEl * sinAz,
  395.                                                 distance * cosEl * cosAz,
  396.                                                 distance * sinEl);
  397.         return parentShape.transform(observed, this, AbsoluteDate.J2000_EPOCH);
  398.     }

  400.     /** Get the {@link PVCoordinates} of the topocentric frame origin in the selected frame.
  401.      * @param date current date
  402.      * @param frame the frame where to define the position
  403.      * @return position/velocity of the topocentric frame origin (m and m/s)
  404.      */
  405.     public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame frame) {
  406.         return getTransformTo(frame, date).transformPVCoordinates(new TimeStampedPVCoordinates(date,
  407.                                                                                                Vector3D.ZERO,
  408.                                                                                                Vector3D.ZERO,
  409.                                                                                                Vector3D.ZERO));
  410.     }

  412. }
Created with JaCoCo 0.8.2.201808211720