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