Commit 6fd989e5 authored by Pascal Parraud's avatar Pascal Parraud
Browse files

Merge branch 'geodeticpoint-interpolation' into develop

parents f6d3c8ea 6bd47f2b
/* 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;
}
}
}
/* 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);
}
}
}
......@@ -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
......
/* 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());
}
/**