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é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 }