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