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;
18  
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;
41  
42  
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 {
59  
60      /** Serializable UID. */
61      private static final long serialVersionUID = -5997915708080966466L;
62  
63      /** Body shape on which the local point is defined. */
64      private final BodyShape parentShape;
65  
66      /** Point where the topocentric frame is defined. */
67      private final GeodeticPoint point;
68  
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) {
76  
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      }
89  
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      }
96  
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     }
103 
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     }
116 
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     }
125 
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     }
134 
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     }
144 
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     }
153 
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     }
163 
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     }
172 
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) {
183 
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);
187 
188         // Elevation angle is PI/2 - angle between zenith and given point direction
189         return extPointTopo.getDelta();
190     }
191 
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) {
204 
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);
208 
209         // Elevation angle is PI/2 - angle between zenith and given point direction
210         return extPointTopo.getDelta();
211     }
212 
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) {
224 
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);
228 
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;
235 
236     }
237 
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) {
251 
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);
255 
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;
262 
263     }
264 
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) {
273 
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);
277 
278         // Compute range
279         return extPointTopo.getNorm();
280 
281     }
282 
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) {
293 
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);
297 
298         // Compute range
299         return extPointTopo.getNorm();
300 
301     }
302 
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) {
311 
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);
315 
316         // Compute range rate (doppler) : relative rate along the line of sight
317         return Vector3D.dotProduct(extPVTopo.getPosition(), extPVTopo.getVelocity()) /
318                extPVTopo.getPosition().getNorm();
319 
320     }
321 
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) {
332 
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);
336 
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());
340 
341     }
342 
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);
363 
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);
373 
374             // return the limit point
375             return pointAtDistance(azimuth, elevation, distance);
376 
377         } catch (MathRuntimeException mrte) {
378             throw new OrekitException(mrte);
379         }
380     }
381 
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     }
399 
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     }
411 
412 }