1 /* Copyright 2002-2024 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.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;
45
46
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éronique Pommier-Maurussane
61 */
62 public class TopocentricFrame extends Frame implements PVCoordinatesProvider {
63
64 /** Serializable UID. */
65 private static final long serialVersionUID = -5997915708080966466L;
66
67 /** Body shape on which the local point is defined. */
68 private final BodyShape parentShape;
69
70 /** Geodetic point where the topocentric frame is defined. */
71 private final GeodeticPoint point;
72
73 /** Cartesian point where the topocentric frame is defined.
74 * @since 12.0
75 */
76 private final Vector3D cartesianPoint;
77
78 /** Simple constructor.
79 * @param parentShape body shape on which the local point is defined
80 * @param point local surface point where topocentric frame is defined
81 * @param name the string representation
82 */
83 public TopocentricFrame(final BodyShape parentShape, final GeodeticPoint point,
84 final String name) {
85
86 super(parentShape.getBodyFrame(),
87 new Transform(AbsoluteDate.ARBITRARY_EPOCH,
88 new Transform(AbsoluteDate.ARBITRARY_EPOCH,
89 parentShape.transform(point).negate()),
90 new Transform(AbsoluteDate.ARBITRARY_EPOCH,
91 new Rotation(point.getEast(), point.getZenith(),
92 Vector3D.PLUS_I, Vector3D.PLUS_K),
93 Vector3D.ZERO)),
94 name, false);
95 this.parentShape = parentShape;
96 this.point = point;
97 this.cartesianPoint = getTransformProvider().
98 getStaticTransform(AbsoluteDate.ARBITRARY_EPOCH).
99 getInverse().
100 transformPosition(Vector3D.ZERO);
101 }
102
103 /** Get the body shape on which the local point is defined.
104 * @return body shape on which the local point is defined
105 */
106 public BodyShape getParentShape() {
107 return parentShape;
108 }
109
110 /** Get the surface point defining the origin of the frame.
111 * @return surface point defining the origin of the frame
112 */
113 public GeodeticPoint getPoint() {
114 return point;
115 }
116
117 /** Get the surface point defining the origin of the frame.
118 * @return surface point defining the origin of the frame in body frame
119 * @since 12.0
120 */
121 public Vector3D getCartesianPoint() {
122 return cartesianPoint;
123 }
124
125 /** Get the surface point defining the origin of the frame.
126 * @param <T> type of the elements
127 * @param field of the elements
128 * @return surface point defining the origin of the frame
129 * @since 9.3
130 */
131 public <T extends CalculusFieldElement<T>> FieldGeodeticPoint<T> getPoint(final Field<T> field) {
132 final T zero = field.getZero();
133 return new FieldGeodeticPoint<>(zero.newInstance(point.getLatitude()),
134 zero.newInstance(point.getLongitude()),
135 zero.newInstance(point.getAltitude()));
136 }
137
138 /** Get the zenith direction of topocentric frame, expressed in parent shape frame.
139 * <p>The zenith direction is defined as the normal to local horizontal plane.</p>
140 * @return unit vector in the zenith direction
141 * @see #getNadir()
142 */
143 public Vector3D getZenith() {
144 return point.getZenith();
145 }
146
147 /** Get the nadir direction of topocentric frame, expressed in parent shape frame.
148 * <p>The nadir direction is the opposite of zenith direction.</p>
149 * @return unit vector in the nadir direction
150 * @see #getZenith()
151 */
152 public Vector3D getNadir() {
153 return point.getNadir();
154 }
155
156 /** Get the north direction of topocentric frame, expressed in parent shape frame.
157 * <p>The north direction is defined in the horizontal plane
158 * (normal to zenith direction) and following the local meridian.</p>
159 * @return unit vector in the north direction
160 * @see #getSouth()
161 */
162 public Vector3D getNorth() {
163 return point.getNorth();
164 }
165
166 /** Get the south direction of topocentric frame, expressed in parent shape frame.
167 * <p>The south direction is the opposite of north direction.</p>
168 * @return unit vector in the south direction
169 * @see #getNorth()
170 */
171 public Vector3D getSouth() {
172 return point.getSouth();
173 }
174
175 /** Get the east direction of topocentric frame, expressed in parent shape frame.
176 * <p>The east direction is defined in the horizontal plane
177 * in order to complete direct triangle (east, north, zenith).</p>
178 * @return unit vector in the east direction
179 * @see #getWest()
180 */
181 public Vector3D getEast() {
182 return point.getEast();
183 }
184
185 /** Get the west direction of topocentric frame, expressed in parent shape frame.
186 * <p>The west direction is the opposite of east direction.</p>
187 * @return unit vector in the west direction
188 * @see #getEast()
189 */
190 public Vector3D getWest() {
191 return point.getWest();
192 }
193
194 /** Get the tracking coordinates of a point with regards to the local point.
195 * @param extPoint point for which elevation shall be computed
196 * @param frame frame in which the point is defined
197 * @param date computation date
198 * @return tracking coordinates of the point
199 * @since 12.0
200 */
201 public TrackingCoordinates getTrackingCoordinates(final Vector3D extPoint, final Frame frame,
202 final AbsoluteDate date) {
203
204 // transform given point from given frame to topocentric frame
205 final Vector3D extPointTopo = transformPoint(extPoint, frame, date);
206
207 final double azimuth = computeAzimuthFromTopoPoint(extPointTopo);
208
209 return new TrackingCoordinates(azimuth, extPointTopo.getDelta(), extPointTopo.getNorm());
210
211 }
212
213 /** Get the tracking coordinates of a point with regards to the local point.
214 * @param <T> type of the field elements
215 * @param extPoint point for which elevation shall be computed
216 * @param frame frame in which the point is defined
217 * @param date computation date
218 * @return tracking coordinates of the point
219 * @since 12.0
220 */
221 public <T extends CalculusFieldElement<T>> FieldTrackingCoordinates<T> getTrackingCoordinates(final FieldVector3D<T> extPoint,
222 final Frame frame,
223 final FieldAbsoluteDate<T> date) {
224
225 // Transform given point from given frame to topocentric frame
226 final FieldVector3D<T> extPointTopo = transformPoint(extPoint, frame, date);
227
228 final T azimuth = computeAzimuthFromTopoPoint(extPointTopo);
229
230 return new FieldTrackingCoordinates<>(azimuth, extPointTopo.getDelta(), extPointTopo.getNorm());
231
232 }
233
234 /** Get the elevation of a point with regards to the local point.
235 * <p>The elevation is the angle between the local horizontal and
236 * the direction from local point to given point.</p>
237 * @param extPoint point for which elevation shall be computed
238 * @param frame frame in which the point is defined
239 * @param date computation date
240 * @return elevation of the point
241 */
242 public double getElevation(final Vector3D extPoint, final Frame frame,
243 final AbsoluteDate date) {
244
245 // Transform given point from given frame to topocentric frame
246 final Vector3D extPointTopo = transformPoint(extPoint, frame, date);
247
248 return extPointTopo.getDelta();
249 }
250
251 /** Get the elevation of a point with regards to the local point.
252 * <p>The elevation is the angle between the local horizontal and
253 * the direction from local point to given point.</p>
254 * @param <T> type of the elements
255 * @param extPoint point for which elevation shall be computed
256 * @param frame frame in which the point is defined
257 * @param date computation date
258 * @return elevation of the point
259 * @since 9.3
260 */
261 public <T extends CalculusFieldElement<T>> T getElevation(final FieldVector3D<T> extPoint, final Frame frame,
262 final FieldAbsoluteDate<T> date) {
263
264 // Transform given point from given frame to topocentric frame
265 final FieldVector3D<T> extPointTopo = transformPoint(extPoint, frame, date);
266
267 return extPointTopo.getDelta();
268 }
269
270 /** Get the azimuth of a point with regards to the topocentric frame center point.
271 * <p>The azimuth is the angle between the North direction at local point and
272 * the projection in local horizontal plane of the direction from local point
273 * to given point. Azimuth angles are counted clockwise, i.e positive towards the East.</p>
274 * @param extPoint point for which elevation shall be computed
275 * @param frame frame in which the point is defined
276 * @param date computation date
277 * @return azimuth of the point
278 */
279 public double getAzimuth(final Vector3D extPoint, final Frame frame,
280 final AbsoluteDate date) {
281
282 // Transform given point from given frame to topocentric frame
283 final Vector3D extPointTopo = transformPoint(extPoint, frame, date);
284
285 return computeAzimuthFromTopoPoint(extPointTopo);
286
287 }
288
289 /** Get the azimuth of a point with regards to the topocentric frame center point.
290 * <p>The azimuth is the angle between the North direction at local point and
291 * the projection in local horizontal plane of the direction from local point
292 * to given point. Azimuth angles are counted clockwise, i.e positive towards the East.</p>
293 * @param <T> type of the elements
294 * @param extPoint point for which elevation shall be computed
295 * @param frame frame in which the point is defined
296 * @param date computation date
297 * @return azimuth of the point
298 * @since 9.3
299 */
300 public <T extends CalculusFieldElement<T>> T getAzimuth(final FieldVector3D<T> extPoint, final Frame frame,
301 final FieldAbsoluteDate<T> date) {
302
303 // Transform given point from given frame to topocentric frame
304 final FieldVector3D<T> extPointTopo = transformPoint(extPoint, frame, date);
305
306 return computeAzimuthFromTopoPoint(extPointTopo);
307
308 }
309
310 /** Get the range of a point with regards to the topocentric frame center point.
311 * @param extPoint point for which range shall be computed
312 * @param frame frame in which the point is defined
313 * @param date computation date
314 * @return range (distance) of the point
315 */
316 public double getRange(final Vector3D extPoint, final Frame frame,
317 final AbsoluteDate date) {
318
319 // Transform given point from given frame to topocentric frame
320 final Vector3D extPointTopo = transformPoint(extPoint, frame, date);
321
322 return extPointTopo.getNorm();
323
324 }
325
326 /** Get the range of a point with regards to the topocentric frame center point.
327 * @param <T> type of the elements
328 * @param extPoint point for which range shall be computed
329 * @param frame frame in which the point is defined
330 * @param date computation date
331 * @return range (distance) of the point
332 * @since 9.3
333 */
334 public <T extends CalculusFieldElement<T>> T getRange(final FieldVector3D<T> extPoint, final Frame frame,
335 final FieldAbsoluteDate<T> date) {
336
337 // Transform given point from given frame to topocentric frame
338 final FieldVector3D<T> extPointTopo = transformPoint(extPoint, frame, date);
339
340 return extPointTopo.getNorm();
341
342 }
343
344 /** Get the range rate of a point with regards to the topocentric frame center point.
345 * @param extPV point/velocity for which range rate shall be computed
346 * @param frame frame in which the point is defined
347 * @param date computation date
348 * @return range rate of the point (positive if point departs from frame)
349 */
350 public double getRangeRate(final PVCoordinates extPV, final Frame frame,
351 final AbsoluteDate date) {
352
353 // Transform given point from given frame to topocentric frame
354 final KinematicTransform t = frame.getKinematicTransformTo(this, date);
355 final PVCoordinates extPVTopo = t.transformOnlyPV(extPV);
356
357 // Compute range rate (doppler) : relative rate along the line of sight
358 return Vector3D.dotProduct(extPVTopo.getPosition(), extPVTopo.getVelocity()) /
359 extPVTopo.getPosition().getNorm();
360
361 }
362
363 /** Get the range rate of a point with regards to the topocentric frame center point.
364 * @param <T> type of the elements
365 * @param extPV point/velocity for which range rate shall be computed
366 * @param frame frame in which the point is defined
367 * @param date computation date
368 * @return range rate of the point (positive if point departs from frame)
369 * @since 9.3
370 */
371 public <T extends CalculusFieldElement<T>> T getRangeRate(final FieldPVCoordinates<T> extPV, final Frame frame,
372 final FieldAbsoluteDate<T> date) {
373
374 // Transform given point from given frame to topocentric frame
375 final FieldKinematicTransform<T> t = frame.getKinematicTransformTo(this, date);
376 final FieldPVCoordinates<T> extPVTopo = t.transformOnlyPV(extPV);
377
378 // Compute range rate (doppler) : relative rate along the line of sight
379 return FieldVector3D.dotProduct(extPVTopo.getPosition(), extPVTopo.getVelocity()).divide(
380 extPVTopo.getPosition().getNorm());
381
382 }
383
384 /**
385 * Compute the limit visibility point for a satellite in a given direction.
386 * <p>
387 * This method can be used to compute visibility circles around ground stations
388 * for example, using a simple loop on azimuth, with either a fixed elevation
389 * or an elevation that depends on azimuth to take ground masks into account.
390 * </p>
391 * @param radius satellite distance to Earth center
392 * @param azimuth pointing azimuth from station
393 * @param elevation pointing elevation from station
394 * @return limit visibility point for the satellite
395 */
396 public GeodeticPoint computeLimitVisibilityPoint(final double radius,
397 final double azimuth, final double elevation) {
398 try {
399 // convergence threshold on point position: 1mm
400 final double deltaP = 0.001;
401 final UnivariateSolver solver =
402 new BracketingNthOrderBrentSolver(deltaP / Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
403 deltaP, deltaP, 5);
404
405 // find the distance such that a point in the specified direction and at the solved-for
406 // distance is exactly at the specified radius
407 final double distance = solver.solve(1000, new UnivariateFunction() {
408 /** {@inheritDoc} */
409 public double value(final double x) {
410 final GeodeticPoint gp = pointAtDistance(azimuth, elevation, x);
411 return parentShape.transform(gp).getNorm() - radius;
412 }
413 }, 0, 2 * radius);
414
415 // return the limit point
416 return pointAtDistance(azimuth, elevation, distance);
417
418 } catch (MathRuntimeException mrte) {
419 throw new OrekitException(mrte);
420 }
421 }
422
423 /** Compute the point observed from the station at some specified distance.
424 * @param azimuth pointing azimuth from station
425 * @param elevation pointing elevation from station
426 * @param distance distance to station
427 * @return observed point
428 */
429 public GeodeticPoint pointAtDistance(final double azimuth, final double elevation,
430 final double distance) {
431 final SinCos scAz = FastMath.sinCos(azimuth);
432 final SinCos scEl = FastMath.sinCos(elevation);
433 final Vector3D observed = new Vector3D(distance * scEl.cos() * scAz.sin(),
434 distance * scEl.cos() * scAz.cos(),
435 distance * scEl.sin());
436 return parentShape.transform(observed, this, AbsoluteDate.ARBITRARY_EPOCH);
437 }
438
439 /** {@inheritDoc} */
440 @Override
441 public Vector3D getPosition(final AbsoluteDate date, final Frame frame) {
442 return getStaticTransformTo(frame, date).transformPosition(Vector3D.ZERO);
443 }
444
445 /** Get the {@link PVCoordinates} of the topocentric frame origin in the selected frame.
446 * @param date current date
447 * @param frame the frame where to define the position
448 * @return position/velocity of the topocentric frame origin (m and m/s)
449 */
450 public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame frame) {
451 return getTransformTo(frame, date).transformPVCoordinates(new TimeStampedPVCoordinates(date,
452 Vector3D.ZERO,
453 Vector3D.ZERO,
454 Vector3D.ZERO));
455 }
456
457 /** Get the topocentric position from {@link TrackingCoordinates}.
458 * @param coords The coordinates that are to be converted.
459 * @return The topocentric coordinates.
460 * @since 12.1
461 */
462 public static Vector3D getTopocentricPosition(final TrackingCoordinates coords) {
463 return getTopocentricPosition(coords.getAzimuth(), coords.getElevation(), coords.getRange());
464 }
465
466 /** Get the topocentric position from {@link FieldTrackingCoordinates}.
467 * @param coords The coordinates that are to be converted.
468 * @param <T> Type of the field coordinates.
469 * @return The topocentric coordinates.
470 * @since 12.1
471 */
472 public static <T extends CalculusFieldElement<T>> FieldVector3D<T> getTopocentricPosition(final FieldTrackingCoordinates<T> coords) {
473 return getTopocentricPosition(coords.getAzimuth(), coords.getElevation(), coords.getRange());
474 }
475
476 /**
477 * Gets the topocentric position from a set of az/el/ra coordinates.
478 * @param azimuth the angle of rotation around the vertical axis, going East.
479 * @param elevation the elevation angle from the local horizon.
480 * @param range the distance from the goedetic position.
481 * @return the topocentric position.
482 * @since 12.1
483 */
484 private static Vector3D getTopocentricPosition(final double azimuth, final double elevation, final double range) {
485 final SinCos sinCosAz = FastMath.sinCos(azimuth);
486 final SinCos sinCosEL = FastMath.sinCos(elevation);
487 return new Vector3D(range * sinCosEL.cos() * sinCosAz.sin(), range * sinCosEL.cos() * sinCosAz.cos(), range * sinCosEL.sin());
488 }
489
490 /**
491 * Gets the topocentric position from a set of az/el/ra coordinates.
492 * @param azimuth the angle of rotation around the vertical axis, going East.
493 * @param elevation the elevation angle from the local horizon.
494 * @param range the distance from the geodetic position.
495 * @return the topocentric position.
496 * @param <T> the type of the az/el/ra coordinates.
497 * @since 12.1
498 */
499 private static <T extends CalculusFieldElement<T>> FieldVector3D<T> getTopocentricPosition(final T azimuth, final T elevation, final T range) {
500 final FieldSinCos<T> sinCosAz = FastMath.sinCos(azimuth);
501 final FieldSinCos<T> sinCosEl = FastMath.sinCos(elevation);
502 return new FieldVector3D<>(
503 range.multiply(sinCosEl.cos()).multiply(sinCosAz.sin()),
504 range.multiply(sinCosEl.cos()).multiply(sinCosAz.cos()),
505 range.multiply(sinCosEl.sin())
506 );
507 }
508
509 /** Transform point in topocentric frame.
510 * @param extPoint point
511 * @param date current date
512 * @param frame the frame where to define the position
513 * @return transformed point in topocentric frame
514 */
515 private Vector3D transformPoint(final Vector3D extPoint, final Frame frame, final AbsoluteDate date) {
516 final StaticTransform t = frame.getStaticTransformTo(this, date);
517 return t.transformPosition(extPoint);
518 }
519
520 /** Transform point in topocentric frame.
521 * @param <T> type of the field elements
522 * @param extPoint point
523 * @param date current date
524 * @param frame the frame where to define the position
525 * @return transformed point in topocentric frame
526 */
527 private <T extends CalculusFieldElement<T>> FieldVector3D<T> transformPoint(final FieldVector3D<T> extPoint,
528 final Frame frame,
529 final FieldAbsoluteDate<T> date) {
530 final FieldStaticTransform<T> t = frame.getStaticTransformTo(this, date);
531 return t.transformPosition(extPoint);
532 }
533
534 /** Compute azimuth from topocentric point.
535 * @param extPointTopo topocentric point
536 * @return azimuth
537 */
538 private double computeAzimuthFromTopoPoint(final Vector3D extPointTopo) {
539 final double azimuth = FastMath.atan2(extPointTopo.getX(), extPointTopo.getY());
540 if (azimuth < 0.0) {
541 return azimuth + MathUtils.TWO_PI;
542 } else {
543 return azimuth;
544 }
545 }
546
547 /** Compute azimuth from topocentric point.
548 * @param <T> type of the field elements
549 * @param extPointTopo topocentric point
550 * @return azimuth
551 */
552 private <T extends CalculusFieldElement<T>> T computeAzimuthFromTopoPoint(final FieldVector3D<T> extPointTopo) {
553 final T azimuth = FastMath.atan2(extPointTopo.getX(), extPointTopo.getY());
554 if (azimuth.getReal() < 0.0) {
555 return azimuth.add(MathUtils.TWO_PI);
556 } else {
557 return azimuth;
558 }
559 }
560
561 }