OneAxisEllipsoid.java

  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;

  19. import java.util.function.DoubleFunction;

  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;


  45. /** Modeling of a one-axis ellipsoid.

  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>

  52.  * @author Luc Maisonobe
  53.  * @author Guylaine Prat
  54.  */
  55. public class OneAxisEllipsoid extends Ellipsoid implements BodyShape {

  57.     /** Threshold for polar and equatorial points detection. */
  58.     private static final double ANGULAR_THRESHOLD = 1.0e-4;

  60.     /** Equatorial radius power 2. */
  61.     private final double ae2;

  63.     /** Polar radius power 2. */
  64.     private final double ap2;

  66.     /** Flattening. */
  67.     private final double f;

  69.     /** Eccentricity. */
  70.     private final double e;

  72.     /** Eccentricity squared. */
  73.     private final double e2;

  75.     /** 1 minus flatness. */
  76.     private final double g;

  78.     /** g squared. */
  79.     private final double g2;

  81.     /** Convergence limit. */
  82.     private double angularThreshold;

  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.     }

  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.     }

  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.     }

  143.     /** Get the flattening of the body: f = (a-b)/a.
  144.      * @return the flattening
  145.      */
  146.     public double getFlattening() {
  147.         return f;
  148.     }

  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.     }

  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.     }

  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.     }

  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) {

  190.         // transform line and close to body frame
  191.         final StaticTransform frameToBodyFrame =
  192.                 frame.getStaticTransformTo(getFrame(), date);
  193.         final Line lineInBodyFrame = frameToBodyFrame.transformLine(line);

  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;

  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;

  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);

  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);

  230.     }

  232.     /** {@inheritDoc} */
  233.     public GeodeticPoint getIntersectionPoint(final Line line, final Vector3D close,
  234.                                               final Frame frame, final AbsoluteDate date) {

  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();

  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);

  248.     }

  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) {

  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);

  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());

  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());

  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));

  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.     }

  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) {

  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();

  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());

  328.     }

  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.     }

  342.     /** {@inheritDoc} */
  343.     public <T extends CalculusFieldElement<T>> FieldVector3D<T> transform(final FieldGeodeticPoint<T> point) {

  345.         final T latitude  = point.getLatitude();
  346.         final T longitude = point.getLongitude();
  347.         final T altitude  = point.getAltitude();

  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);

  358.         return new FieldVector3D<>(r.multiply(cLambda),
  359.                                    r.multiply(sLambda),
  360.                                    sPhi.multiply(altitude.add(n.multiply(g2))));
  361.     }

  363.     /** {@inheritDoc} */
  364.     public Vector3D projectToGround(final Vector3D point, final AbsoluteDate date, final Frame frame) {

  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());

  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());

  378.         // find the closest point in the meridian plane
  379.         final Vector3D groundPoint = meridian.toSpace(meridian.projectToEllipse(new Vector2D(r, z)));

  381.         // transform point back to initial frame
  382.         return toBody.getInverse().transformPosition(groundPoint);

  384.     }

  386.     /** {@inheritDoc} */
  387.     public TimeStampedPVCoordinates projectToGround(final TimeStampedPVCoordinates pv, final Frame frame) {

  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());

  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());

  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();

  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);

  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);

  416.         final Vector3D gpV = gpFirst.getVelocity().add(gpSecond.getVelocity());
  417.         final Vector3D gpA = gpFirst.getAcceleration().add(gpSecond.getAcceleration());

  419.         // moving projected point
  420.         final TimeStampedPVCoordinates groundPV =
  421.                 new TimeStampedPVCoordinates(pv.getDate(), gpP, gpV, gpA);

  423.         // transform moving projected point back to initial frame
  424.         return toBody.getInverse().transformPVCoordinates(groundPV);

  426.     }

  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) {

  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();

  463.         final double   lambda           = FastMath.atan2(pointInBodyFrame.getY(), pointInBodyFrame.getX());

  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

  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.             }

  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 {

  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);

  539.                     sn2 = sn * sn;
  540.                     cn2 = cn * cn;
  541.                     an2 = cn2 + sn2;
  542.                     an  = FastMath.sqrt(an2);

  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.                     }

  553.                 }

  555.             }

  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.             }

  563.         }

  565.         return new GeodeticPoint(phi, lambda, h);

  567.     }

  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) {

  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();

  606.         final T   lambda                        = pointInBodyFrame.getY().atan2(pointInBodyFrame.getX());

  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

  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.             }

  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 {

  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);

  682.                     sn2 = sn.square();
  683.                     cn2 = cn.square();
  684.                     an2 = cn2.add(sn2);
  685.                     an  = an2.sqrt();

  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.                     }

  696.                 }

  698.             }

  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.             }

  706.         }

  708.         return new FieldGeodeticPoint<>(phi, lambda, h);

  710.     }

  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) {

  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();

  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();

  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();

  743.         return new FieldGeodeticPoint<>(FastMath.atan2(gpz, gpr.multiply(g2)), FastMath.atan2(p.getY(), p.getX()),
  744.             FastMath.hypot(dr, dz).copySign(insideIfNegative));
  745.     }

  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());

  761.         final double az = FastMath.atan2(dLon, destIsoLat - originIsoLat);
  762.         if (az < 0.) {
  763.             return az + MathUtils.TWO_PI;
  764.         }
  765.         return az;
  766.     }

  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());

  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.     }

  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.         }

  803.         final double eSinLat = e * FastMath.sin(geodeticLatitude);

  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));

  810.         return a + b;
  811.     }

  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());

  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))));

  834.         return a.add(b);
  835.     }

  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) {

  845.         final Vector3D delta = endpoint2.subtract(endpoint1);

  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);

  852.         // first endpoint
  853.         final GeodeticPoint gp1 = intermediate.apply(0.0);

  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

  862.             // second endpoint
  863.             final GeodeticPoint gp2 = intermediate.apply(1.0);

  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.         }

  879.     }

  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) {

  891.         final FieldVector3D<T> delta = endpoint2.subtract(endpoint1);

  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);

  898.         // first endpoint
  899.         final FieldGeodeticPoint<T> gp1 = intermediate.apply(0.0);

  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

  908.             // second endpoint
  909.             final FieldGeodeticPoint<T> gp2 = intermediate.apply(1.0);

  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.         }

  925.     }

  927. }
Created with JaCoCo 0.8.12.202403310830