OneAxisEllipsoid.java

  1. /* Copyright 2002-2019 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (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.io.Serializable;

  21. import org.hipparchus.RealFieldElement;
  22. import org.hipparchus.analysis.differentiation.DerivativeStructure;
  23. import org.hipparchus.geometry.euclidean.threed.FieldLine;
  24. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  25. import org.hipparchus.geometry.euclidean.threed.Line;
  26. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  27. import org.hipparchus.geometry.euclidean.twod.Vector2D;
  28. import org.hipparchus.util.FastMath;
  29. import org.hipparchus.util.MathArrays;
  30. import org.orekit.frames.FieldTransform;
  31. import org.orekit.frames.Frame;
  32. import org.orekit.frames.Transform;
  33. import org.orekit.time.AbsoluteDate;
  34. import org.orekit.time.FieldAbsoluteDate;
  35. import org.orekit.utils.PVCoordinates;
  36. import org.orekit.utils.TimeStampedPVCoordinates;


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

  41.  * <p>One-axis ellipsoids is a good approximate model for most planet-size
  42.  * and larger natural bodies. It is the equilibrium shape reached by
  43.  * a fluid body under its own gravity field when it rotates. The symmetry
  44.  * axis is the rotation or polar axis.</p>

  46.  * @author Luc Maisonobe
  47.  */
  48. public class OneAxisEllipsoid extends Ellipsoid implements BodyShape {

  50.     /** Serializable UID. */
  51.     private static final long serialVersionUID = 20130518L;

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

  56.     /** Body frame related to body shape. */
  57.     private final Frame bodyFrame;

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

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

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

  68.     /** Eccentricity power 2. */
  69.     private final double e2;

  71.     /** 1 minus flatness. */
  72.     private final double g;

  74.     /** g * g. */
  75.     private final double g2;

  77.     /** Convergence limit. */
  78.     private double angularThreshold;

  80.     /** Simple constructor.
  81.      * <p>Standard values for Earth models can be found in the {@link org.orekit.utils.Constants Constants} class:</p>
  82.      * <table border="1" cellpadding="5" style="background-color:#f5f5dc;" summary="">
  83.      * <caption>Ellipsoid Models</caption>
  84.      * <tr style="background-color:#c9d5c9;"><th>model</th><th>a<sub>e</sub> (m)</th> <th>f</th></tr>
  85.      * <tr><td style="background-color:#c9d5c9;">GRS 80</td>
  86.      *     <td>{@link org.orekit.utils.Constants#GRS80_EARTH_EQUATORIAL_RADIUS Constants.GRS80_EARTH_EQUATORIAL_RADIUS}</td>
  87.      *     <td>{@link org.orekit.utils.Constants#GRS80_EARTH_FLATTENING Constants.GRS80_EARTH_FLATTENING}</td></tr>
  88.      * <tr><td style="background-color:#c9d5c9;">WGS84</td>
  89.      *     <td>{@link org.orekit.utils.Constants#WGS84_EARTH_EQUATORIAL_RADIUS Constants.WGS84_EARTH_EQUATORIAL_RADIUS}</td>
  90.      *     <td>{@link org.orekit.utils.Constants#WGS84_EARTH_FLATTENING Constants.WGS84_EARTH_FLATTENING}</td></tr>
  91.      * </table>
  92.      * @param ae equatorial radius
  93.      * @param f the flattening (f = (a-b)/a)
  94.      * @param bodyFrame body frame related to body shape
  95.      * @see org.orekit.frames.FramesFactory#getITRF(org.orekit.utils.IERSConventions, boolean)
  96.      */
  97.     public OneAxisEllipsoid(final double ae, final double f,
  98.                             final Frame bodyFrame) {
  99.         super(bodyFrame, ae, ae, ae * (1.0 - f));
  100.         this.f    = f;
  101.         this.ae2  = ae * ae;
  102.         this.e2   = f * (2.0 - f);
  103.         this.g    = 1.0 - f;
  104.         this.g2   = g * g;
  105.         this.ap2  = ae2 * g2;
  106.         setAngularThreshold(1.0e-12);
  107.         this.bodyFrame = bodyFrame;
  108.     }

  110.     /** Set the angular convergence threshold.
  111.      * <p>The angular threshold is used both to identify points close to
  112.      * the ellipse axes and as the convergence threshold used to
  113.      * stop the iterations in the {@link #transform(Vector3D, Frame,
  114.      * AbsoluteDate)} method.</p>
  115.      * <p>If this method is not called, the default value is set to
  116.      * 10<sup>-12</sup>.</p>
  117.      * @param angularThreshold angular convergence threshold (rad)
  118.      */
  119.     public void setAngularThreshold(final double angularThreshold) {
  120.         this.angularThreshold = angularThreshold;
  121.     }

  123.     /** Get the equatorial radius of the body.
  124.      * @return equatorial radius of the body (m)
  125.      */
  126.     public double getEquatorialRadius() {
  127.         return getA();
  128.     }

  130.     /** Get the flattening of the body: f = (a-b)/a.
  131.      * @return the flattening
  132.      */
  133.     public double getFlattening() {
  134.         return f;
  135.     }

  137.     /** {@inheritDoc} */
  138.     public Frame getBodyFrame() {
  139.         return bodyFrame;
  140.     }

  142.     /** Get the intersection point of a line with the surface of the body.
  143.      * <p>A line may have several intersection points with a closed
  144.      * surface (we consider the one point case as a degenerated two
  145.      * points case). The close parameter is used to select which of
  146.      * these points should be returned. The selected point is the one
  147.      * that is closest to the close point.</p>
  148.      * @param line test line (may intersect the body or not)
  149.      * @param close point used for intersections selection
  150.      * @param frame frame in which line is expressed
  151.      * @param date date of the line in given frame
  152.      * @return intersection point at altitude zero or null if the line does
  153.      * not intersect the surface
  154.      * @since 9.3
  155.      */
  156.     public Vector3D getCartesianIntersectionPoint(final Line line, final Vector3D close,
  157.                                                   final Frame frame, final AbsoluteDate date) {

  159.         // transform line and close to body frame
  160.         final Transform frameToBodyFrame = frame.getTransformTo(bodyFrame, date);
  161.         final Line lineInBodyFrame = frameToBodyFrame.transformLine(line);

  163.         // compute some miscellaneous variables
  164.         final Vector3D point    = lineInBodyFrame.getOrigin();
  165.         final double x          = point.getX();
  166.         final double y          = point.getY();
  167.         final double z          = point.getZ();
  168.         final double z2         = z * z;
  169.         final double r2         = x * x + y * y;

  171.         final Vector3D direction = lineInBodyFrame.getDirection();
  172.         final double dx         = direction.getX();
  173.         final double dy         = direction.getY();
  174.         final double dz         = direction.getZ();
  175.         final double cz2        = dx * dx + dy * dy;

  177.         // abscissa of the intersection as a root of a 2nd degree polynomial :
  178.         // a k^2 - 2 b k + c = 0
  179.         final double a  = 1.0 - e2 * cz2;
  180.         final double b  = -(g2 * (x * dx + y * dy) + z * dz);
  181.         final double c  = g2 * (r2 - ae2) + z2;
  182.         final double b2 = b * b;
  183.         final double ac = a * c;
  184.         if (b2 < ac) {
  185.             return null;
  186.         }
  187.         final double s  = FastMath.sqrt(b2 - ac);
  188.         final double k1 = (b < 0) ? (b - s) / a : c / (b + s);
  189.         final double k2 = c / (a * k1);

  191.         // select the right point
  192.         final Vector3D closeInBodyFrame = frameToBodyFrame.transformPosition(close);
  193.         final double   closeAbscissa    = lineInBodyFrame.getAbscissa(closeInBodyFrame);
  194.         final double k =
  195.             (FastMath.abs(k1 - closeAbscissa) < FastMath.abs(k2 - closeAbscissa)) ? k1 : k2;
  196.         return lineInBodyFrame.pointAt(k);

  198.     }

  200.     /** {@inheritDoc} */
  201.     public GeodeticPoint getIntersectionPoint(final Line line, final Vector3D close,
  202.                                               final Frame frame, final AbsoluteDate date) {

  204.         final Vector3D intersection = getCartesianIntersectionPoint(line, close, frame, date);
  205.         if (intersection == null) {
  206.             return null;
  207.         }
  208.         final double ix = intersection.getX();
  209.         final double iy = intersection.getY();
  210.         final double iz = intersection.getZ();

  212.         final double lambda = FastMath.atan2(iy, ix);
  213.         final double phi    = FastMath.atan2(iz, g2 * FastMath.sqrt(ix * ix + iy * iy));
  214.         return new GeodeticPoint(phi, lambda, 0.0);

  216.     }

  218.     /** Get the intersection point of a line with the surface of the body.
  219.      * <p>A line may have several intersection points with a closed
  220.      * surface (we consider the one point case as a degenerated two
  221.      * points case). The close parameter is used to select which of
  222.      * these points should be returned. The selected point is the one
  223.      * that is closest to the close point.</p>
  224.      * @param line test line (may intersect the body or not)
  225.      * @param close point used for intersections selection
  226.      * @param frame frame in which line is expressed
  227.      * @param date date of the line in given frame
  228.      * @param <T> type of the field elements
  229.      * @return intersection point at altitude zero or null if the line does
  230.      * not intersect the surface
  231.      * @since 9.3
  232.      */
  233.     public <T extends RealFieldElement<T>> FieldVector3D<T> getCartesianIntersectionPoint(final FieldLine<T> line,
  234.                                                                                           final FieldVector3D<T> close,
  235.                                                                                           final Frame frame,
  236.                                                                                           final FieldAbsoluteDate<T> date) {

  238.         // transform line and close to body frame
  239.         final FieldTransform<T> frameToBodyFrame = frame.getTransformTo(bodyFrame, date);
  240.         final FieldLine<T>      lineInBodyFrame  = frameToBodyFrame.transformLine(line);

  242.         // compute some miscellaneous variables
  243.         final FieldVector3D<T> point = lineInBodyFrame.getOrigin();
  244.         final T x  = point.getX();
  245.         final T y  = point.getY();
  246.         final T z  = point.getZ();
  247.         final T z2 = z.multiply(z);
  248.         final T r2 = x.multiply(x).add(y.multiply(y));

  250.         final FieldVector3D<T> direction = lineInBodyFrame.getDirection();
  251.         final T dx  = direction.getX();
  252.         final T dy  = direction.getY();
  253.         final T dz  = direction.getZ();
  254.         final T cz2 = dx.multiply(dx).add(dy.multiply(dy));

  256.         // abscissa of the intersection as a root of a 2nd degree polynomial :
  257.         // a k^2 - 2 b k + c = 0
  258.         final T a  = cz2.multiply(e2).subtract(1.0).negate();
  259.         final T b  = x.multiply(dx).add(y.multiply(dy)).multiply(g2).add(z.multiply(dz)).negate();
  260.         final T c  = r2.subtract(ae2).multiply(g2).add(z2);
  261.         final T b2 = b.multiply(b);
  262.         final T ac = a.multiply(c);
  263.         if (b2.getReal() < ac.getReal()) {
  264.             return null;
  265.         }
  266.         final T s  = b2.subtract(ac).sqrt();
  267.         final T k1 = (b.getReal() < 0) ? b.subtract(s).divide(a) : c.divide(b.add(s));
  268.         final T k2 = c.divide(a.multiply(k1));

  270.         // select the right point
  271.         final FieldVector3D<T>  closeInBodyFrame = frameToBodyFrame.transformPosition(close);
  272.         final T                 closeAbscissa    = lineInBodyFrame.getAbscissa(closeInBodyFrame);
  273.         final T k = (FastMath.abs(k1.getReal() - closeAbscissa.getReal()) < FastMath.abs(k2.getReal() - closeAbscissa.getReal())) ?
  274.                     k1 : k2;
  275.         return lineInBodyFrame.pointAt(k);
  276.     }

  278.     /** {@inheritDoc} */
  279.     public <T extends RealFieldElement<T>> FieldGeodeticPoint<T> getIntersectionPoint(final FieldLine<T> line,
  280.                                                                                       final FieldVector3D<T> close,
  281.                                                                                       final Frame frame,
  282.                                                                                       final FieldAbsoluteDate<T> date) {

  284.         final FieldVector3D<T> intersection = getCartesianIntersectionPoint(line, close, frame, date);
  285.         if (intersection == null) {
  286.             return null;
  287.         }
  288.         final T ix = intersection.getX();
  289.         final T iy = intersection.getY();
  290.         final T iz = intersection.getZ();

  292.         final T lambda = iy.atan2(ix);
  293.         final T phi    = iz.atan2(ix.multiply(ix).add(iy.multiply(iy)).sqrt().multiply(g2));
  294.         return new FieldGeodeticPoint<>(phi, lambda, phi.getField().getZero());

  296.     }

  298.     /** {@inheritDoc} */
  299.     public Vector3D transform(final GeodeticPoint point) {
  300.         final double longitude = point.getLongitude();
  301.         final double cLambda   = FastMath.cos(longitude);
  302.         final double sLambda   = FastMath.sin(longitude);
  303.         final double latitude  = point.getLatitude();
  304.         final double cPhi      = FastMath.cos(latitude);
  305.         final double sPhi      = FastMath.sin(latitude);
  306.         final double h         = point.getAltitude();
  307.         final double n         = getA() / FastMath.sqrt(1.0 - e2 * sPhi * sPhi);
  308.         final double r         = (n + h) * cPhi;
  309.         return new Vector3D(r * cLambda, r * sLambda, (g2 * n + h) * sPhi);
  310.     }

  312.     /** {@inheritDoc} */
  313.     public <T extends RealFieldElement<T>> FieldVector3D<T> transform(final FieldGeodeticPoint<T> point) {

  315.         final T latitude  = point.getLatitude();
  316.         final T longitude = point.getLongitude();
  317.         final T altitude  = point.getAltitude();

  319.         final T cLambda = longitude.cos();
  320.         final T sLambda = longitude.sin();
  321.         final T cPhi    = latitude.cos();
  322.         final T sPhi    = latitude.sin();
  323.         final T n       = sPhi.multiply(sPhi).multiply(e2).subtract(1.0).negate().sqrt().reciprocal().multiply(getA());
  324.         final T r       = n.add(altitude).multiply(cPhi);

  326.         return new FieldVector3D<>(r.multiply(cLambda),
  327.                                    r.multiply(sLambda),
  328.                                    sPhi.multiply(altitude.add(n.multiply(g2))));
  329.     }

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

  334.         // transform point to body frame
  335.         final Transform  toBody    = frame.getTransformTo(bodyFrame, date);
  336.         final Vector3D   p         = toBody.transformPosition(point);
  337.         final double     z         = p.getZ();
  338.         final double     r         = FastMath.hypot(p.getX(), p.getY());

  340.         // set up the 2D meridian ellipse
  341.         final Ellipse meridian = new Ellipse(Vector3D.ZERO,
  342.                                              new Vector3D(p.getX() / r, p.getY() / r, 0),
  343.                                              Vector3D.PLUS_K,
  344.                                              getA(), getC(), bodyFrame);

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

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

  352.     }

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

  357.         // transform point to body frame
  358.         final Transform                toBody        = frame.getTransformTo(bodyFrame, pv.getDate());
  359.         final TimeStampedPVCoordinates pvInBodyFrame = toBody.transformPVCoordinates(pv);
  360.         final Vector3D                 p             = pvInBodyFrame.getPosition();
  361.         final double                   r             = FastMath.hypot(p.getX(), p.getY());

  363.         // set up the 2D ellipse corresponding to first principal curvature along meridian
  364.         final Vector3D meridian = new Vector3D(p.getX() / r, p.getY() / r, 0);
  365.         final Ellipse firstPrincipalCurvature =
  366.                 new Ellipse(Vector3D.ZERO, meridian, Vector3D.PLUS_K, getA(), getC(), bodyFrame);

  368.         // project coordinates in the meridian plane
  369.         final TimeStampedPVCoordinates gpFirst = firstPrincipalCurvature.projectToEllipse(pvInBodyFrame);
  370.         final Vector3D                 gpP     = gpFirst.getPosition();
  371.         final double                   gr      = MathArrays.linearCombination(gpP.getX(), meridian.getX(),
  372.                                                                               gpP.getY(), meridian.getY());
  373.         final double                   gz      = gpP.getZ();

  375.         // topocentric frame
  376.         final Vector3D east   = new Vector3D(-meridian.getY(), meridian.getX(), 0);
  377.         final Vector3D zenith = new Vector3D(gr * getC() / getA(), meridian, gz * getA() / getC(), Vector3D.PLUS_K).normalize();
  378.         final Vector3D north  = Vector3D.crossProduct(zenith, east);

  380.         // set up the ellipse corresponding to second principal curvature in the zenith/east plane
  381.         final Ellipse secondPrincipalCurvature  = getPlaneSection(gpP, north);
  382.         final TimeStampedPVCoordinates gpSecond = secondPrincipalCurvature.projectToEllipse(pvInBodyFrame);

  384.         final Vector3D gpV = gpFirst.getVelocity().add(gpSecond.getVelocity());
  385.         final Vector3D gpA = gpFirst.getAcceleration().add(gpSecond.getAcceleration());

  387.         // moving projected point
  388.         final TimeStampedPVCoordinates groundPV =
  389.                 new TimeStampedPVCoordinates(pv.getDate(), gpP, gpV, gpA);

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

  394.     }

  396.     /** {@inheritDoc}
  397.      * <p>
  398.      * This method is based on Toshio Fukushima's algorithm which uses Halley's method.
  399.      * <a href="https://www.researchgate.net/publication/227215135_Transformation_from_Cartesian_to_Geodetic_Coordinates_Accelerated_by_Halley's_Method">
  400.      * transformation from Cartesian to Geodetic Coordinates Accelerated by Halley's Method</a>,
  401.      * Toshio Fukushima, Journal of Geodesy 9(12):689-693, February 2006
  402.      * </p>
  403.      * <p>
  404.      * Some changes have been added to the original method:
  405.      * <ul>
  406.      *   <li>in order to handle more accurately corner cases near the pole</li>
  407.      *   <li>in order to handle properly corner cases near the equatorial plane, even far inside the ellipsoid</li>
  408.      *   <li>in order to handle very flat ellipsoids</li>
  409.      * </ul>
  410.      * </p>
  411.      */
  412.     public GeodeticPoint transform(final Vector3D point, final Frame frame, final AbsoluteDate date) {

  414.         // transform point to body frame
  415.         final Vector3D pointInBodyFrame = frame.getTransformTo(bodyFrame, date).transformPosition(point);
  416.         final double   r2               = pointInBodyFrame.getX() * pointInBodyFrame.getX() +
  417.                                           pointInBodyFrame.getY() * pointInBodyFrame.getY();
  418.         final double   r                = FastMath.sqrt(r2);
  419.         final double   z                = pointInBodyFrame.getZ();

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

  423.         double h;
  424.         double phi;
  425.         if (r <= ANGULAR_THRESHOLD * FastMath.abs(z)) {
  426.             // the point is almost on the polar axis, approximate the ellipsoid with
  427.             // the osculating sphere whose center is at evolute cusp along polar axis
  428.             final double osculatingRadius = ae2 / getC();
  429.             final double evoluteCuspZ     = FastMath.copySign(getA() * e2 / g, -z);
  430.             final double deltaZ           = z - evoluteCuspZ;
  431.             // we use π/2 - atan(r/Δz) instead of atan(Δz/r) for accuracy purposes, as r is much smaller than Δz
  432.             phi = FastMath.copySign(0.5 * FastMath.PI - FastMath.atan(r / FastMath.abs(deltaZ)), deltaZ);
  433.             h   = FastMath.hypot(deltaZ, r) - osculatingRadius;
  434.         } else if (FastMath.abs(z) <= ANGULAR_THRESHOLD * r) {
  435.             // the point is almost on the major axis

  437.             final double osculatingRadius = ap2 / getA();
  438.             final double evoluteCuspR     = getA() * e2;
  439.             final double deltaR           = r - evoluteCuspR;
  440.             if (deltaR >= 0) {
  441.                 // the point is outside of the ellipse evolute, approximate the ellipse
  442.                 // with the osculating circle whose center is at evolute cusp along major axis
  443.                 phi = (deltaR == 0) ? 0.0 : FastMath.atan(z / deltaR);
  444.                 h   = FastMath.hypot(deltaR, z) - osculatingRadius;
  445.             } else {
  446.                 // the point is on the part of the major axis within ellipse evolute
  447.                 // we can compute the closest ellipse point analytically, and it is NOT near the equator
  448.                 final double rClose = r / e2;
  449.                 final double zClose = FastMath.copySign(g * FastMath.sqrt(ae2 - rClose * rClose), z);
  450.                 phi = FastMath.atan((zClose - z) / (rClose - r));
  451.                 h   = -FastMath.hypot(r - rClose, z - zClose);
  452.             }

  454.         } else {
  455.             // use Toshio Fukushima method, with several iterations
  456.             final double epsPhi = 1.0e-15;
  457.             final double epsH   = 1.0e-14 * FastMath.max(getA(), FastMath.sqrt(r2 + z * z));
  458.             final double c     = getA() * e2;
  459.             final double absZ  = FastMath.abs(z);
  460.             final double zc    = g * absZ;
  461.             double sn  = absZ;
  462.             double sn2 = sn * sn;
  463.             double cn  = g * r;
  464.             double cn2 = cn * cn;
  465.             double an2 = cn2 + sn2;
  466.             double an  = FastMath.sqrt(an2);
  467.             double bn  = 0;
  468.             phi = Double.POSITIVE_INFINITY;
  469.             h   = Double.POSITIVE_INFINITY;
  470.             for (int i = 0; i < 10; ++i) { // this usually converges in 2 iterations
  471.                 final double oldSn  = sn;
  472.                 final double oldCn  = cn;
  473.                 final double oldPhi = phi;
  474.                 final double oldH   = h;
  475.                 final double an3    = an2 * an;
  476.                 final double csncn  = c * sn * cn;
  477.                 bn    = 1.5 * csncn * ((r * sn - zc * cn) * an - csncn);
  478.                 sn    = (zc * an3 + c * sn2 * sn) * an3 - bn * sn;
  479.                 cn    = (r  * an3 - c * cn2 * cn) * an3 - bn * cn;
  480.                 if (sn * oldSn < 0 || cn < 0) {
  481.                     // the Halley iteration went too far, we restrict it and iterate again
  482.                     while (sn * oldSn < 0 || cn < 0) {
  483.                         sn = (sn + oldSn) / 2;
  484.                         cn = (cn + oldCn) / 2;
  485.                     }
  486.                 } else {

  488.                     // rescale components to avoid overflow when several iterations are used
  489.                     final int exp = (FastMath.getExponent(sn) + FastMath.getExponent(cn)) / 2;
  490.                     sn = FastMath.scalb(sn, -exp);
  491.                     cn = FastMath.scalb(cn, -exp);

  493.                     sn2 = sn * sn;
  494.                     cn2 = cn * cn;
  495.                     an2 = cn2 + sn2;
  496.                     an  = FastMath.sqrt(an2);

  498.                     final double cc = g * cn;
  499.                     h = (r * cc + absZ * sn - getA() * g * an) / FastMath.sqrt(an2 - e2 * cn2);
  500.                     if (FastMath.abs(oldH   - h)   < epsH) {
  501.                         phi = FastMath.copySign(FastMath.atan(sn / cc), z);
  502.                         if (FastMath.abs(oldPhi - phi) < epsPhi) {
  503.                             break;
  504.                         }
  505.                     }

  507.                 }

  509.             }
  510.         }

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

  514.     }

  516.     /** {@inheritDoc}
  517.      * <p>
  518.      * This method is based on Toshio Fukushima's algorithm which uses Halley's method.
  519.      * <a href="https://www.researchgate.net/publication/227215135_Transformation_from_Cartesian_to_Geodetic_Coordinates_Accelerated_by_Halley's_Method">
  520.      * transformation from Cartesian to Geodetic Coordinates Accelerated by Halley's Method</a>,
  521.      * Toshio Fukushima, Journal of Geodesy 9(12):689-693, February 2006
  522.      * </p>
  523.      * <p>
  524.      * Some changes have been added to the original method:
  525.      * <ul>
  526.      *   <li>in order to handle more accurately corner cases near the pole</li>
  527.      *   <li>in order to handle properly corner cases near the equatorial plane, even far inside the ellipsoid</li>
  528.      *   <li>in order to handle very flat ellipsoids</li>
  529.      * </ul>
  530.      * </p>
  531.      */
  532.     public <T extends RealFieldElement<T>> FieldGeodeticPoint<T> transform(final FieldVector3D<T> point,
  533.                                                                            final Frame frame,
  534.                                                                            final FieldAbsoluteDate<T> date) {

  536.         // transform point to body frame
  537.         final FieldVector3D<T> pointInBodyFrame = frame.getTransformTo(bodyFrame, date).transformPosition(point);
  538.         final T   r2                            = pointInBodyFrame.getX().multiply(pointInBodyFrame.getX()).
  539.                                               add(pointInBodyFrame.getY().multiply(pointInBodyFrame.getY()));
  540.         final T   r                             = r2.sqrt();
  541.         final T   z                             = pointInBodyFrame.getZ();

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

  545.         T h;
  546.         T phi;
  547.         if (r.getReal() <= ANGULAR_THRESHOLD * FastMath.abs(z.getReal())) {
  548.             // the point is almost on the polar axis, approximate the ellipsoid with
  549.             // the osculating sphere whose center is at evolute cusp along polar axis
  550.             final double osculatingRadius = ae2 / getC();
  551.             final double evoluteCuspZ     = FastMath.copySign(getA() * e2 / g, -z.getReal());
  552.             final T      deltaZ           = z.subtract(evoluteCuspZ);
  553.             // we use π/2 - atan(r/Δz) instead of atan(Δz/r) for accuracy purposes, as r is much smaller than Δz
  554.             phi = r.divide(deltaZ.abs()).atan().negate().add(0.5 * FastMath.PI).copySign(deltaZ);
  555.             h   = deltaZ.hypot(r).subtract(osculatingRadius);
  556.         } else if (FastMath.abs(z.getReal()) <= ANGULAR_THRESHOLD * r.getReal()) {
  557.             // the point is almost on the major axis

  559.             final double osculatingRadius = ap2 / getA();
  560.             final double evoluteCuspR     = getA() * e2;
  561.             final T      deltaR           = r.subtract(evoluteCuspR);
  562.             if (deltaR.getReal() >= 0) {
  563.                 // the point is outside of the ellipse evolute, approximate the ellipse
  564.                 // with the osculating circle whose center is at evolute cusp along major axis
  565.                 phi = (deltaR.getReal() == 0) ? z.getField().getZero() : z.divide(deltaR).atan();
  566.                 h   = deltaR.hypot(z).subtract(osculatingRadius);
  567.             } else {
  568.                 // the point is on the part of the major axis within ellipse evolute
  569.                 // we can compute the closest ellipse point analytically, and it is NOT near the equator
  570.                 final T rClose = r.divide(e2);
  571.                 final T zClose = rClose.multiply(rClose).negate().add(ae2).sqrt().multiply(g).copySign(z);
  572.                 phi = zClose.subtract(z).divide(rClose.subtract(r)).atan();
  573.                 h   = r.subtract(rClose).hypot(z.subtract(zClose)).negate();
  574.             }

  576.         } else {
  577.             // use Toshio Fukushima method, with several iterations
  578.             final double epsPhi = 1.0e-15;
  579.             final double epsH   = 1.0e-14 * getA();
  580.             final double c      = getA() * e2;
  581.             final T      absZ   = z.abs();
  582.             final T      zc     = absZ.multiply(g);
  583.             T            sn     = absZ;
  584.             T            sn2    = sn.multiply(sn);
  585.             T            cn     = r.multiply(g);
  586.             T            cn2    = cn.multiply(cn);
  587.             T            an2    = cn2.add(sn2);
  588.             T            an     = an2.sqrt();
  589.             T            bn     = an.getField().getZero();
  590.             phi = an.getField().getZero().add(Double.POSITIVE_INFINITY);
  591.             h   = an.getField().getZero().add(Double.POSITIVE_INFINITY);
  592.             for (int i = 0; i < 10; ++i) { // this usually converges in 2 iterations
  593.                 final T oldSn  = sn;
  594.                 final T oldCn  = cn;
  595.                 final T oldPhi = phi;
  596.                 final T oldH   = h;
  597.                 final T an3    = an2.multiply(an);
  598.                 final T csncn  = sn.multiply(cn).multiply(c);
  599.                 bn    = csncn.multiply(1.5).multiply((r.multiply(sn).subtract(zc.multiply(cn))).multiply(an).subtract(csncn));
  600.                 sn    = zc.multiply(an3).add(sn2.multiply(sn).multiply(c)).multiply(an3).subtract(bn.multiply(sn));
  601.                 cn    = r.multiply(an3).subtract(cn2.multiply(cn).multiply(c)).multiply(an3).subtract(bn.multiply(cn));
  602.                 if (sn.getReal() * oldSn.getReal() < 0 || cn.getReal() < 0) {
  603.                     // the Halley iteration went too far, we restrict it and iterate again
  604.                     while (sn.getReal() * oldSn.getReal() < 0 || cn.getReal() < 0) {
  605.                         sn = sn.add(oldSn).multiply(0.5);
  606.                         cn = cn.add(oldCn).multiply(0.5);
  607.                     }
  608.                 } else {

  610.                     // rescale components to avoid overflow when several iterations are used
  611.                     final int exp = (FastMath.getExponent(sn.getReal()) + FastMath.getExponent(cn.getReal())) / 2;
  612.                     sn = sn.scalb(-exp);
  613.                     cn = cn.scalb(-exp);

  615.                     sn2 = sn.multiply(sn);
  616.                     cn2 = cn.multiply(cn);
  617.                     an2 = cn2.add(sn2);
  618.                     an  = an2.sqrt();

  620.                     final T cc = cn.multiply(g);
  621.                     h = r.multiply(cc).add(absZ.multiply(sn)).subtract(an.multiply(getA() * g)).divide(an2.subtract(cn2.multiply(e2)).sqrt());
  622.                     if (FastMath.abs(oldH.getReal()  - h.getReal())   < epsH) {
  623.                         phi = sn.divide(cc).atan().copySign(z);
  624.                         if (FastMath.abs(oldPhi.getReal() - phi.getReal()) < epsPhi) {
  625.                             break;
  626.                         }
  627.                     }

  629.                 }

  631.             }
  632.         }

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

  636.     }

  638.     /** Transform a Cartesian point to a surface-relative point.
  639.      * @param point Cartesian point
  640.      * @param frame frame in which Cartesian point is expressed
  641.      * @param date date of the computation (used for frames conversions)
  642.      * @return point at the same location but as a surface-relative point,
  643.      * using time as the single derivation parameter
  644.      */
  645.     public FieldGeodeticPoint<DerivativeStructure> transform(final PVCoordinates point,
  646.                                                              final Frame frame, final AbsoluteDate date) {

  648.         // transform point to body frame
  649.         final Transform toBody = frame.getTransformTo(bodyFrame, date);
  650.         final PVCoordinates pointInBodyFrame = toBody.transformPVCoordinates(point);
  651.         final FieldVector3D<DerivativeStructure> p = pointInBodyFrame.toDerivativeStructureVector(2);
  652.         final DerivativeStructure   pr2 = p.getX().multiply(p.getX()).add(p.getY().multiply(p.getY()));
  653.         final DerivativeStructure   pr  = pr2.sqrt();
  654.         final DerivativeStructure   pz  = p.getZ();

  656.         // project point on the ellipsoid surface
  657.         final TimeStampedPVCoordinates groundPoint = projectToGround(new TimeStampedPVCoordinates(date, pointInBodyFrame),
  658.                                                                      bodyFrame);
  659.         final FieldVector3D<DerivativeStructure> gp = groundPoint.toDerivativeStructureVector(2);
  660.         final DerivativeStructure   gpr2 = gp.getX().multiply(gp.getX()).add(gp.getY().multiply(gp.getY()));
  661.         final DerivativeStructure   gpr  = gpr2.sqrt();
  662.         final DerivativeStructure   gpz  = gp.getZ();

  664.         // relative position of test point with respect to its ellipse sub-point
  665.         final DerivativeStructure dr  = pr.subtract(gpr);
  666.         final DerivativeStructure dz  = pz.subtract(gpz);
  667.         final double insideIfNegative = g2 * (pr2.getReal() - ae2) + pz.getReal() * pz.getReal();

  669.         return new FieldGeodeticPoint<>(DerivativeStructure.atan2(gpz, gpr.multiply(g2)),
  670.                                                                   DerivativeStructure.atan2(p.getY(), p.getX()),
  671.                                                                   DerivativeStructure.hypot(dr, dz).copySign(insideIfNegative));
  672.     }

  674.     /** Replace the instance with a data transfer object for serialization.
  675.      * <p>
  676.      * This intermediate class serializes the files supported names, the
  677.      * ephemeris type and the body name.
  678.      * </p>
  679.      * @return data transfer object that will be serialized
  680.      */
  681.     private Object writeReplace() {
  682.         return new DataTransferObject(getA(), f, bodyFrame, angularThreshold);
  683.     }

  685.     /** Internal class used only for serialization. */
  686.     private static class DataTransferObject implements Serializable {

  688.         /** Serializable UID. */
  689.         private static final long serialVersionUID = 20130518L;

  691.         /** Equatorial radius. */
  692.         private final double ae;

  694.         /** Flattening. */
  695.         private final double f;

  697.         /** Body frame related to body shape. */
  698.         private final Frame bodyFrame;

  700.         /** Convergence limit. */
  701.         private final double angularThreshold;

  703.         /** Simple constructor.
  704.          * @param ae equatorial radius
  705.          * @param f the flattening (f = (a-b)/a)
  706.          * @param bodyFrame body frame related to body shape
  707.          * @param angularThreshold convergence limit
  708.          */
  709.         DataTransferObject(final double ae, final double f,
  710.                                   final Frame bodyFrame, final double angularThreshold) {
  711.             this.ae               = ae;
  712.             this.f                = f;
  713.             this.bodyFrame        = bodyFrame;
  714.             this.angularThreshold = angularThreshold;
  715.         }

  717.         /** Replace the deserialized data transfer object with a
  718.          * {@link JPLCelestialBody}.
  719.          * @return replacement {@link JPLCelestialBody}
  720.          */
  721.         private Object readResolve() {
  722.             final OneAxisEllipsoid ellipsoid = new OneAxisEllipsoid(ae, f, bodyFrame);
  723.             ellipsoid.setAngularThreshold(angularThreshold);
  724.             return ellipsoid;
  725.         }

  727.     }

  729. }
Created with JaCoCo 0.8.2.201808211720