1 /* Copyright 2002-2021 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;
18
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.MathUtils;
30 import org.hipparchus.util.SinCos;
31 import org.orekit.bodies.BodyShape;
32 import org.orekit.bodies.FieldGeodeticPoint;
33 import org.orekit.bodies.GeodeticPoint;
34 import org.orekit.errors.OrekitException;
35 import org.orekit.time.AbsoluteDate;
36 import org.orekit.time.FieldAbsoluteDate;
37 import org.orekit.utils.Constants;
38 import org.orekit.utils.FieldPVCoordinates;
39 import org.orekit.utils.PVCoordinates;
40 import org.orekit.utils.PVCoordinatesProvider;
41 import org.orekit.utils.TimeStampedPVCoordinates;
42
43
44 /** Topocentric frame.
45 * <p>Frame associated to a position near the surface of a body shape.</p>
46 * <p>
47 * The origin of the frame is at the defining {@link GeodeticPoint geodetic point}
48 * location, and the right-handed canonical trihedra is:
49 * </p>
50 * <ul>
51 * <li>X axis in the local horizontal plane (normal to zenith direction) and
52 * following the local parallel towards East</li>
53 * <li>Y axis in the horizontal plane (normal to zenith direction) and
54 * following the local meridian towards North</li>
55 * <li>Z axis towards Zenith direction</li>
56 * </ul>
57 * @author Véronique Pommier-Maurussane
58 */
59 public class TopocentricFrame extends Frame implements PVCoordinatesProvider {
60
61 /** Serializable UID. */
62 private static final long serialVersionUID = -5997915708080966466L;
63
64 /** Body shape on which the local point is defined. */
65 private final BodyShape parentShape;
66
67 /** Point where the topocentric frame is defined. */
68 private final GeodeticPoint point;
69
70 /** Simple constructor.
71 * @param parentShape body shape on which the local point is defined
72 * @param point local surface point where topocentric frame is defined
73 * @param name the string representation
74 */
75 public TopocentricFrame(final BodyShape parentShape, final GeodeticPoint point,
76 final String name) {
77
78 super(parentShape.getBodyFrame(),
79 new Transform(AbsoluteDate.ARBITRARY_EPOCH,
80 new Transform(AbsoluteDate.ARBITRARY_EPOCH,
81 parentShape.transform(point).negate()),
82 new Transform(AbsoluteDate.ARBITRARY_EPOCH,
83 new Rotation(point.getEast(), point.getZenith(),
84 Vector3D.PLUS_I, Vector3D.PLUS_K),
85 Vector3D.ZERO)),
86 name, false);
87 this.parentShape = parentShape;
88 this.point = point;
89 }
90
91 /** Get the body shape on which the local point is defined.
92 * @return body shape on which the local point is defined
93 */
94 public BodyShape getParentShape() {
95 return parentShape;
96 }
97
98 /** Get the surface point defining the origin of the frame.
99 * @return surface point defining the origin of the frame
100 */
101 public GeodeticPoint getPoint() {
102 return point;
103 }
104
105 /** Get the surface point defining the origin of the frame.
106 * @param <T> tyoe of the elements
107 * @param field of the elements
108 * @return surface point defining the origin of the frame
109 * @since 9.3
110 */
111 public <T extends CalculusFieldElement<T>> FieldGeodeticPoint<T> getPoint(final Field<T> field) {
112 final T zero = field.getZero();
113 return new FieldGeodeticPoint<>(zero.add(point.getLatitude()),
114 zero.add(point.getLongitude()),
115 zero.add(point.getAltitude()));
116 }
117
118 /** Get the zenith direction of topocentric frame, expressed in parent shape frame.
119 * <p>The zenith direction is defined as the normal to local horizontal plane.</p>
120 * @return unit vector in the zenith direction
121 * @see #getNadir()
122 */
123 public Vector3D getZenith() {
124 return point.getZenith();
125 }
126
127 /** Get the nadir direction of topocentric frame, expressed in parent shape frame.
128 * <p>The nadir direction is the opposite of zenith direction.</p>
129 * @return unit vector in the nadir direction
130 * @see #getZenith()
131 */
132 public Vector3D getNadir() {
133 return point.getNadir();
134 }
135
136 /** Get the north direction of topocentric frame, expressed in parent shape frame.
137 * <p>The north direction is defined in the horizontal plane
138 * (normal to zenith direction) and following the local meridian.</p>
139 * @return unit vector in the north direction
140 * @see #getSouth()
141 */
142 public Vector3D getNorth() {
143 return point.getNorth();
144 }
145
146 /** Get the south direction of topocentric frame, expressed in parent shape frame.
147 * <p>The south direction is the opposite of north direction.</p>
148 * @return unit vector in the south direction
149 * @see #getNorth()
150 */
151 public Vector3D getSouth() {
152 return point.getSouth();
153 }
154
155 /** Get the east direction of topocentric frame, expressed in parent shape frame.
156 * <p>The east direction is defined in the horizontal plane
157 * in order to complete direct triangle (east, north, zenith).</p>
158 * @return unit vector in the east direction
159 * @see #getWest()
160 */
161 public Vector3D getEast() {
162 return point.getEast();
163 }
164
165 /** Get the west direction of topocentric frame, expressed in parent shape frame.
166 * <p>The west direction is the opposite of east direction.</p>
167 * @return unit vector in the west direction
168 * @see #getEast()
169 */
170 public Vector3D getWest() {
171 return point.getWest();
172 }
173
174 /** Get the elevation of a point with regards to the local point.
175 * <p>The elevation is the angle between the local horizontal and
176 * the direction from local point to given point.</p>
177 * @param extPoint point for which elevation shall be computed
178 * @param frame frame in which the point is defined
179 * @param date computation date
180 * @return elevation of the point
181 */
182 public double getElevation(final Vector3D extPoint, final Frame frame,
183 final AbsoluteDate date) {
184
185 // Transform given point from given frame to topocentric frame
186 final Transform t = frame.getTransformTo(this, date);
187 final Vector3D extPointTopo = t.transformPosition(extPoint);
188
189 // Elevation angle is PI/2 - angle between zenith and given point direction
190 return extPointTopo.getDelta();
191 }
192
193 /** Get the elevation of a point with regards to the local point.
194 * <p>The elevation is the angle between the local horizontal and
195 * the direction from local point to given point.</p>
196 * @param <T> type of the elements
197 * @param extPoint point for which elevation shall be computed
198 * @param frame frame in which the point is defined
199 * @param date computation date
200 * @return elevation of the point
201 * @since 9.3
202 */
203 public <T extends CalculusFieldElement<T>> T getElevation(final FieldVector3D<T> extPoint, final Frame frame,
204 final FieldAbsoluteDate<T> date) {
205
206 // Transform given point from given frame to topocentric frame
207 final FieldTransform<T> t = frame.getTransformTo(this, date);
208 final FieldVector3D<T> extPointTopo = t.transformPosition(extPoint);
209
210 // Elevation angle is PI/2 - angle between zenith and given point direction
211 return extPointTopo.getDelta();
212 }
213
214 /** Get the azimuth of a point with regards to the topocentric frame center point.
215 * <p>The azimuth is the angle between the North direction at local point and
216 * the projection in local horizontal plane of the direction from local point
217 * to given point. Azimuth angles are counted clockwise, i.e positive towards the East.</p>
218 * @param extPoint point for which elevation shall be computed
219 * @param frame frame in which the point is defined
220 * @param date computation date
221 * @return azimuth of the point
222 */
223 public double getAzimuth(final Vector3D extPoint, final Frame frame,
224 final AbsoluteDate date) {
225
226 // Transform given point from given frame to topocentric frame
227 final Transform t = getTransformTo(frame, date).getInverse();
228 final Vector3D extPointTopo = t.transformPosition(extPoint);
229
230 // Compute azimuth
231 double azimuth = FastMath.atan2(extPointTopo.getX(), extPointTopo.getY());
232 if (azimuth < 0.) {
233 azimuth += MathUtils.TWO_PI;
234 }
235 return azimuth;
236
237 }
238
239 /** Get the azimuth of a point with regards to the topocentric frame center point.
240 * <p>The azimuth is the angle between the North direction at local point and
241 * the projection in local horizontal plane of the direction from local point
242 * to given point. Azimuth angles are counted clockwise, i.e positive towards the East.</p>
243 * @param <T> type of the elements
244 * @param extPoint point for which elevation shall be computed
245 * @param frame frame in which the point is defined
246 * @param date computation date
247 * @return azimuth of the point
248 * @since 9.3
249 */
250 public <T extends CalculusFieldElement<T>> T getAzimuth(final FieldVector3D<T> extPoint, final Frame frame,
251 final FieldAbsoluteDate<T> date) {
252
253 // Transform given point from given frame to topocentric frame
254 final FieldTransform<T> t = getTransformTo(frame, date).getInverse();
255 final FieldVector3D<T> extPointTopo = t.transformPosition(extPoint);
256
257 // Compute azimuth
258 T azimuth = FastMath.atan2(extPointTopo.getX(), extPointTopo.getY());
259 if (azimuth.getReal() < 0.) {
260 azimuth = azimuth.add(MathUtils.TWO_PI);
261 }
262 return azimuth;
263
264 }
265
266 /** Get the range of a point with regards to the topocentric frame center point.
267 * @param extPoint point for which range shall be computed
268 * @param frame frame in which the point is defined
269 * @param date computation date
270 * @return range (distance) of the point
271 */
272 public double getRange(final Vector3D extPoint, final Frame frame,
273 final AbsoluteDate date) {
274
275 // Transform given point from given frame to topocentric frame
276 final Transform t = frame.getTransformTo(this, date);
277 final Vector3D extPointTopo = t.transformPosition(extPoint);
278
279 // Compute range
280 return extPointTopo.getNorm();
281
282 }
283
284 /** Get the range of a point with regards to the topocentric frame center point.
285 * @param <T> type of the elements
286 * @param extPoint point for which range shall be computed
287 * @param frame frame in which the point is defined
288 * @param date computation date
289 * @return range (distance) of the point
290 * @since 9.3
291 */
292 public <T extends CalculusFieldElement<T>> T getRange(final FieldVector3D<T> extPoint, final Frame frame,
293 final FieldAbsoluteDate<T> date) {
294
295 // Transform given point from given frame to topocentric frame
296 final FieldTransform<T> t = frame.getTransformTo(this, date);
297 final FieldVector3D<T> extPointTopo = t.transformPosition(extPoint);
298
299 // Compute range
300 return extPointTopo.getNorm();
301
302 }
303
304 /** Get the range rate of a point with regards to the topocentric frame center point.
305 * @param extPV point/velocity for which range rate shall be computed
306 * @param frame frame in which the point is defined
307 * @param date computation date
308 * @return range rate of the point (positive if point departs from frame)
309 */
310 public double getRangeRate(final PVCoordinates extPV, final Frame frame,
311 final AbsoluteDate date) {
312
313 // Transform given point from given frame to topocentric frame
314 final Transform t = frame.getTransformTo(this, date);
315 final PVCoordinates extPVTopo = t.transformPVCoordinates(extPV);
316
317 // Compute range rate (doppler) : relative rate along the line of sight
318 return Vector3D.dotProduct(extPVTopo.getPosition(), extPVTopo.getVelocity()) /
319 extPVTopo.getPosition().getNorm();
320
321 }
322
323 /** Get the range rate of a point with regards to the topocentric frame center point.
324 * @param <T> type of the elements
325 * @param extPV point/velocity for which range rate shall be computed
326 * @param frame frame in which the point is defined
327 * @param date computation date
328 * @return range rate of the point (positive if point departs from frame)
329 * @since 9.3
330 */
331 public <T extends CalculusFieldElement<T>> T getRangeRate(final FieldPVCoordinates<T> extPV, final Frame frame,
332 final FieldAbsoluteDate<T> date) {
333
334 // Transform given point from given frame to topocentric frame
335 final FieldTransform<T> t = frame.getTransformTo(this, date);
336 final FieldPVCoordinates<T> extPVTopo = t.transformPVCoordinates(extPV);
337
338 // Compute range rate (doppler) : relative rate along the line of sight
339 return FieldVector3D.dotProduct(extPVTopo.getPosition(), extPVTopo.getVelocity()).divide(
340 extPVTopo.getPosition().getNorm());
341
342 }
343
344 /**
345 * Compute the limit visibility point for a satellite in a given direction.
346 * <p>
347 * This method can be used to compute visibility circles around ground stations
348 * for example, using a simple loop on azimuth, with either a fixed elevation
349 * or an elevation that depends on azimuth to take ground masks into account.
350 * </p>
351 * @param radius satellite distance to Earth center
352 * @param azimuth pointing azimuth from station
353 * @param elevation pointing elevation from station
354 * @return limit visibility point for the satellite
355 */
356 public GeodeticPoint computeLimitVisibilityPoint(final double radius,
357 final double azimuth, final double elevation) {
358 try {
359 // convergence threshold on point position: 1mm
360 final double deltaP = 0.001;
361 final UnivariateSolver solver =
362 new BracketingNthOrderBrentSolver(deltaP / Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
363 deltaP, deltaP, 5);
364
365 // find the distance such that a point in the specified direction and at the solved-for
366 // distance is exactly at the specified radius
367 final double distance = solver.solve(1000, new UnivariateFunction() {
368 /** {@inheritDoc} */
369 public double value(final double x) {
370 final GeodeticPoint gp = pointAtDistance(azimuth, elevation, x);
371 return parentShape.transform(gp).getNorm() - radius;
372 }
373 }, 0, 2 * radius);
374
375 // return the limit point
376 return pointAtDistance(azimuth, elevation, distance);
377
378 } catch (MathRuntimeException mrte) {
379 throw new OrekitException(mrte);
380 }
381 }
382
383 /** Compute the point observed from the station at some specified distance.
384 * @param azimuth pointing azimuth from station
385 * @param elevation pointing elevation from station
386 * @param distance distance to station
387 * @return observed point
388 */
389 public GeodeticPoint pointAtDistance(final double azimuth, final double elevation,
390 final double distance) {
391 final SinCos scAz = FastMath.sinCos(azimuth);
392 final SinCos scEl = FastMath.sinCos(elevation);
393 final Vector3D observed = new Vector3D(distance * scEl.cos() * scAz.sin(),
394 distance * scEl.cos() * scAz.cos(),
395 distance * scEl.sin());
396 return parentShape.transform(observed, this, AbsoluteDate.ARBITRARY_EPOCH);
397 }
398
399 /** Get the {@link PVCoordinates} of the topocentric frame origin in the selected frame.
400 * @param date current date
401 * @param frame the frame where to define the position
402 * @return position/velocity of the topocentric frame origin (m and m/s)
403 */
404 public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame frame) {
405 return getTransformTo(frame, date).transformPVCoordinates(new TimeStampedPVCoordinates(date,
406 Vector3D.ZERO,
407 Vector3D.ZERO,
408 Vector3D.ZERO));
409 }
410
411 }