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