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