Merge branch 'geodeticpoint-interpolation' into develop

src/main/java/org/orekit/bodies/Loxodrome.java

+/* Copyright 2022 Joseph Reed
+ * Licensed to CS GROUP (CS) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * Joseph Reed licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package org.orekit.bodies;
+
+import org.hipparchus.util.FastMath;
+import org.hipparchus.util.MathUtils;
+
+/** Perform calculations on a loxodrome (commonly, a rhumb line) on an ellipsoid.
+ * <p>
+ * A <a href="https://en.wikipedia.org/wiki/Rhumb_line">loxodrome or rhumb line</a>
+ * is an arc on an ellipsoid's surface that intersects every meridian at the same angle.
+ *
+ * @author Joe Reed
+ * @since 11.3
+ */
+public class Loxodrome {
+
+    /** Threshold for cos angles being equal. */
+    private static final double COS_ANGLE_THRESHOLD = 1e-6;
+
+    /** Threshold for when distances are close enough to zero. */
+    private static final double DISTANCE_THRESHOLD = 1e-9;
+
+    /** Reference point for the loxodrome. */
+    private final GeodeticPoint point;
+
+    /** Azimuth-off-north angle of the loxodrome. */
+    private final double azimuth;
+
+    /** Reference body. */
+    private final OneAxisEllipsoid body;
+
+    /** Altitude above the body. */
+    private final double altitude;
+
+    /** Constructor building a loxodrome from an initial point and an azimuth-off-local-north heading.
+     *
+     * This method is an equivalent to {@code new Loxodrome(point, azimuth, body, point.getAltitude())}
+     *
+     * @param point the initial loxodrome point
+     * @param azimuth the heading, clockwise angle from north (radians, {@code [0,2pi]})
+     * @param body ellipsoid body on which the loxodrome is defined
+     */
+    public Loxodrome(final GeodeticPoint point, final double azimuth, final OneAxisEllipsoid body) {
+        this(point, azimuth, body, point.getAltitude());
+    }
+
+    /** Constructor building a loxodrome from an initial point and an azimuth-off-local-north heading.
+     *
+     * @param point the initial loxodrome point
+     * @param azimuth the heading, clockwise angle from north (radians, {@code [0,2pi]})
+     * @param body ellipsoid body on which the loxodrome is defined
+     * @param altitude altitude above the reference body
+     */
+    public Loxodrome(final GeodeticPoint point, final double azimuth, final OneAxisEllipsoid body,
+                     final double altitude) {
+        this.point    = point;
+        this.azimuth  = azimuth;
+        this.body     = body;
+        this.altitude = altitude;
+    }
+
+    /** Get the geodetic point defining the loxodrome.
+     * @return the geodetic point defining the loxodrome
+     */
+    public GeodeticPoint getPoint() {
+        return this.point;
+    }
+
+    /** Get the azimuth.
+     * @return the azimuth
+     */
+    public double getAzimuth() {
+        return this.azimuth;
+    }
+
+    /** Get the body on which the loxodrome is defined.
+     * @return the body on which the loxodrome is defined
+     */
+    public OneAxisEllipsoid getBody() {
+        return this.body;
+    }
+
+    /** Get the altitude above the reference body.
+     * @return the altitude above the reference body
+     */
+    public double getAltitude() {
+        return this.altitude;
+    }
+
+    /** Calculate the point at the specified distance from the origin point along the loxodrome.
+     *
+     * A positive distance follows the line in the azumuth direction (i.e. northward for arcs with azimuth
+     * angles {@code [3pi/2, 2pi]} or {@code [0, pi/2]}). Negative distances travel in the opposite direction along
+     * the rhumb line.
+     *
+     * Distance is computed at the altitude of the origin point.
+     *
+     * @param distance the distance to travel (meters)
+     * @return the point at the specified distance from the origin
+     */
+    public GeodeticPoint pointAtDistance(final double distance) {
+        if (FastMath.abs(distance) < DISTANCE_THRESHOLD) {
+            return this.point;
+        }
+
+        // compute the e sin(lat)^2
+        final double sinLat = FastMath.sin(point.getLatitude());
+        final double eccSinLatSq = body.getEccentricitySquared() * sinLat * sinLat;
+
+        // compute intermediate values
+        final double t1 = 1. - body.getEccentricitySquared();
+        final double t2 = 1. - eccSinLatSq;
+        final double t3 = FastMath.sqrt(t2);
+
+        final double semiMajorAxis = getBody().getEquatorialRadius() + getAltitude();
+
+        final double meridianCurve = (semiMajorAxis * t1) / (t2 * t3);
+
+        final double cosAzimuth = FastMath.cos(azimuth);
+
+        final double lat;
+        final double lon;
+        if (FastMath.abs(cosAzimuth) < COS_ANGLE_THRESHOLD) {
+            lat = point.getLatitude();
+            lon = point.getLongitude() + ((distance * FastMath.sin(azimuth) * t3) / semiMajorAxis * FastMath.cos(point.getLatitude()));
+        }
+        else {
+            final double eccSq34 = 0.75 * body.getEccentricitySquared();
+            final double halfEccSq34 = eccSq34 / 2.;
+            final double t4 = meridianCurve / (t1 * semiMajorAxis);
+
+            final double latPrime = point.getLatitude() + distance * cosAzimuth / meridianCurve;
+            final double latOffset = t4 * (
+                ((1. - eccSq34) * (latPrime - point.getLatitude())) +
+                        (halfEccSq34 * (FastMath.sin(2. * latPrime) - FastMath.sin(2. * point.getLatitude()))));
+
+            lat = fixLatitude(point.getLatitude() + latOffset);
+
+            final double lonOffset = FastMath.tan(azimuth) * (body.geodeticToIsometricLatitude(lat) - body.geodeticToIsometricLatitude(point.getLatitude()));
+            lon = point.getLongitude() + lonOffset;
+        }
+
+        return new GeodeticPoint(lat, lon, getAltitude());
+    }
+
+    /** Adjust the latitude if necessary, ensuring it's always between -pi/2 and +pi/2.
+     *
+     * @param lat the latitude value
+     * @return the latitude, within {@code [-pi/2,+pi/2]}
+     */
+    static double fixLatitude(final double lat) {
+        if (lat < -MathUtils.SEMI_PI) {
+            return -MathUtils.SEMI_PI;
+        }
+        else if (lat > MathUtils.SEMI_PI) {
+            return MathUtils.SEMI_PI;
+        }
+        else {
+            return lat;
+        }
+    }
+}

