WaypointPVBuilder.java
/* Copyright 2002-2024 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.Map.Entry;
import java.util.TreeMap;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.geometry.spherical.twod.Circle;
import org.hipparchus.geometry.spherical.twod.S2Point;
import org.hipparchus.util.FastMath;
import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.LoxodromeArc;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.frames.Frame;
import org.orekit.frames.TopocentricFrame;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.TimeOffset;
/** Builder class, enabling incremental building of an {@link PVCoordinatesProvider}
* instance using waypoints defined on an ellipsoid.
* <p>
* Given a series of waypoints ({@code (date, point)} tuples),
* build a {@link PVCoordinatesProvider} representing the path.
* The static methods provide implementations for the most common path definitions
* (cartesian, great-circle, loxodrome). If these methods are insufficient,
* the public constructor provides a way to customize the path definition.
* </p>
* <p>
* This class connects the path segments using the {@link AggregatedPVCoordinatesProvider}.
* As such, no effort is made to smooth the velocity between segments.
* While position is unaffected, the velocity may be discontinuous between adjacent time points.
* Thus, care should be taken when modeling paths with abrupt direction changes
* (e.g. fast-moving aircraft); understand how the {@link PVCoordinatesProvider}
* will be used in the particular application.
* </p>
* @author Joe Reed
* @since 11.3
*/
public class WaypointPVBuilder {
/** Factory used to create intermediate pv providers between waypoints. */
private final InterpolationFactory factory;
/** Central body, on which the waypoints are defined. */
private final OneAxisEllipsoid body;
/** Set of waypoints, indexed by time. */
private final TreeMap<AbsoluteDate, GeodeticPoint> waypoints;
/** Whether the resulting provider should be invalid or constant prior to the first waypoint. */
private boolean invalidBefore;
/** Whether the resulting provider should be invalid or constant after to the last waypoint. */
private boolean invalidAfter;
/** Create a new instance.
* @param factory The factory used to create the intermediate coordinate providers between waypoints.
* @param body The central body, on which the way points are defined.
*/
public WaypointPVBuilder(final InterpolationFactory factory, final OneAxisEllipsoid body) {
this.factory = factory;
this.body = body;
this.waypoints = new TreeMap<>();
this.invalidBefore = true;
this.invalidAfter = true;
}
/** Construct a waypoint builder interpolating points using a linear cartesian interpolation.
*
* @param body the reference ellipsoid on which the waypoints are defined.
* @return the waypoint builder
*/
public static WaypointPVBuilder cartesianBuilder(final OneAxisEllipsoid body) {
return new WaypointPVBuilder(CartesianWaypointPVProv::new, body);
}
/** Construct a waypoint builder interpolating points using a loxodrome (or Rhumbline).
*
* @param body the reference ellipsoid on which the waypoints are defined.
* @return the waypoint builder
*/
public static WaypointPVBuilder loxodromeBuilder(final OneAxisEllipsoid body) {
return new WaypointPVBuilder(LoxodromeWaypointPVProv::new, body);
}
/** Construct a waypoint builder interpolating points using a great-circle.
* <p>
* The altitude of the intermediate points is linearly interpolated from the bounding waypoints.
* Extrapolating before the first waypoint or after the last waypoint may result in undefined altitudes.
* </p>
* @param body the reference ellipsoid on which the waypoints are defined.
* @return the waypoint builder
*/
public static WaypointPVBuilder greatCircleBuilder(final OneAxisEllipsoid body) {
return new WaypointPVBuilder(GreatCircleWaypointPVProv::new, body);
}
/** Add a waypoint.
*
* @param point the waypoint location
* @param date the waypoint time
* @return this instance
*/
public WaypointPVBuilder addWaypoint(final GeodeticPoint point, final AbsoluteDate date) {
waypoints.put(date, point);
return this;
}
/** Indicate the resulting {@link PVCoordinatesProvider} should be invalid before the first waypoint.
*
* @return this instance
*/
public WaypointPVBuilder invalidBefore() {
invalidBefore = true;
return this;
}
/** Indicate the resulting {@link PVCoordinatesProvider} provide
* a constant location of the first waypoint prior to the first time.
*
* @return this instance
*/
public WaypointPVBuilder constantBefore() {
invalidBefore = false;
return this;
}
/** Indicate the resulting {@link PVCoordinatesProvider} should be invalid after the last waypoint.
*
* @return this instance
*/
public WaypointPVBuilder invalidAfter() {
invalidAfter = true;
return this;
}
/** Indicate the resulting {@link PVCoordinatesProvider} provide
* a constant location of the last waypoint after to the last time.
*
* @return this instance
*/
public WaypointPVBuilder constantAfter() {
invalidAfter = false;
return this;
}
/** Build a {@link PVCoordinatesProvider} from the waypoints added to this builder.
*
* @return the coordinates provider instance.
*/
public PVCoordinatesProvider build() {
final PVCoordinatesProvider initialProvider = createInitial(waypoints.firstEntry().getValue());
final AggregatedPVCoordinatesProvider.Builder builder = new AggregatedPVCoordinatesProvider.Builder(initialProvider);
Entry<AbsoluteDate, GeodeticPoint> previousEntry = null;
for (final Entry<AbsoluteDate, GeodeticPoint> entry: waypoints.entrySet()) {
if (previousEntry != null) {
builder.addPVProviderAfter(previousEntry.getKey(),
factory.create(previousEntry.getKey(),
previousEntry.getValue(),
entry.getKey(),
entry.getValue(),
body),
true);
}
previousEntry = entry;
}
// add the point so we're valid at the final waypoint
builder.addPVProviderAfter(previousEntry.getKey(),
new ConstantPVCoordinatesProvider(previousEntry.getValue(), body),
true);
// add the final provider after the final waypoint
builder.addPVProviderAfter(previousEntry.getKey().shiftedBy(TimeOffset.ATTOSECOND),
createFinal(previousEntry.getValue()),
true);
return builder.build();
}
/**
* Create the initial provider.
* <p>
* This method uses the internal {@code validBefore} flag to either return an invalid PVCoordinatesProvider or a
* constant one.
* </p>
*
* @param firstPoint the first waypoint
* @return the coordinate provider
*/
protected PVCoordinatesProvider createInitial(final GeodeticPoint firstPoint) {
if (invalidBefore) {
return new AggregatedPVCoordinatesProvider.InvalidPVProvider();
} else {
return new ConstantPVCoordinatesProvider(firstPoint, body);
}
}
/**
* Create the final provider.
* <p>
* This method uses the internal {@code validAfter} flag to either return an invalid PVCoordinatesProvider or a
* constant one.
* </p>
*
* @param lastPoint the last waypoint
* @return the coordinate provider
*/
protected PVCoordinatesProvider createFinal(final GeodeticPoint lastPoint) {
if (invalidAfter) {
return new AggregatedPVCoordinatesProvider.InvalidPVProvider();
} else {
return new ConstantPVCoordinatesProvider(lastPoint, body);
}
}
/**
* Factory interface, creating the {@link PVCoordinatesProvider} instances between the provided waypoints.
*/
@FunctionalInterface
public interface InterpolationFactory {
/** Create a {@link PVCoordinatesProvider} which interpolates between the provided waypoints.
*
* @param date1 the first waypoint's date
* @param point1 the first waypoint's location
* @param date2 the second waypoint's date
* @param point2 the second waypoint's location
* @param body the body on which the waypoints are defined
* @return a {@link PVCoordinatesProvider} providing the locations at times between the waypoints.
*/
PVCoordinatesProvider create(AbsoluteDate date1, GeodeticPoint point1,
AbsoluteDate date2, GeodeticPoint point2,
OneAxisEllipsoid body);
}
/**
* Coordinate provider interpolating along the great-circle between two points.
*/
static class GreatCircleWaypointPVProv implements PVCoordinatesProvider {
/** Great circle estimation. */
private final Circle circle;
/** Duration between the two points (seconds). */
private final double duration;
/** Phase along the circle of the first point. */
private final double phase0;
/** Phase length from the first point to the second. */
private final double phaseLength;
/** Time at which interpolation results in the initial point. */
private final AbsoluteDate t0;
/** Body on which the great circle is defined. */
private final OneAxisEllipsoid body;
/** Phase of one second. */
private final double oneSecondPhase;
/** Altitude of the initial point. */
private final double initialAltitude;
/** Time-derivative of the altitude. */
private final double altitudeSlope;
/** Class constructor. Aligns to the {@link InterpolationFactory} functional interface.
*
* @param date1 the first waypoint's date
* @param point1 the first waypoint's location
* @param date2 the second waypoint's date
* @param point2 the second waypoint's location
* @param body the body on which the waypoints are defined
* @see InterpolationFactory
*/
GreatCircleWaypointPVProv(final AbsoluteDate date1, final GeodeticPoint point1,
final AbsoluteDate date2, final GeodeticPoint point2,
final OneAxisEllipsoid body) {
this.t0 = date1;
this.duration = date2.durationFrom(date1);
this.body = body;
final S2Point s0 = toSpherical(point1);
final S2Point s1 = toSpherical(point2);
circle = new Circle(s0, s1, 1e-9);
phase0 = circle.getPhase(s0.getVector());
phaseLength = circle.getPhase(s1.getVector()) - phase0;
oneSecondPhase = phaseLength / duration;
altitudeSlope = (point2.getAltitude() - point1.getAltitude()) / duration;
initialAltitude = point1.getAltitude();
}
@Override
public Vector3D getPosition(final AbsoluteDate date, final Frame frame) {
final double d = date.durationFrom(t0);
final double fraction = d / duration;
final double phase = fraction * phaseLength;
final S2Point sp = new S2Point(circle.getPointAt(phase0 + phase));
final GeodeticPoint point = toGeodetic(sp, initialAltitude + d * altitudeSlope);
final Vector3D p = body.transform(point);
return body.getBodyFrame().getStaticTransformTo(frame, date).transformPosition(p);
}
@Override
public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame frame) {
final double d = date.durationFrom(t0);
final double fraction = d / duration;
final double phase = fraction * phaseLength;
final S2Point sp = new S2Point(circle.getPointAt(phase0 + phase));
final GeodeticPoint point = toGeodetic(sp, initialAltitude + d * altitudeSlope);
final Vector3D p = body.transform(point);
// add 1 second to get another point along the circle, to use for velocity
final S2Point sp2 = new S2Point(circle.getPointAt(phase0 + phase + oneSecondPhase));
final GeodeticPoint point2 = toGeodetic(sp2, initialAltitude + (d + 1) * altitudeSlope);
final Vector3D p2 = body.transform(point2);
final Vector3D v = p2.subtract(p);
final TimeStampedPVCoordinates tpv = new TimeStampedPVCoordinates(date, p, v);
return body.getBodyFrame().getTransformTo(frame, date).transformPVCoordinates(tpv);
}
/** Converts the given geodetic point to a point on the 2-sphere.
* @param point input geodetic point
* @return a point on the 2-sphere
*/
static S2Point toSpherical(final GeodeticPoint point) {
return new S2Point(point.getLongitude(), 0.5 * FastMath.PI - point.getLatitude());
}
/** Converts a 2-sphere point to a geodetic point.
* @param point point on the 2-sphere
* @param alt point altitude
* @return a geodetic point
*/
static GeodeticPoint toGeodetic(final S2Point point, final double alt) {
return new GeodeticPoint(0.5 * FastMath.PI - point.getPhi(), point.getTheta(), alt);
}
}
/**
* Coordinate provider interpolating along the loxodrome between two points.
*/
static class LoxodromeWaypointPVProv implements PVCoordinatesProvider {
/** Arc along which the interpolation occurs. */
private final LoxodromeArc arc;
/** Time at which the interpolation begins (at arc start). */
private final AbsoluteDate t0;
/** Total duration to get the length of the arc (seconds). */
private final double duration;
/** Velocity along the arc (m/s). */
private final double velocity;
/** Class constructor. Aligns to the {@link InterpolationFactory} functional interface.
*
* @param date1 the first waypoint's date
* @param point1 the first waypoint's location
* @param date2 the second waypoint's date
* @param point2 the second waypoint's location
* @param body the body on which the waypoints are defined
* @see InterpolationFactory
*/
LoxodromeWaypointPVProv(final AbsoluteDate date1, final GeodeticPoint point1, final AbsoluteDate date2,
final GeodeticPoint point2, final OneAxisEllipsoid body) {
this.arc = new LoxodromeArc(point1, point2, body);
this.t0 = date1;
this.duration = date2.durationFrom(date1);
this.velocity = arc.getDistance() / duration;
}
@Override
public Vector3D getPosition(final AbsoluteDate date, final Frame frame) {
final double fraction = date.durationFrom(t0) / duration;
final GeodeticPoint point = arc.calculatePointAlongArc(fraction);
final Vector3D p = arc.getBody().transform(point);
return arc.getBody().getBodyFrame().getStaticTransformTo(frame, date).transformPosition(p);
}
@Override
public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame frame) {
final double fraction = date.durationFrom(t0) / duration;
final GeodeticPoint point = arc.calculatePointAlongArc(fraction);
final Vector3D p = arc.getBody().transform(point);
final Vector3D vp = arc.getBody().transform(
new TopocentricFrame(arc.getBody(), point, "frame")
.pointAtDistance(arc.getAzimuth(), 0, velocity));
final TimeStampedPVCoordinates tpv = new TimeStampedPVCoordinates(date, p, vp.subtract(p));
return arc.getBody().getBodyFrame().getTransformTo(frame, date).transformPVCoordinates(tpv);
}
}
/**
* Coordinate provider interpolating along the cartesian (3-space) line between two points.
*/
static class CartesianWaypointPVProv implements PVCoordinatesProvider {
/** Date at which the position is valid. */
private final AbsoluteDate t0;
/** Initial point. */
private final Vector3D p0;
/** Velocity. */
private final Vector3D vel;
/** Frame in which the point and velocity are defined. */
private final Frame sourceFrame;
/** Class constructor. Aligns to the {@link InterpolationFactory} functional interface.
*
* @param date1 the first waypoint's date
* @param point1 the first waypoint's location
* @param date2 the second waypoint's date
* @param point2 the second waypoint's location
* @param body the body on which the waypoints are defined
* @see InterpolationFactory
*/
CartesianWaypointPVProv(final AbsoluteDate date1, final GeodeticPoint point1,
final AbsoluteDate date2, final GeodeticPoint point2,
final OneAxisEllipsoid body) {
this.t0 = date1;
this.p0 = body.transform(point1);
this.vel = body.transform(point2).subtract(p0).scalarMultiply(1. / date2.durationFrom(t0));
this.sourceFrame = body.getBodyFrame();
}
@Override
public Vector3D getPosition(final AbsoluteDate date, final Frame frame) {
final double d = date.durationFrom(t0);
final Vector3D p = p0.add(vel.scalarMultiply(d));
return sourceFrame.getStaticTransformTo(frame, date).transformPosition(p);
}
@Override
public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame frame) {
final double d = date.durationFrom(t0);
final Vector3D p = p0.add(vel.scalarMultiply(d));
final TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(date, p, vel);
return sourceFrame.getTransformTo(frame, date).transformPVCoordinates(pv);
}
}
}