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.bodies;
18
19 import java.util.function.DoubleFunction;
20
21 import org.hipparchus.CalculusFieldElement;
22 import org.hipparchus.Field;
23 import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
24 import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
25 import org.hipparchus.geometry.euclidean.threed.FieldLine;
26 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
27 import org.hipparchus.geometry.euclidean.threed.Line;
28 import org.hipparchus.geometry.euclidean.threed.Vector3D;
29 import org.hipparchus.geometry.euclidean.twod.Vector2D;
30 import org.hipparchus.util.FastMath;
31 import org.hipparchus.util.FieldSinCos;
32 import org.hipparchus.util.MathArrays;
33 import org.hipparchus.util.MathUtils;
34 import org.hipparchus.util.SinCos;
35 import org.orekit.frames.FieldStaticTransform;
36 import org.orekit.frames.Frame;
37 import org.orekit.frames.StaticTransform;
38 import org.orekit.frames.Transform;
39 import org.orekit.time.AbsoluteDate;
40 import org.orekit.time.FieldAbsoluteDate;
41 import org.orekit.utils.PVCoordinates;
42 import org.orekit.utils.TimeStampedPVCoordinates;
43
44
45 /** Modeling of a one-axis ellipsoid.
46
47 * <p>One-axis ellipsoids is a good approximate model for most planet-size
48 * and larger natural bodies. It is the equilibrium shape reached by
49 * a fluid body under its own gravity field when it rotates. The symmetry
50 * axis is the rotation or polar axis.</p>
51
52 * @author Luc Maisonobe
53 * @author Guylaine Prat
54 */
55 public class OneAxisEllipsoid extends Ellipsoid implements BodyShape {
56
57 /** Threshold for polar and equatorial points detection. */
58 private static final double ANGULAR_THRESHOLD = 1.0e-4;
59
60 /** Equatorial radius power 2. */
61 private final double ae2;
62
63 /** Polar radius power 2. */
64 private final double ap2;
65
66 /** Flattening. */
67 private final double f;
68
69 /** Eccentricity. */
70 private final double e;
71
72 /** Eccentricity squared. */
73 private final double e2;
74
75 /** 1 minus flatness. */
76 private final double g;
77
78 /** g squared. */
79 private final double g2;
80
81 /** Convergence limit. */
82 private double angularThreshold;
83
84 /** Simple constructor.
85 * <p>Standard values for Earth models can be found in the {@link org.orekit.utils.Constants Constants} class:</p>
86 * <table border="1" style="background-color:#f5f5dc;">
87 * <caption>Ellipsoid Models</caption>
88 * <tr style="background-color:#c9d5c9;"><th>model</th><th>a<sub>e</sub> (m)</th> <th>f</th></tr>
89 * <tr><td style="background-color:#c9d5c9; padding:5px">GRS 80</td>
90 * <td>{@link org.orekit.utils.Constants#GRS80_EARTH_EQUATORIAL_RADIUS Constants.GRS80_EARTH_EQUATORIAL_RADIUS}</td>
91 * <td>{@link org.orekit.utils.Constants#GRS80_EARTH_FLATTENING Constants.GRS80_EARTH_FLATTENING}</td></tr>
92 * <tr><td style="background-color:#c9d5c9; padding:5px">WGS84</td>
93 * <td>{@link org.orekit.utils.Constants#WGS84_EARTH_EQUATORIAL_RADIUS Constants.WGS84_EARTH_EQUATORIAL_RADIUS}</td>
94 * <td>{@link org.orekit.utils.Constants#WGS84_EARTH_FLATTENING Constants.WGS84_EARTH_FLATTENING}</td></tr>
95 * <tr><td style="background-color:#c9d5c9; padding:5px">IERS96</td>
96 * <td>{@link org.orekit.utils.Constants#IERS96_EARTH_EQUATORIAL_RADIUS Constants.IERS96_EARTH_EQUATORIAL_RADIUS}</td>
97 * <td>{@link org.orekit.utils.Constants#IERS96_EARTH_FLATTENING Constants.IERS96_EARTH_FLATTENING}</td></tr>
98 * <tr><td style="background-color:#c9d5c9; padding:5px">IERS2003</td>
99 * <td>{@link org.orekit.utils.Constants#IERS2003_EARTH_EQUATORIAL_RADIUS Constants.IERS2003_EARTH_EQUATORIAL_RADIUS}</td>
100 * <td>{@link org.orekit.utils.Constants#IERS2003_EARTH_FLATTENING Constants.IERS2003_EARTH_FLATTENING}</td></tr>
101 * <tr><td style="background-color:#c9d5c9; padding:5px">IERS2010</td>
102 * <td>{@link org.orekit.utils.Constants#IERS2010_EARTH_EQUATORIAL_RADIUS Constants.IERS2010_EARTH_EQUATORIAL_RADIUS}</td>
103 * <td>{@link org.orekit.utils.Constants#IERS2010_EARTH_FLATTENING Constants.IERS2010_EARTH_FLATTENING}</td></tr>
104 * </table>
105 * @param ae equatorial radius
106 * @param f the flattening (f = (a-b)/a)
107 * @param bodyFrame body frame related to body shape
108 * @see org.orekit.frames.FramesFactory#getITRF(org.orekit.utils.IERSConventions, boolean)
109 */
110 public OneAxisEllipsoid(final double ae, final double f,
111 final Frame bodyFrame) {
112 super(bodyFrame, ae, ae, ae * (1.0 - f));
113 this.f = f;
114 this.ae2 = ae * ae;
115 this.e2 = f * (2.0 - f);
116 this.e = FastMath.sqrt(e2);
117 this.g = 1.0 - f;
118 this.g2 = g * g;
119 this.ap2 = ae2 * g2;
120 setAngularThreshold(1.0e-12);
121 }
122
123 /** Set the angular convergence threshold.
124 * <p>The angular threshold is used both to identify points close to
125 * the ellipse axes and as the convergence threshold used to
126 * stop the iterations in the {@link #transform(Vector3D, Frame,
127 * AbsoluteDate)} method.</p>
128 * <p>If this method is not called, the default value is set to
129 * 10<sup>-12</sup>.</p>
130 * @param angularThreshold angular convergence threshold (rad)
131 */
132 public void setAngularThreshold(final double angularThreshold) {
133 this.angularThreshold = angularThreshold;
134 }
135
136 /** Get the equatorial radius of the body.
137 * @return equatorial radius of the body (m)
138 */
139 public double getEquatorialRadius() {
140 return getA();
141 }
142
143 /** Get the flattening of the body: f = (a-b)/a.
144 * @return the flattening
145 */
146 public double getFlattening() {
147 return f;
148 }
149
150 /** Get the first eccentricity squared of the ellipsoid: e^2 = f * (2.0 - f).
151 * @return the eccentricity squared
152 */
153 public double getEccentricitySquared() {
154 return e2;
155 }
156
157 /** Get the first eccentricity of the ellipsoid: e = sqrt(f * (2.0 - f)).
158 * @return the eccentricity
159 */
160 public double getEccentricity() {
161 return e;
162 }
163
164 /** Get body frame related to body shape.
165 * <p>Be mindful that the OneAxisEllipsoid.getBodyFrame() and
166 * the OneAxisEllipsoid.getFrame() methods return the same object.</p>
167 * @return body frame related to body shape
168 */
169 public Frame getBodyFrame() {
170 return getFrame();
171 }
172
173 /** Get the intersection point of a line with the surface of the body.
174 * <p>A line may have several intersection points with a closed
175 * surface (we consider the one point case as a degenerated two
176 * points case). The close parameter is used to select which of
177 * these points should be returned. The selected point is the one
178 * that is closest to the close point.</p>
179 * @param line test line (may intersect the body or not)
180 * @param close point used for intersections selection
181 * @param frame frame in which line is expressed
182 * @param date date of the line in given frame
183 * @return intersection point at altitude zero or null if the line does
184 * not intersect the surface
185 * @since 9.3
186 */
187 public Vector3D getCartesianIntersectionPoint(final Line line, final Vector3D close,
188 final Frame frame, final AbsoluteDate date) {
189
190 // transform line and close to body frame
191 final StaticTransform frameToBodyFrame =
192 frame.getStaticTransformTo(getFrame(), date);
193 final Line lineInBodyFrame = frameToBodyFrame.transformLine(line);
194
195 // compute some miscellaneous variables
196 final Vector3D point = lineInBodyFrame.getOrigin();
197 final double x = point.getX();
198 final double y = point.getY();
199 final double z = point.getZ();
200 final double z2 = z * z;
201 final double r2 = x * x + y * y;
202
203 final Vector3D direction = lineInBodyFrame.getDirection();
204 final double dx = direction.getX();
205 final double dy = direction.getY();
206 final double dz = direction.getZ();
207 final double cz2 = dx * dx + dy * dy;
208
209 // abscissa of the intersection as a root of a 2nd degree polynomial :
210 // a k^2 - 2 b k + c = 0
211 final double a = 1.0 - e2 * cz2;
212 final double b = -(g2 * (x * dx + y * dy) + z * dz);
213 final double c = g2 * (r2 - ae2) + z2;
214 final double b2 = b * b;
215 final double ac = a * c;
216 if (b2 < ac) {
217 return null;
218 }
219 final double s = FastMath.sqrt(b2 - ac);
220 final double k1 = (b < 0) ? (b - s) / a : c / (b + s);
221 final double k2 = c / (a * k1);
222
223 // select the right point
224 final Vector3D closeInBodyFrame = frameToBodyFrame.transformPosition(close);
225 final double closeAbscissa = lineInBodyFrame.getAbscissa(closeInBodyFrame);
226 final double k =
227 (FastMath.abs(k1 - closeAbscissa) < FastMath.abs(k2 - closeAbscissa)) ? k1 : k2;
228 return lineInBodyFrame.pointAt(k);
229
230 }
231
232 /** {@inheritDoc} */
233 public GeodeticPoint getIntersectionPoint(final Line line, final Vector3D close,
234 final Frame frame, final AbsoluteDate date) {
235
236 final Vector3D intersection = getCartesianIntersectionPoint(line, close, frame, date);
237 if (intersection == null) {
238 return null;
239 }
240 final double ix = intersection.getX();
241 final double iy = intersection.getY();
242 final double iz = intersection.getZ();
243
244 final double lambda = FastMath.atan2(iy, ix);
245 final double phi = FastMath.atan2(iz, g2 * FastMath.sqrt(ix * ix + iy * iy));
246 return new GeodeticPoint(phi, lambda, 0.0);
247
248 }
249
250 /** Get the intersection point of a line with the surface of the body.
251 * <p>A line may have several intersection points with a closed
252 * surface (we consider the one point case as a degenerated two
253 * points case). The close parameter is used to select which of
254 * these points should be returned. The selected point is the one
255 * that is closest to the close point.</p>
256 * @param line test line (may intersect the body or not)
257 * @param close point used for intersections selection
258 * @param frame frame in which line is expressed
259 * @param date date of the line in given frame
260 * @param <T> type of the field elements
261 * @return intersection point at altitude zero or null if the line does
262 * not intersect the surface
263 * @since 9.3
264 */
265 public <T extends CalculusFieldElement<T>> FieldVector3D<T> getCartesianIntersectionPoint(final FieldLine<T> line,
266 final FieldVector3D<T> close,
267 final Frame frame,
268 final FieldAbsoluteDate<T> date) {
269
270 // transform line and close to body frame
271 final FieldStaticTransform<T> frameToBodyFrame = frame.getStaticTransformTo(getFrame(), date);
272 final FieldLine<T> lineInBodyFrame = frameToBodyFrame.transformLine(line);
273
274 // compute some miscellaneous variables
275 final FieldVector3D<T> point = lineInBodyFrame.getOrigin();
276 final T x = point.getX();
277 final T y = point.getY();
278 final T z = point.getZ();
279 final T z2 = z.square();
280 final T r2 = x.square().add(y.square());
281
282 final FieldVector3D<T> direction = lineInBodyFrame.getDirection();
283 final T dx = direction.getX();
284 final T dy = direction.getY();
285 final T dz = direction.getZ();
286 final T cz2 = dx.square().add(dy.square());
287
288 // abscissa of the intersection as a root of a 2nd degree polynomial :
289 // a k^2 - 2 b k + c = 0
290 final T a = cz2.multiply(e2).subtract(1.0).negate();
291 final T b = x.multiply(dx).add(y.multiply(dy)).multiply(g2).add(z.multiply(dz)).negate();
292 final T c = r2.subtract(ae2).multiply(g2).add(z2);
293 final T b2 = b.square();
294 final T ac = a.multiply(c);
295 if (b2.getReal() < ac.getReal()) {
296 return null;
297 }
298 final T s = b2.subtract(ac).sqrt();
299 final T k1 = (b.getReal() < 0) ? b.subtract(s).divide(a) : c.divide(b.add(s));
300 final T k2 = c.divide(a.multiply(k1));
301
302 // select the right point
303 final FieldVector3D<T> closeInBodyFrame = frameToBodyFrame.transformPosition(close);
304 final T closeAbscissa = lineInBodyFrame.getAbscissa(closeInBodyFrame);
305 final T k = (FastMath.abs(k1.getReal() - closeAbscissa.getReal()) < FastMath.abs(k2.getReal() - closeAbscissa.getReal())) ?
306 k1 : k2;
307 return lineInBodyFrame.pointAt(k);
308 }
309
310 /** {@inheritDoc} */
311 public <T extends CalculusFieldElement<T>> FieldGeodeticPoint<T> getIntersectionPoint(final FieldLine<T> line,
312 final FieldVector3D<T> close,
313 final Frame frame,
314 final FieldAbsoluteDate<T> date) {
315
316 final FieldVector3D<T> intersection = getCartesianIntersectionPoint(line, close, frame, date);
317 if (intersection == null) {
318 return null;
319 }
320 final T ix = intersection.getX();
321 final T iy = intersection.getY();
322 final T iz = intersection.getZ();
323
324 final T lambda = iy.atan2(ix);
325 final T phi = iz.atan2(ix.multiply(ix).add(iy.multiply(iy)).sqrt().multiply(g2));
326 return new FieldGeodeticPoint<>(phi, lambda, phi.getField().getZero());
327
328 }
329
330 /** {@inheritDoc} */
331 public Vector3D transform(final GeodeticPoint point) {
332 final double longitude = point.getLongitude();
333 final SinCos scLambda = FastMath.sinCos(longitude);
334 final double latitude = point.getLatitude();
335 final SinCos scPhi = FastMath.sinCos(latitude);
336 final double h = point.getAltitude();
337 final double n = getA() / FastMath.sqrt(1.0 - e2 * scPhi.sin() * scPhi.sin());
338 final double r = (n + h) * scPhi.cos();
339 return new Vector3D(r * scLambda.cos(), r * scLambda.sin(), (g2 * n + h) * scPhi.sin());
340 }
341
342 /** {@inheritDoc} */
343 public <T extends CalculusFieldElement<T>> FieldVector3D<T> transform(final FieldGeodeticPoint<T> point) {
344
345 final T latitude = point.getLatitude();
346 final T longitude = point.getLongitude();
347 final T altitude = point.getAltitude();
348
349 final FieldSinCos<T> scLambda = FastMath.sinCos(longitude);
350 final FieldSinCos<T> scPhi = FastMath.sinCos(latitude);
351 final T cLambda = scLambda.cos();
352 final T sLambda = scLambda.sin();
353 final T cPhi = scPhi.cos();
354 final T sPhi = scPhi.sin();
355 final T n = sPhi.multiply(sPhi).multiply(e2).subtract(1.0).negate().sqrt().reciprocal().multiply(getA());
356 final T r = n.add(altitude).multiply(cPhi);
357
358 return new FieldVector3D<>(r.multiply(cLambda),
359 r.multiply(sLambda),
360 sPhi.multiply(altitude.add(n.multiply(g2))));
361 }
362
363 /** {@inheritDoc} */
364 public Vector3D projectToGround(final Vector3D point, final AbsoluteDate date, final Frame frame) {
365
366 // transform point to body frame
367 final StaticTransform toBody = frame.getStaticTransformTo(getFrame(), date);
368 final Vector3D p = toBody.transformPosition(point);
369 final double z = p.getZ();
370 final double r = FastMath.hypot(p.getX(), p.getY());
371
372 // set up the 2D meridian ellipse
373 final Ellipse meridian = new Ellipse(Vector3D.ZERO,
374 r == 0 ? Vector3D.PLUS_I : new Vector3D(p.getX() / r, p.getY() / r, 0),
375 Vector3D.PLUS_K,
376 getA(), getC(), getFrame());
377
378 // find the closest point in the meridian plane
379 final Vector3D groundPoint = meridian.toSpace(meridian.projectToEllipse(new Vector2D(r, z)));
380
381 // transform point back to initial frame
382 return toBody.getInverse().transformPosition(groundPoint);
383
384 }
385
386 /** {@inheritDoc} */
387 public TimeStampedPVCoordinates projectToGround(final TimeStampedPVCoordinates pv, final Frame frame) {
388
389 // transform point to body frame
390 final Transform toBody = frame.getTransformTo(getFrame(), pv.getDate());
391 final TimeStampedPVCoordinates pvInBodyFrame = toBody.transformPVCoordinates(pv);
392 final Vector3D p = pvInBodyFrame.getPosition();
393 final double r = FastMath.hypot(p.getX(), p.getY());
394
395 // set up the 2D ellipse corresponding to first principal curvature along meridian
396 final Vector3D meridian = r == 0 ? Vector3D.PLUS_I : new Vector3D(p.getX() / r, p.getY() / r, 0);
397 final Ellipse firstPrincipalCurvature =
398 new Ellipse(Vector3D.ZERO, meridian, Vector3D.PLUS_K, getA(), getC(), getFrame());
399
400 // project coordinates in the meridian plane
401 final TimeStampedPVCoordinates gpFirst = firstPrincipalCurvature.projectToEllipse(pvInBodyFrame);
402 final Vector3D gpP = gpFirst.getPosition();
403 final double gr = MathArrays.linearCombination(gpP.getX(), meridian.getX(),
404 gpP.getY(), meridian.getY());
405 final double gz = gpP.getZ();
406
407 // topocentric frame
408 final Vector3D east = new Vector3D(-meridian.getY(), meridian.getX(), 0);
409 final Vector3D zenith = new Vector3D(gr * getC() / getA(), meridian, gz * getA() / getC(), Vector3D.PLUS_K).normalize();
410 final Vector3D north = Vector3D.crossProduct(zenith, east);
411
412 // set up the ellipse corresponding to second principal curvature in the zenith/east plane
413 final Ellipse secondPrincipalCurvature = getPlaneSection(gpP, north);
414 final TimeStampedPVCoordinates gpSecond = secondPrincipalCurvature.projectToEllipse(pvInBodyFrame);
415
416 final Vector3D gpV = gpFirst.getVelocity().add(gpSecond.getVelocity());
417 final Vector3D gpA = gpFirst.getAcceleration().add(gpSecond.getAcceleration());
418
419 // moving projected point
420 final TimeStampedPVCoordinates groundPV =
421 new TimeStampedPVCoordinates(pv.getDate(), gpP, gpV, gpA);
422
423 // transform moving projected point back to initial frame
424 return toBody.getInverse().transformPVCoordinates(groundPV);
425
426 }
427
428 /** {@inheritDoc}
429 * <p>
430 * This method is based on Toshio Fukushima's algorithm which uses Halley's method.
431 * <a href="https://www.researchgate.net/publication/227215135_Transformation_from_Cartesian_to_Geodetic_Coordinates_Accelerated_by_Halley's_Method">
432 * transformation from Cartesian to Geodetic Coordinates Accelerated by Halley's Method</a>,
433 * Toshio Fukushima, Journal of Geodesy 9(12):689-693, February 2006
434 * </p>
435 * <p>
436 * Some changes have been added to the original method:
437 * </p>
438 * <ul>
439 * <li>in order to handle more accurately corner cases near the pole</li>
440 * <li>in order to handle properly corner cases near the equatorial plane, even far inside the ellipsoid</li>
441 * <li>in order to handle very flat ellipsoids</li>
442 * </ul>
443 * <p>
444 * In some rare cases (for example very flat ellipsoid, or points close to ellipsoid center), the loop
445 * may fail to converge. As this seems to happen only in degenerate cases, a design choice was to return
446 * an approximate point corresponding to last iteration. This point may be incorrect and fail to give the
447 * initial point back if doing roundtrip by calling {@link #transform(GeodeticPoint)}. This design choice
448 * was made to avoid NaNs appearing for example in inter-satellites visibility checks when two satellites
449 * are almost on opposite sides of Earth. The intermediate points far within the Earth should not prevent
450 * the detection algorithm to find visibility start/end.
451 * </p>
452 */
453 public GeodeticPoint transform(final Vector3D point, final Frame frame, final AbsoluteDate date) {
454
455 // transform point to body frame
456 final Vector3D pointInBodyFrame = frame.getStaticTransformTo(getFrame(), date)
457 .transformPosition(point);
458 final double r2 = pointInBodyFrame.getX() * pointInBodyFrame.getX() +
459 pointInBodyFrame.getY() * pointInBodyFrame.getY();
460 final double r = FastMath.sqrt(r2);
461 final double z = pointInBodyFrame.getZ();
462
463 final double lambda = FastMath.atan2(pointInBodyFrame.getY(), pointInBodyFrame.getX());
464
465 double h;
466 double phi;
467 if (r <= ANGULAR_THRESHOLD * FastMath.abs(z)) {
468 // the point is almost on the polar axis, approximate the ellipsoid with
469 // the osculating sphere whose center is at evolute cusp along polar axis
470 final double osculatingRadius = ae2 / getC();
471 final double evoluteCuspZ = FastMath.copySign(getA() * e2 / g, -z);
472 final double deltaZ = z - evoluteCuspZ;
473 // we use π/2 - atan(r/Δz) instead of atan(Δz/r) for accuracy purposes, as r is much smaller than Δz
474 phi = FastMath.copySign(0.5 * FastMath.PI - FastMath.atan(r / FastMath.abs(deltaZ)), deltaZ);
475 h = FastMath.hypot(deltaZ, r) - osculatingRadius;
476 } else if (FastMath.abs(z) <= ANGULAR_THRESHOLD * r) {
477 // the point is almost on the major axis
478
479 final double osculatingRadius = ap2 / getA();
480 final double evoluteCuspR = getA() * e2;
481 final double deltaR = r - evoluteCuspR;
482 if (deltaR >= 0) {
483 // the point is outside of the ellipse evolute, approximate the ellipse
484 // with the osculating circle whose center is at evolute cusp along major axis
485 phi = (deltaR == 0) ? 0.0 : FastMath.atan(z / deltaR);
486 h = FastMath.hypot(deltaR, z) - osculatingRadius;
487 } else {
488 // the point is on the part of the major axis within ellipse evolute
489 // we can compute the closest ellipse point analytically, and it is NOT near the equator
490 final double rClose = r / e2;
491 final double zClose = FastMath.copySign(g * FastMath.sqrt(ae2 - rClose * rClose), z);
492 phi = FastMath.atan((zClose - z) / (rClose - r));
493 h = -FastMath.hypot(r - rClose, z - zClose);
494 }
495
496 } else {
497 // use Toshio Fukushima method, with several iterations
498 final double epsPhi = 1.0e-15;
499 final double epsH = 1.0e-14 * FastMath.max(getA(), FastMath.sqrt(r2 + z * z));
500 final double c = getA() * e2;
501 final double absZ = FastMath.abs(z);
502 final double zc = g * absZ;
503 double sn = absZ;
504 double sn2 = sn * sn;
505 double cn = g * r;
506 double cn2 = cn * cn;
507 double an2 = cn2 + sn2;
508 double an = FastMath.sqrt(an2);
509 double bn = 0;
510 phi = Double.POSITIVE_INFINITY;
511 h = Double.POSITIVE_INFINITY;
512 for (int i = 0; i < 1000; ++i) {
513 // this usually converges in 2 iterations, but in rare cases it can take much more
514 // see https://gitlab.orekit.org/orekit/orekit/-/issues/1224 for examples
515 // with points near Earth center which need 137 iterations for the first example
516 // and 1150 iterations for the second example
517 final double oldSn = sn;
518 final double oldCn = cn;
519 final double oldPhi = phi;
520 final double oldH = h;
521 final double an3 = an2 * an;
522 final double csncn = c * sn * cn;
523 bn = 1.5 * csncn * ((r * sn - zc * cn) * an - csncn);
524 sn = (zc * an3 + c * sn2 * sn) * an3 - bn * sn;
525 cn = (r * an3 - c * cn2 * cn) * an3 - bn * cn;
526 if (sn * oldSn < 0 || cn < 0) {
527 // the Halley iteration went too far, we restrict it and iterate again
528 while (sn * oldSn < 0 || cn < 0) {
529 sn = (sn + oldSn) / 2;
530 cn = (cn + oldCn) / 2;
531 }
532 } else {
533
534 // rescale components to avoid overflow when several iterations are used
535 final int exp = (FastMath.getExponent(sn) + FastMath.getExponent(cn)) / 2;
536 sn = FastMath.scalb(sn, -exp);
537 cn = FastMath.scalb(cn, -exp);
538
539 sn2 = sn * sn;
540 cn2 = cn * cn;
541 an2 = cn2 + sn2;
542 an = FastMath.sqrt(an2);
543
544 final double cc = g * cn;
545 h = (r * cc + absZ * sn - getA() * g * an) / FastMath.sqrt(an2 - e2 * cn2);
546 if (FastMath.abs(oldH - h) < epsH) {
547 phi = FastMath.copySign(FastMath.atan(sn / cc), z);
548 if (FastMath.abs(oldPhi - phi) < epsPhi) {
549 break;
550 }
551 }
552
553 }
554
555 }
556
557 if (Double.isInfinite(phi)) {
558 // we did not converge, the point is probably within the ellipsoid
559 // we just compute the "best" phi we can to avoid NaN
560 phi = FastMath.copySign(FastMath.atan(sn / (g * cn)), z);
561 }
562
563 }
564
565 return new GeodeticPoint(phi, lambda, h);
566
567 }
568
569 /** {@inheritDoc}
570 * <p>
571 * This method is based on Toshio Fukushima's algorithm which uses Halley's method.
572 * <a href="https://www.researchgate.net/publication/227215135_Transformation_from_Cartesian_to_Geodetic_Coordinates_Accelerated_by_Halley's_Method">
573 * transformation from Cartesian to Geodetic Coordinates Accelerated by Halley's Method</a>,
574 * Toshio Fukushima, Journal of Geodesy 9(12):689-693, February 2006
575 * </p>
576 * <p>
577 * Some changes have been added to the original method:
578 * <ul>
579 * <li>in order to handle more accurately corner cases near the pole</li>
580 * <li>in order to handle properly corner cases near the equatorial plane, even far inside the ellipsoid</li>
581 * <li>in order to handle very flat ellipsoids</li>
582 * </ul>
583 * <p>
584 * In some rare cases (for example very flat ellipsoid, or points close to ellipsoid center), the loop
585 * may fail to converge. As this seems to happen only in degenerate cases, a design choice was to return
586 * an approximate point corresponding to last iteration. This point may be incorrect and fail to give the
587 * initial point back if doing roundtrip by calling {@link #transform(GeodeticPoint)}. This design choice
588 * was made to avoid NaNs appearing for example in inter-satellites visibility checks when two satellites
589 * are almost on opposite sides of Earth. The intermediate points far within the Earth should not prevent
590 * the detection algorithm to find visibility start/end.
591 * </p>
592 */
593 public <T extends CalculusFieldElement<T>> FieldGeodeticPoint<T> transform(final FieldVector3D<T> point,
594 final Frame frame,
595 final FieldAbsoluteDate<T> date) {
596
597 // transform point to body frame
598 final FieldVector3D<T> pointInBodyFrame = (frame == getFrame()) ?
599 point :
600 frame.getStaticTransformTo(getFrame(), date).transformPosition(point);
601 final T r2 = pointInBodyFrame.getX().multiply(pointInBodyFrame.getX()).
602 add(pointInBodyFrame.getY().multiply(pointInBodyFrame.getY()));
603 final T r = r2.sqrt();
604 final T z = pointInBodyFrame.getZ();
605
606 final T lambda = pointInBodyFrame.getY().atan2(pointInBodyFrame.getX());
607
608 T h;
609 T phi;
610 if (r.getReal() <= ANGULAR_THRESHOLD * FastMath.abs(z.getReal())) {
611 // the point is almost on the polar axis, approximate the ellipsoid with
612 // the osculating sphere whose center is at evolute cusp along polar axis
613 final double osculatingRadius = ae2 / getC();
614 final double evoluteCuspZ = FastMath.copySign(getA() * e2 / g, -z.getReal());
615 final T deltaZ = z.subtract(evoluteCuspZ);
616 // we use π/2 - atan(r/Δz) instead of atan(Δz/r) for accuracy purposes, as r is much smaller than Δz
617 phi = r.divide(deltaZ.abs()).atan().negate().add(r.getPi().multiply(0.5)).copySign(deltaZ);
618 h = deltaZ.hypot(r).subtract(osculatingRadius);
619 } else if (FastMath.abs(z.getReal()) <= ANGULAR_THRESHOLD * r.getReal()) {
620 // the point is almost on the major axis
621
622 final double osculatingRadius = ap2 / getA();
623 final double evoluteCuspR = getA() * e2;
624 final T deltaR = r.subtract(evoluteCuspR);
625 if (deltaR.getReal() >= 0) {
626 // the point is outside of the ellipse evolute, approximate the ellipse
627 // with the osculating circle whose center is at evolute cusp along major axis
628 phi = (deltaR.getReal() == 0) ? z.getField().getZero() : z.divide(deltaR).atan();
629 h = deltaR.hypot(z).subtract(osculatingRadius);
630 } else {
631 // the point is on the part of the major axis within ellipse evolute
632 // we can compute the closest ellipse point analytically, and it is NOT near the equator
633 final T rClose = r.divide(e2);
634 final T zClose = rClose.multiply(rClose).negate().add(ae2).sqrt().multiply(g).copySign(z);
635 phi = zClose.subtract(z).divide(rClose.subtract(r)).atan();
636 h = r.subtract(rClose).hypot(z.subtract(zClose)).negate();
637 }
638
639 } else {
640 // use Toshio Fukushima method, with several iterations
641 final double epsPhi = 1.0e-15;
642 final double epsH = 1.0e-14 * getA();
643 final double c = getA() * e2;
644 final T absZ = z.abs();
645 final T zc = absZ.multiply(g);
646 T sn = absZ;
647 T sn2 = sn.multiply(sn);
648 T cn = r.multiply(g);
649 T cn2 = cn.multiply(cn);
650 T an2 = cn2.add(sn2);
651 T an = an2.sqrt();
652 T bn = an.getField().getZero();
653 phi = an.getField().getZero().add(Double.POSITIVE_INFINITY);
654 h = an.getField().getZero().add(Double.POSITIVE_INFINITY);
655 for (int i = 0; i < 1000; ++i) {
656 // this usually converges in 2 iterations, but in rare cases it can take much more
657 // see https://gitlab.orekit.org/orekit/orekit/-/issues/1224 for examples
658 // with points near Earth center which need 137 iterations for the first example
659 // and 1150 iterations for the second example
660 final T oldSn = sn;
661 final T oldCn = cn;
662 final T oldPhi = phi;
663 final T oldH = h;
664 final T an3 = an2.multiply(an);
665 final T csncn = sn.multiply(cn).multiply(c);
666 bn = csncn.multiply(1.5).multiply((r.multiply(sn).subtract(zc.multiply(cn))).multiply(an).subtract(csncn));
667 sn = zc.multiply(an3).add(sn2.multiply(sn).multiply(c)).multiply(an3).subtract(bn.multiply(sn));
668 cn = r.multiply(an3).subtract(cn2.multiply(cn).multiply(c)).multiply(an3).subtract(bn.multiply(cn));
669 if (sn.getReal() * oldSn.getReal() < 0 || cn.getReal() < 0) {
670 // the Halley iteration went too far, we restrict it and iterate again
671 while (sn.getReal() * oldSn.getReal() < 0 || cn.getReal() < 0) {
672 sn = sn.add(oldSn).multiply(0.5);
673 cn = cn.add(oldCn).multiply(0.5);
674 }
675 } else {
676
677 // rescale components to avoid overflow when several iterations are used
678 final int exp = (FastMath.getExponent(sn.getReal()) + FastMath.getExponent(cn.getReal())) / 2;
679 sn = sn.scalb(-exp);
680 cn = cn.scalb(-exp);
681
682 sn2 = sn.square();
683 cn2 = cn.square();
684 an2 = cn2.add(sn2);
685 an = an2.sqrt();
686
687 final T cc = cn.multiply(g);
688 h = r.multiply(cc).add(absZ.multiply(sn)).subtract(an.multiply(getA() * g)).divide(an2.subtract(cn2.multiply(e2)).sqrt());
689 if (FastMath.abs(oldH.getReal() - h.getReal()) < epsH) {
690 phi = sn.divide(cc).atan().copySign(z);
691 if (FastMath.abs(oldPhi.getReal() - phi.getReal()) < epsPhi) {
692 break;
693 }
694 }
695
696 }
697
698 }
699
700 if (Double.isInfinite(phi.getReal())) {
701 // we did not converge, the point is probably within the ellipsoid
702 // we just compute the "best" phi we can to avoid NaN
703 phi = sn.divide(cn.multiply(g)).atan().copySign(z);
704 }
705
706 }
707
708 return new FieldGeodeticPoint<>(phi, lambda, h);
709
710 }
711
712 /** Transform a Cartesian point to a surface-relative point.
713 * @param point Cartesian point
714 * @param frame frame in which Cartesian point is expressed
715 * @param date date of the computation (used for frames conversions)
716 * @return point at the same location but as a surface-relative point,
717 * using time as the single derivation parameter
718 */
719 public FieldGeodeticPoint<UnivariateDerivative2> transform(final PVCoordinates point,
720 final Frame frame, final AbsoluteDate date) {
721
722 // transform point to body frame
723 final Transform toBody = frame.getTransformTo(getFrame(), date);
724 final PVCoordinates pointInBodyFrame = toBody.transformPVCoordinates(point);
725 final FieldVector3D<UnivariateDerivative2> p = pointInBodyFrame.toUnivariateDerivative2Vector();
726 final UnivariateDerivative2 pr2 = p.getX().square().add(p.getY().square());
727 final UnivariateDerivative2 pr = pr2.sqrt();
728 final UnivariateDerivative2 pz = p.getZ();
729
730 // project point on the ellipsoid surface
731 final TimeStampedPVCoordinates groundPoint = projectToGround(new TimeStampedPVCoordinates(date, pointInBodyFrame),
732 getFrame());
733 final FieldVector3D<UnivariateDerivative2> gp = groundPoint.toUnivariateDerivative2Vector();
734 final UnivariateDerivative2 gpr2 = gp.getX().square().add(gp.getY().square());
735 final UnivariateDerivative2 gpr = gpr2.sqrt();
736 final UnivariateDerivative2 gpz = gp.getZ();
737
738 // relative position of test point with respect to its ellipse sub-point
739 final UnivariateDerivative2 dr = pr.subtract(gpr);
740 final UnivariateDerivative2 dz = pz.subtract(gpz);
741 final double insideIfNegative = g2 * (pr2.getReal() - ae2) + pz.getReal() * pz.getReal();
742
743 return new FieldGeodeticPoint<>(FastMath.atan2(gpz, gpr.multiply(g2)), FastMath.atan2(p.getY(), p.getX()),
744 FastMath.hypot(dr, dz).copySign(insideIfNegative));
745 }
746
747 /** Compute the azimuth angle from local north between the two points.
748 * The angle is calculated clockwise from local north at the origin point
749 * and follows the rhumb line to the destination point.
750 *
751 * @param origin the origin point, at which the azimuth angle will be computed (non-{@code null})
752 * @param destination the destination point, to which the angle is defined (non-{@code null})
753 * @return the resulting azimuth angle (radians, {@code [0-2pi)})
754 * @since 11.3
755 */
756 public double azimuthBetweenPoints(final GeodeticPoint origin, final GeodeticPoint destination) {
757 final double dLon = MathUtils.normalizeAngle(destination.getLongitude(), origin.getLongitude()) - origin.getLongitude();
758 final double originIsoLat = geodeticToIsometricLatitude(origin.getLatitude());
759 final double destIsoLat = geodeticToIsometricLatitude(destination.getLatitude());
760
761 final double az = FastMath.atan2(dLon, destIsoLat - originIsoLat);
762 if (az < 0.) {
763 return az + MathUtils.TWO_PI;
764 }
765 return az;
766 }
767
768 /** Compute the azimuth angle from local north between the two points.
769 *
770 * The angle is calculated clockwise from local north at the origin point
771 * and follows the rhumb line to the destination point.
772 *
773 * @param origin the origin point, at which the azimuth angle will be computed (non-{@code null})
774 * @param destination the destination point, to which the angle is defined (non-{@code null})
775 * @param <T> the type of field elements
776 * @return the resulting azimuth angle (radians, {@code [0-2pi)})
777 * @since 11.3
778 */
779 public <T extends CalculusFieldElement<T>> T azimuthBetweenPoints(final FieldGeodeticPoint<T> origin, final FieldGeodeticPoint<T> destination) {
780 final T dLon = MathUtils.normalizeAngle(destination.getLongitude().subtract(origin.getLongitude()), origin.getLongitude().getField().getZero());
781 final T originIsoLat = geodeticToIsometricLatitude(origin.getLatitude());
782 final T destIsoLat = geodeticToIsometricLatitude(destination.getLatitude());
783
784 final T az = FastMath.atan2(dLon, destIsoLat.subtract(originIsoLat));
785 if (az.getReal() < 0.) {
786 return az.add(az.getPi().multiply(2));
787 }
788 return az;
789 }
790
791 /** Compute the <a href="https://mathworld.wolfram.com/IsometricLatitude.html">isometric latitude</a>
792 * corresponding to the provided latitude.
793 *
794 * @param geodeticLatitude the latitude (radians, within interval {@code [-pi/2, +pi/2]})
795 * @return the isometric latitude (radians)
796 * @since 11.3
797 */
798 public double geodeticToIsometricLatitude(final double geodeticLatitude) {
799 if (FastMath.abs(geodeticLatitude) <= angularThreshold) {
800 return 0.;
801 }
802
803 final double eSinLat = e * FastMath.sin(geodeticLatitude);
804
805 // first term: ln(tan(pi/4 + lat/2))
806 final double a = FastMath.log(FastMath.tan(FastMath.PI / 4. + geodeticLatitude / 2.));
807 // second term: (ecc / 2) * ln((1 - ecc*sin(lat)) / (1 + ecc * sin(lat)))
808 final double b = (e / 2.) * FastMath.log((1. - eSinLat) / (1. + eSinLat));
809
810 return a + b;
811 }
812
813 /** Compute the <a href="https://mathworld.wolfram.com/IsometricLatitude.html">isometric latitude</a>
814 * corresponding to the provided latitude.
815 *
816 * @param geodeticLatitude the latitude (radians, within interval {@code [-pi/2, +pi/2]})
817 * @param <T> the type of field elements
818 * @return the isometric latitude (radians)
819 * @since 11.3
820 */
821 public <T extends CalculusFieldElement<T>> T geodeticToIsometricLatitude(final T geodeticLatitude) {
822 if (geodeticLatitude.abs().getReal() <= angularThreshold) {
823 return geodeticLatitude.getField().getZero();
824 }
825 final Field<T> field = geodeticLatitude.getField();
826 final T ecc = geodeticLatitude.newInstance(e);
827 final T eSinLat = ecc.multiply(geodeticLatitude.sin());
828
829 // first term: ln(tan(pi/4 + lat/2))
830 final T a = FastMath.log(FastMath.tan(geodeticLatitude.getPi().divide(4.).add(geodeticLatitude.divide(2.))));
831 // second term: (ecc / 2) * ln((1 - ecc*sin(lat)) / (1 + ecc * sin(lat)))
832 final T b = ecc.divide(2.).multiply(FastMath.log(field.getOne().subtract(eSinLat).divide(field.getOne().add(eSinLat))));
833
834 return a.add(b);
835 }
836
837 /** Find intermediate point of lowest altitude along a line between two endpoints.
838 * @param endpoint1 first endpoint, in body frame
839 * @param endpoint2 second endpoint, in body frame
840 * @return point with lowest altitude between {@code endpoint1} and {@code endpoint2}.
841 * @since 12.0
842 */
843 public GeodeticPoint lowestAltitudeIntermediate(final Vector3D endpoint1, final Vector3D endpoint2) {
844
845 final Vector3D delta = endpoint2.subtract(endpoint1);
846
847 // function computing intermediate point above ellipsoid (lambda varying between 0 and 1)
848 final DoubleFunction<GeodeticPoint> intermediate =
849 lambda -> transform(new Vector3D(1 - lambda, endpoint1, lambda, endpoint2),
850 getFrame(), null);
851
852 // first endpoint
853 final GeodeticPoint gp1 = intermediate.apply(0.0);
854
855 if (Vector3D.dotProduct(delta, gp1.getZenith()) >= 0) {
856 // the line from first endpoint to second endpoint is going away from central body
857 // the minimum altitude is reached at first endpoint
858 return gp1;
859 } else {
860 // the line from first endpoint to second endpoint is closing the central body
861
862 // second endpoint
863 final GeodeticPoint gp2 = intermediate.apply(1.0);
864
865 if (Vector3D.dotProduct(delta, gp2.getZenith()) <= 0) {
866 // the line from first endpoint to second endpoint is still decreasing when reaching second endpoint,
867 // the minimum altitude is reached at second endpoint
868 return gp2;
869 } else {
870 // the line from first endpoint to second endpoint reaches a minimum between first and second endpoints
871 final double lambdaMin = new BracketingNthOrderBrentSolver(1.0e-14, 5).
872 solve(1000,
873 lambda -> Vector3D.dotProduct(delta, intermediate.apply(lambda).getZenith()),
874 0.0, 1.0);
875 return intermediate.apply(lambdaMin);
876 }
877 }
878
879 }
880
881 /** Find intermediate point of lowest altitude along a line between two endpoints.
882 * @param endpoint1 first endpoint, in body frame
883 * @param endpoint2 second endpoint, in body frame
884 * @param <T> type of the field elements
885 * @return point with lowest altitude between {@code endpoint1} and {@code endpoint2}.
886 * @since 12.0
887 */
888 public <T extends CalculusFieldElement<T>> FieldGeodeticPoint<T> lowestAltitudeIntermediate(final FieldVector3D<T> endpoint1,
889 final FieldVector3D<T> endpoint2) {
890
891 final FieldVector3D<T> delta = endpoint2.subtract(endpoint1);
892
893 // function computing intermediate point above ellipsoid (lambda varying between 0 and 1)
894 final DoubleFunction<FieldGeodeticPoint<T>> intermediate =
895 lambda -> transform(new FieldVector3D<>(1 - lambda, endpoint1, lambda, endpoint2),
896 getFrame(), null);
897
898 // first endpoint
899 final FieldGeodeticPoint<T> gp1 = intermediate.apply(0.0);
900
901 if (FieldVector3D.dotProduct(delta, gp1.getZenith()).getReal() >= 0) {
902 // the line from first endpoint to second endpoint is going away from central body
903 // the minimum altitude is reached at first endpoint
904 return gp1;
905 } else {
906 // the line from first endpoint to second endpoint is closing the central body
907
908 // second endpoint
909 final FieldGeodeticPoint<T> gp2 = intermediate.apply(1.0);
910
911 if (FieldVector3D.dotProduct(delta, gp2.getZenith()).getReal() <= 0) {
912 // the line from first endpoint to second endpoint is still decreasing when reaching second endpoint,
913 // the minimum altitude is reached at second endpoint
914 return gp2;
915 } else {
916 // the line from first endpoint to second endpoint reaches a minimum between first and second endpoints
917 final double lambdaMin = new BracketingNthOrderBrentSolver(1.0e-14, 5).
918 solve(1000,
919 lambda -> FieldVector3D.dotProduct(delta, intermediate.apply(lambda).getZenith()).getReal(),
920 0.0, 1.0);
921 return intermediate.apply(lambdaMin);
922 }
923 }
924
925 }
926
927 }