src/main/java/org/orekit/bodies/LoxodromeArc.java

+/* Copyright 2022 Joseph Reed
+ * Licensed to CS GROUP (CS) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * Joseph Reed licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package org.orekit.bodies;
+
+import org.hipparchus.util.FastMath;
+import org.hipparchus.util.MathUtils;
+
+/** Loxodrome defined by a start and ending point.
+ *
+ * @author Joe Reed
+ * @since 11.3
+ */
+public class LoxodromeArc extends Loxodrome {
+
+    /** Threshold for considering latitudes equal, in radians. */
+    private static final double LATITUDE_THRESHOLD = 1e-6;
+
+    /** Maximum number of iterations used when computing distance between points. */
+    private static final int MAX_ITER = 50;
+
+    /** Ending point of the arc. */
+    private final GeodeticPoint endPoint;
+
+    /** Delta longitude, cached, radians. */
+    private final double deltaLon;
+
+    /** Cached arc distance, meters. */
+    private double distance = -1;
+
+    /** Class constructor where the arc's altitude is the average of the initial and final points.
+     * @param point the starting point
+     * @param endPoint the ending point
+     * @param body the body on which the loxodrome is defined
+     */
+    public LoxodromeArc(final GeodeticPoint point, final GeodeticPoint endPoint, final OneAxisEllipsoid body) {
+        this(point, endPoint, body, (point.getAltitude() + endPoint.getAltitude()) / 2.);
+    }
+
+    /** Class constructor.
+     * @param point the starting point
+     * @param endPoint the ending point
+     * @param body the body on which the loxodrome is defined
+     * @param altitude the altitude above the reference body (meters)
+     */
+    public LoxodromeArc(final GeodeticPoint point, final GeodeticPoint endPoint, final OneAxisEllipsoid body,
+                        final double altitude) {
+        super(point, body.azimuthBetweenPoints(point, endPoint), body, altitude);
+        this.endPoint = endPoint;
+        this.deltaLon = MathUtils.normalizeAngle(endPoint.getLongitude(), point.getLongitude()) -
+                        point.getLongitude();
+    }
+
+    /** Get the final point of the arc.
+     *
+     * @return the ending point of the arc
+     */
+    public GeodeticPoint getFinalPoint() {
+        return this.endPoint;
+    }
+
+    /** Compute the distance of the arc along the surface of the ellipsoid.
+     *
+     * @return the distance (meters)
+     */
+    public double getDistance() {
+        if (distance >= 0) {
+            return distance;
+        }
+
+        // compute the e sin(lat)^2
+        final double ptLat  = getPoint().getLatitude();
+        final double sinLat = FastMath.sin(ptLat);
+        final double eccSinLatSq = getBody().getEccentricitySquared() * sinLat * sinLat;
+
+        // compute intermediate values
+        final double t1 = 1. - getBody().getEccentricitySquared();
+        final double t2 = 1. - eccSinLatSq;
+        final double t3 = FastMath.sqrt(t2);
+
+        final double semiMajorAxis = getBody().getEquatorialRadius() + getAltitude();
+
+        final double meridianCurve = (semiMajorAxis * t1) / (t2 * t3);
+
+        if (FastMath.abs(endPoint.getLatitude() - ptLat) < LATITUDE_THRESHOLD) {
+            distance = (semiMajorAxis / t3) * FastMath.abs(FastMath.cos(ptLat) * deltaLon);
+        }
+        else {
+            final double eccSq34 = 0.75 * getBody().getEccentricitySquared();
+            final double halfEccSq34 = eccSq34 / 2.;
+            final double t6 = 1. / (1. - eccSq34);
+            final double t7 = t1 * semiMajorAxis / meridianCurve;
+
+            final double t8 = ptLat + t6 *
+                (t7 * (endPoint.getLatitude() - ptLat) + halfEccSq34 * FastMath.sin(ptLat * 2.));
+            final double t9 = halfEccSq34 * t6;
+
+            double guess = 0;
+            double lat = endPoint.getLatitude();
+            for (int i = 0; i < MAX_ITER; i++) {
+                guess = lat;
+                lat = t8 - t9 * FastMath.sin(2. * guess);
+
+                if (FastMath.abs(lat - guess) < LATITUDE_THRESHOLD) {
+                    break;
+                }
+            }
+
+            final double azimuth = FastMath.atan2(deltaLon,
+                getBody().geodeticToIsometricLatitude(lat) - getBody().geodeticToIsometricLatitude(ptLat));
+            distance = meridianCurve * FastMath.abs((lat - ptLat) / FastMath.cos(azimuth));
+        }
+
+        return distance;
+    }
+
+    /** Calculate a point at a specific percentage along the arc.
+     *
+     * @param fraction the fraction along the arc to compute the point
+     * @return the point along the arc
+     */
+    public GeodeticPoint calculatePointAlongArc(final double fraction) {
+        if (fraction == 0.) {
+            return getPoint();
+        }
+        else if (fraction == 1.) {
+            return getFinalPoint();
+        }
+        else {
+            final double d = getDistance() * fraction;
+            return this.pointAtDistance(d);
+        }
+    }
+}

