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