src/main/java/org/orekit/bodies/OneAxisEllipsoid.java

@@ -19,6 +19,7 @@ package org.orekit.bodies;
 import java.io.Serializable;
 
 import org.hipparchus.CalculusFieldElement;
+import org.hipparchus.Field;
 import org.hipparchus.analysis.differentiation.DerivativeStructure;
 import org.hipparchus.geometry.euclidean.threed.FieldLine;
 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
@@ -28,6 +29,7 @@ import org.hipparchus.geometry.euclidean.twod.Vector2D;
 import org.hipparchus.util.FastMath;
 import org.hipparchus.util.FieldSinCos;
 import org.hipparchus.util.MathArrays;
+import org.hipparchus.util.MathUtils;
 import org.hipparchus.util.SinCos;
 import org.orekit.frames.FieldTransform;
 import org.orekit.frames.Frame;
@@ -69,13 +71,16 @@ public class OneAxisEllipsoid extends Ellipsoid implements BodyShape {
     /** Flattening. */
     private final double f;
 
-    /** Eccentricity power 2. */
+    /** Eccentricity. */
+    private final double e;
+
+    /** Eccentricity squared. */
     private final double e2;
 
     /** 1 minus flatness. */
     private final double g;
 
-    /** g * g. */
+    /** g squared. */
     private final double g2;
 
     /** Convergence limit. */
@@ -113,6 +118,7 @@ public class OneAxisEllipsoid extends Ellipsoid implements BodyShape {
         this.f    = f;
         this.ae2  = ae * ae;
         this.e2   = f * (2.0 - f);
+        this.e    = FastMath.sqrt(e2);
         this.g    = 1.0 - f;
         this.g2   = g * g;
         this.ap2  = ae2 * g2;
@@ -147,6 +153,20 @@ public class OneAxisEllipsoid extends Ellipsoid implements BodyShape {
         return f;
     }
 
+    /** Get the first eccentricity squared of the ellipsoid: e^2 = f * (2.0 - f).
+     * @return the eccentricity squared
+     */
+    public double getEccentricitySquared() {
+        return e2;
+    }
+
+    /** Get the first eccentricity of the ellipsoid: e = sqrt(f * (2.0 - f)).
+     * @return the eccentricity
+     */
+    public double getEccentricity() {
+        return e;
+    }
+
     /** {@inheritDoc} */
     public Frame getBodyFrame() {
         return bodyFrame;
@@ -684,6 +704,97 @@ public class OneAxisEllipsoid extends Ellipsoid implements BodyShape {
                                                                   DerivativeStructure.hypot(dr, dz).copySign(insideIfNegative));
     }
 
+    /** Compute the azimuth angle from local north between the two points.
+     *
+     * The angle is calculated clockwise from local north at the origin point
+     * and follows the rhumb line to the destination point.
+     *
+     * @param origin the origin point, at which the azimuth angle will be computed (non-{@code null})
+     * @param destination the destination point, to which the angle is defined (non-{@code null})
+     * @return the resulting azimuth angle (radians, {@code [0-2pi)})
+     * @since 11.3
+     */
+    public double azimuthBetweenPoints(final GeodeticPoint origin, final GeodeticPoint destination) {
+        final double dLon = MathUtils.normalizeAngle(destination.getLongitude(), origin.getLongitude()) - origin.getLongitude();
+        final double originIsoLat = geodeticToIsometricLatitude(origin.getLatitude());
+        final double destIsoLat = geodeticToIsometricLatitude(destination.getLatitude());
+
+        final double az = FastMath.atan2(dLon, destIsoLat - originIsoLat);
+        if (az < 0.) {
+            return az + MathUtils.TWO_PI;
+        }
+        return az;
+    }
+
+    /** Compute the azimuth angle from local north between the two points.
+     *
+     * The angle is calculated clockwise from local north at the origin point
+     * and follows the rhumb line to the destination point.
+     *
+     * @param origin the origin point, at which the azimuth angle will be computed (non-{@code null})
+     * @param destination the destination point, to which the angle is defined (non-{@code null})
+     * @param <T> the type of field elements
+     * @return the resulting azimuth angle (radians, {@code [0-2pi)})
+     * @since 11.3
+     */
+    public <T extends CalculusFieldElement<T>> T azimuthBetweenPoints(final FieldGeodeticPoint<T> origin, final FieldGeodeticPoint<T> destination) {
+        final T dLon = MathUtils.normalizeAngle(destination.getLongitude().subtract(origin.getLongitude()), origin.getLongitude().getField().getZero());
+        final T originIsoLat = geodeticToIsometricLatitude(origin.getLatitude());
+        final T destIsoLat = geodeticToIsometricLatitude(destination.getLatitude());
+
+        final T az = FastMath.atan2(dLon, destIsoLat.subtract(originIsoLat));
+        if (az.getReal() < 0.) {
+            return az.add(az.getPi().multiply(2));
+        }
+        return az;
+    }
+
+    /** Compute the <a href="https://mathworld.wolfram.com/IsometricLatitude.html">isometric latitude</a>
+     *  corresponding to the provided latitude.
+     *
+     * @param geodeticLatitude the latitude (radians, within interval {@code [-pi/2, +pi/2]})
+     * @return the isometric latitude (radians)
+     * @since 11.3
+     */
+    public double geodeticToIsometricLatitude(final double geodeticLatitude) {
+        if (FastMath.abs(geodeticLatitude) <= angularThreshold) {
+            return 0.;
+        }
+
+        final double eSinLat = e * FastMath.sin(geodeticLatitude);
+
+        // first term: ln(tan(pi/4 + lat/2))
+        final double a = FastMath.log(FastMath.tan(FastMath.PI / 4. + geodeticLatitude / 2.));
+        // second term: (ecc / 2) * ln((1 - ecc*sin(lat)) / (1 + ecc * sin(lat)))
+        final double b = (e / 2.) * FastMath.log((1. - eSinLat) / (1. + eSinLat));
+
+        return a + b;
+    }
+
+    /** Compute the <a href="https://mathworld.wolfram.com/IsometricLatitude.html">isometric latitude</a>
+     *  corresponding to the provided latitude.
+     *
+     * @param geodeticLatitude the latitude (radians, within interval {@code [-pi/2, +pi/2]})
+     * @param <T> the type of field elements
+     * @return the isometric latitude (radians)
+     * @since 11.3
+     */
+    public <T extends CalculusFieldElement<T>> T geodeticToIsometricLatitude(final T geodeticLatitude) {
+        if (geodeticLatitude.abs().getReal() <= angularThreshold) {
+            return geodeticLatitude.getField().getZero();
+        }
+        final Field<T> field = geodeticLatitude.getField();
+        final T ecc = geodeticLatitude.newInstance(e);
+        final T eSinLat = ecc.multiply(geodeticLatitude.sin());
+
+        // first term: ln(tan(pi/4 + lat/2))
+        final T a = FastMath.log(FastMath.tan(geodeticLatitude.getPi().divide(4.).add(geodeticLatitude.divide(2.))));
+        // second term: (ecc / 2) * ln((1 - ecc*sin(lat)) / (1 + ecc * sin(lat)))
+        final T b = ecc.divide(2.).multiply(FastMath.log(field.getOne().subtract(eSinLat).divide(field.getOne().add(eSinLat))));
+
+        return a.add(b);
+    }
+
     /** Replace the instance with a data transfer object for serialization.
      * <p>
      * This intermediate class serializes the files supported names, the

src/main/java/org/orekit/utils/AggregatedPVCoordinatesProvider.java

+/* Copyright 2002-2022 Joseph Reed
+ * Licensed to CS GROUP (CS) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * Joseph Reed licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package org.orekit.utils;
+
+import java.util.Objects;
+
+import org.orekit.errors.OrekitIllegalArgumentException;
+import org.orekit.errors.OrekitMessages;
+import org.orekit.frames.Frame;
+import org.orekit.time.AbsoluteDate;
+
+/** Aggreate multiple {@link PVCoordinatesProvider} instances together
+ *
+ * This can be used to describe an aircraft or surface vehicle.
+ *
+ * @author Joe Reed
+ * @since 11.3
+ */
+public class AggregatedPVCoordinatesProvider implements PVCoordinatesProvider {
+
+    /** Map of provider instances by transition time. */
+    private final TimeSpanMap<PVCoordinatesProvider> pvProvMap;
+
+    /** Earliest date at which {@link #getPVCoordinates(AbsoluteDate, Frame)} will return a valid result. */
+    private final AbsoluteDate minDate;
+
+    /** Latest date at which {@link #getPVCoordinates(AbsoluteDate, Frame)} will return a valid result. */
+    private final AbsoluteDate maxDate;
+
+    /** Class constructor.
+     *
+     * Note the provided {@code map} is used directly. Modification of the
+     * map after calling this constructor may result in undefined behavior.
+     *
+     * @param map the map of {@link PVCoordinatesProvider} instances by time.
+     */
+    public AggregatedPVCoordinatesProvider(final TimeSpanMap<PVCoordinatesProvider> map) {
+        this(map, null, null);
+    }
+
+    /** Class constructor.
+     *
+     * Note the provided {@code map} is used directly. Modification of the
+     * map after calling this constructor may result in undefined behavior.
+     *
+     * @param map the map of {@link PVCoordinatesProvider} instances by time.
+     * @param minDate the earliest valid date, {@code null} if always valid
+     * @param maxDate the latest valid date, {@code null} if always valid
+     */
+    public AggregatedPVCoordinatesProvider(final TimeSpanMap<PVCoordinatesProvider> map,
+            final AbsoluteDate minDate, final AbsoluteDate maxDate) {
+        this.pvProvMap = Objects.requireNonNull(map, "PVCoordinatesProvider map must be non-null");
+        this.minDate = minDate == null ? AbsoluteDate.PAST_INFINITY : minDate;
+        this.maxDate = maxDate == null ? AbsoluteDate.FUTURE_INFINITY : maxDate;
+    }
+
+    /** Get the first date of the range.
+     * @return the first date of the range
+     */
+    public AbsoluteDate getMinDate() {
+        return minDate;
+    }
+
+    /** Get the last date of the range.
+     * @return the last date of the range
+     */
+    public AbsoluteDate getMaxDate() {
+        return maxDate;
+    }
+
+    @Override
+    public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame frame) {
+        if (date.isBefore(minDate) || date.isAfter(maxDate)) {
+            throw new OrekitIllegalArgumentException(OrekitMessages.OUT_OF_RANGE_EPHEMERIDES_DATE, date, minDate, maxDate);
+        }
+        return pvProvMap.get(date).getPVCoordinates(date, frame);
+    }
+
+    /**
+     * Builder class for {@link AggregatedPVCoordinatesProvider}.
+     */
+    public static class Builder {
+
+        /** Time span map holding the incremental values. */
+        private TimeSpanMap<PVCoordinatesProvider> pvProvMap = null;
+
+        /**
+         * Create a builder using the {@link InvalidPVProvider} as the initial provider.
+         */
+        public Builder() {
+            this(new InvalidPVProvider());
+        }
+
+        /**