WaypointPVBuilder.java

  1. /* Copyright 2002-2022 Joseph Reed
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * Joseph Reed 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.utils;

  18. import java.util.Map.Entry;
  19. import java.util.TreeMap;

  20. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  21. import org.hipparchus.geometry.spherical.twod.Circle;
  22. import org.hipparchus.geometry.spherical.twod.S2Point;
  23. import org.hipparchus.util.FastMath;
  24. import org.orekit.bodies.GeodeticPoint;
  25. import org.orekit.bodies.LoxodromeArc;
  26. import org.orekit.bodies.OneAxisEllipsoid;
  27. import org.orekit.frames.Frame;
  28. import org.orekit.frames.TopocentricFrame;
  29. import org.orekit.time.AbsoluteDate;

  30. /** Builder class, enabling incremental building of an {@link PVCoordinatesProvider}
  31.  * instance using waypoints defined on an ellipsoid.
  32.  *
  33.  * Given a series of waypoints ({@code (date, point)} tuples),
  34.  * build a {@link PVCoordinatesProvider} representing the path.
  35.  * The static methods provide implementations for the most common path definitions
  36.  * (cartesian, great-circle, loxodrome). If these methods are insufficient,
  37.  * the public constructor provides a way to customize the path definition.
  38.  *
  39.  * This class connects the path segments using the {@link AggregatedPVCoordinatesProvider}.
  40.  * As such, no effort is made to smooth the velocity between segments.
  41.  * While position is unaffected, the velocity may be discontinuous between adjacent time points.
  42.  * Thus, care should be taken when modeling paths with abrupt direction changes
  43.  * (e.g. fast-moving aircraft); understand how the {@link PVCoordinatesProvider}
  44.  * will be used in the particular application.
  45.  *
  46.  * @author Joe Reed
  47.  * @since 11.3
  48.  */
  49. public class WaypointPVBuilder {

  50.     /** Factory used to create intermediate pv providers between waypoints. */
  51.     private final InterpolationFactory factory;

  52.     /** Central body, on which the waypoints are defined. */
  53.     private final OneAxisEllipsoid body;

  54.     /** Set of waypoints, indexed by time. */
  55.     private final TreeMap<AbsoluteDate, GeodeticPoint> waypoints = new TreeMap<>();

  56.     /** Whether the resulting provider should be invalid or constant prior to the first waypoint. */
  57.     private boolean invalidBefore = true;

  58.     /** Whether the resulting provider should be invalid or constant after to the last waypoint. */
  59.     private boolean invalidAfter = true;

  60.     /** Create a new instance.
  61.      * @param factory The factory used to create the intermediate coordinate providers between waypoints.
  62.      * @param body The central body, on which the way points are defined.
  63.      */
  64.     public WaypointPVBuilder(final InterpolationFactory factory, final OneAxisEllipsoid body) {
  65.         this.factory = factory;
  66.         this.body = body;
  67.     }

  68.     /** Construct a waypoint builder interpolating points using a linear cartesian interpolation.
  69.      *
  70.      * @param body the reference ellipsoid on which the waypoints are defined.
  71.      * @return the waypoint builder
  72.      */
  73.     public static WaypointPVBuilder cartesianBuilder(final OneAxisEllipsoid body) {
  74.         return new WaypointPVBuilder(CartesianWaypointPVProv::new, body);
  75.     }

  76.     /** Construct a waypoint builder interpolating points using a loxodrome (or Rhumbline).
  77.      *
  78.      * @param body the reference ellipsoid on which the waypoints are defined.
  79.      * @return the waypoint builder
  80.      */
  81.     public static WaypointPVBuilder loxodromeBuilder(final OneAxisEllipsoid body) {
  82.         return new WaypointPVBuilder(LoxodromeWaypointPVProv::new, body);
  83.     }

  84.     /** Construct a waypoint builder interpolating points using a great-circle.
  85.      *
  86.      * The altitude of the intermediate points is linearly interpolated from the bounding waypoints.
  87.      * Extrapolating before the first waypoint or after the last waypoint may result in undefined altitudes.
  88.      *
  89.      * @param body the reference ellipsoid on which the waypoints are defined.
  90.      * @return the waypoint builder
  91.      */
  92.     public static WaypointPVBuilder greatCircleBuilder(final OneAxisEllipsoid body) {
  93.         return new WaypointPVBuilder(GreatCircleWaypointPVProv::new, body);
  94.     }

  95.     /** Add a waypoint.
  96.      *
  97.      * @param point the waypoint location
  98.      * @param date the waypoint time
  99.      * @return this instance
  100.      */
  101.     public WaypointPVBuilder addWaypoint(final GeodeticPoint point, final AbsoluteDate date) {
  102.         waypoints.put(date, point);
  103.         return this;
  104.     }

  105.     /** Indicate the resulting {@link PVCoordinatesProvider} should be invalid before the first waypoint.
  106.      *
  107.      * @return this instance
  108.      */
  109.     public WaypointPVBuilder invalidBefore() {
  110.         invalidBefore = true;
  111.         return this;
  112.     }

  113.     /** Indicate the resulting {@link PVCoordinatesProvider} provide
  114.      * a constant location of the first waypoint prior to the first time.
  115.      *
  116.      * @return this instance
  117.      */
  118.     public WaypointPVBuilder constantBefore() {
  119.         invalidBefore = false;
  120.         return this;
  121.     }

  122.     /** Indicate the resulting {@link PVCoordinatesProvider} should be invalid after the last waypoint.
  123.      *
  124.      * @return this instance
  125.      */
  126.     public WaypointPVBuilder invalidAfter() {
  127.         invalidAfter = true;
  128.         return this;
  129.     }

  130.     /** Indicate the resulting {@link PVCoordinatesProvider} provide
  131.      * a constant location of the last waypoint after to the last time.
  132.      *
  133.      * @return this instance
  134.      */
  135.     public WaypointPVBuilder constantAfter() {
  136.         invalidAfter = false;
  137.         return this;
  138.     }

  139.     /** Build a {@link PVCoordinatesProvider} from the waypoints added to this builder.
  140.      *
  141.      * @return the coordinates provider instance.
  142.      */
  143.     public PVCoordinatesProvider build() {
  144.         final PVCoordinatesProvider initialProvider = createInitial(waypoints.firstKey(),
  145.                                                                     waypoints.firstEntry().getValue());
  146.         final AggregatedPVCoordinatesProvider.Builder builder = new AggregatedPVCoordinatesProvider.Builder(initialProvider);

  147.         Entry<AbsoluteDate, GeodeticPoint> previousEntry = null;
  148.         for (final Entry<AbsoluteDate, GeodeticPoint> entry: waypoints.entrySet()) {
  149.             if (previousEntry != null) {
  150.                 builder.addPVProviderAfter(previousEntry.getKey(),
  151.                                            factory.create(previousEntry.getKey(),
  152.                                                           previousEntry.getValue(),
  153.                                                           entry.getKey(),
  154.                                                           entry.getValue(),
  155.                                                           body),
  156.                                            true);
  157.             }
  158.             previousEntry = entry;
  159.         }
  160.         // add the point so we're valid at the final waypoint
  161.         builder.addPVProviderAfter(previousEntry.getKey(),
  162.                                    new ConstantPVCoordinatesProvider(previousEntry.getValue(), body),
  163.                                    true);
  164.         // add the final propagator after the final waypoint
  165.         builder.addPVProviderAfter(previousEntry.getKey().shiftedBy(Double.MIN_VALUE),
  166.                                    createFinal(previousEntry.getKey(), previousEntry.getValue()),
  167.                                    true);

  168.         return builder.build();
  169.     }

  170.     /** Create the initial provider.
  171.      *
  172.      * This method uses the internal {@code validBefore} flag to
  173.      * either return an invalid PVCoordinatesProvider or a constant one.
  174.      *
  175.      * @param firstDate the date at which the first waypoint is reached
  176.      *                  and this provider will no longer be called
  177.      * @param firstPoint the first waypoint
  178.      * @return the coordinate provider
  179.      */
  180.     protected PVCoordinatesProvider createInitial(final AbsoluteDate firstDate, final GeodeticPoint firstPoint) {
  181.         if (invalidBefore) {
  182.             return new AggregatedPVCoordinatesProvider.InvalidPVProvider();
  183.         }
  184.         else {
  185.             return new ConstantPVCoordinatesProvider(firstPoint, body);
  186.         }
  187.     }

  188.     /** Create the final provider.
  189.      *
  190.      * This method uses the internal {@code validAfter} flag to
  191.      * either return an invalid PVCoordinatesProvider or a constant one.
  192.      *
  193.      * @param lastDate the date at which the last waypoint is reached
  194.      *                 and this provider will be called
  195.      * @param lastPoint the last waypoint
  196.      * @return the coordinate provider
  197.      */
  198.     protected PVCoordinatesProvider createFinal(final AbsoluteDate lastDate, final GeodeticPoint lastPoint) {
  199.         if (invalidAfter) {
  200.             return new AggregatedPVCoordinatesProvider.InvalidPVProvider();
  201.         }
  202.         else {
  203.             return new ConstantPVCoordinatesProvider(lastPoint, body);
  204.         }
  205.     }

  206.     /**
  207.      * Factory interface, creating the {@link PVCoordinatesProvider} instances between the provided waypoints.
  208.      */
  209.     @FunctionalInterface
  210.     public interface InterpolationFactory {

  211.         /** Create a {@link PVCoordinatesProvider} which interpolates between the provided waypoints.
  212.          *
  213.          * @param date1 the first waypoint's date
  214.          * @param point1 the first waypoint's location
  215.          * @param date2 the second waypoint's date
  216.          * @param point2 the second waypoint's location
  217.          * @param body the body on which the waypoints are defined
  218.          * @return a {@link PVCoordinatesProvider} providing the locations at times between the waypoints.
  219.          */
  220.         PVCoordinatesProvider create(AbsoluteDate date1, GeodeticPoint point1,
  221.                                      AbsoluteDate date2, GeodeticPoint point2,
  222.                                      OneAxisEllipsoid body);
  223.     }

  224.     /**
  225.      * Coordinate provider interpolating along the great-circle between two points.
  226.      */
  227.     static class GreatCircleWaypointPVProv implements PVCoordinatesProvider {

  228.         /** Great circle estimation. */
  229.         private final Circle circle;
  230.         /** Duration between the two points (seconds). */
  231.         private final double duration;
  232.         /** Phase along the circle of the first point. */
  233.         private final double phase0;
  234.         /** Phase length from the first point to the second. */
  235.         private final double phaseLength;
  236.         /** Time at which interpolation results in the initial point. */
  237.         private final AbsoluteDate t0;
  238.         /** Body on which the great circle is defined. */
  239.         private final OneAxisEllipsoid body;
  240.         /** Phase of one second. */
  241.         private double oneSecondPhase;
  242.         /** Altitude of the initial point. */
  243.         private double initialAltitude;
  244.         /** Time-derivative of the altitude. */
  245.         private double altitudeSlope;

  246.         /** Class constructor. Aligns to the {@link InterpolationFactory} functional interface.
  247.          *
  248.          * @param date1 the first waypoint's date
  249.          * @param point1 the first waypoint's location
  250.          * @param date2 the second waypoint's date
  251.          * @param point2 the second waypoint's location
  252.          * @param body the body on which the waypoints are defined
  253.          * @see InterpolationFactory
  254.          */
  255.         GreatCircleWaypointPVProv(final AbsoluteDate date1, final GeodeticPoint point1,
  256.                                   final AbsoluteDate date2, final GeodeticPoint point2,
  257.                                   final OneAxisEllipsoid body) {
  258.             this.t0 = date1;
  259.             this.duration = date2.durationFrom(date1);
  260.             this.body = body;
  261.             final S2Point s0 = toSpherical(point1);
  262.             final S2Point s1 = toSpherical(point2);
  263.             circle = new Circle(s0, s1, 1e-9);

  264.             phase0 = circle.getPhase(s0.getVector());
  265.             phaseLength = circle.getPhase(s1.getVector()) - phase0;

  266.             oneSecondPhase = phaseLength / duration;
  267.             altitudeSlope = (point2.getAltitude() - point1.getAltitude()) / duration;
  268.             initialAltitude = point1.getAltitude();
  269.         }

  270.         @Override
  271.         public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame frame) {
  272.             final double d = date.durationFrom(t0);
  273.             final double fraction = d / duration;
  274.             final double phase = fraction * phaseLength;

  275.             final S2Point sp = new S2Point(circle.getPointAt(phase0 + phase));
  276.             final GeodeticPoint point = toGeodetic(sp, initialAltitude + d * altitudeSlope);
  277.             final Vector3D p = body.transform(point);

  278.             // add 1 second to get another point along the circle, to use for velocity
  279.             final S2Point sp2 = new S2Point(circle.getPointAt(phase0 + phase + oneSecondPhase));
  280.             final GeodeticPoint point2 = toGeodetic(sp2, initialAltitude + (d + 1) * altitudeSlope);
  281.             final Vector3D p2 = body.transform(point2);
  282.             final Vector3D v = p2.subtract(p);

  283.             final TimeStampedPVCoordinates tpv = new TimeStampedPVCoordinates(date, p, v);
  284.             return body.getBodyFrame().getTransformTo(frame, date).transformPVCoordinates(tpv);
  285.         }

  286.         static S2Point toSpherical(final GeodeticPoint point) {
  287.             return new S2Point(point.getLongitude(), 0.5 * FastMath.PI - point.getLatitude());
  288.         }

  289.         static GeodeticPoint toGeodetic(final S2Point point, final double alt) {
  290.             return new GeodeticPoint(0.5 * FastMath.PI - point.getPhi(), point.getTheta(), alt);
  291.         }
  292.     }

  293.     /**
  294.      * Coordinate provider interpolating along the loxodrome between two points.
  295.      */
  296.     static class LoxodromeWaypointPVProv implements PVCoordinatesProvider {

  297.         /** Arc along which the interpolation occurs. */
  298.         private final LoxodromeArc arc;
  299.         /** Time at which the interpolation begins (at arc start). */
  300.         private final AbsoluteDate t0;
  301.         /** Total duration to get the length of the arc (seconds). */
  302.         private final double duration;
  303.         /** Velocity along the arc (m/s). */
  304.         private final double velocity;

  305.         /** Class constructor. Aligns to the {@link InterpolationFactory} functional interface.
  306.          *
  307.          * @param date1 the first waypoint's date
  308.          * @param point1 the first waypoint's location
  309.          * @param date2 the second waypoint's date
  310.          * @param point2 the second waypoint's location
  311.          * @param body the body on which the waypoints are defined
  312.          * @see InterpolationFactory
  313.          */
  314.         LoxodromeWaypointPVProv(final AbsoluteDate date1, final GeodeticPoint point1, final AbsoluteDate date2,
  315.                 final GeodeticPoint point2, final OneAxisEllipsoid body) {
  316.             this.arc = new LoxodromeArc(point1, point2, body);
  317.             this.t0 = date1;
  318.             this.duration = date2.durationFrom(date1);
  319.             this.velocity = arc.getDistance() / duration;
  320.         }

  321.         @Override
  322.         public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame frame) {
  323.             final double fraction = date.durationFrom(t0) / duration;
  324.             final GeodeticPoint point = arc.calculatePointAlongArc(fraction);
  325.             final Vector3D p = arc.getBody().transform(point);
  326.             final Vector3D vp = arc.getBody().transform(
  327.                     new TopocentricFrame(arc.getBody(), point, "frame")
  328.                         .pointAtDistance(arc.getAzimuth(), 0, velocity));

  329.             final TimeStampedPVCoordinates tpv = new TimeStampedPVCoordinates(date, p, vp.subtract(p));
  330.             return arc.getBody().getBodyFrame().getTransformTo(frame, date).transformPVCoordinates(tpv);
  331.         }
  332.     }

  333.     /**
  334.      * Coordinate provider interpolating along the cartesian (3-space) line between two points.
  335.      */
  336.     static class CartesianWaypointPVProv implements PVCoordinatesProvider {

  337.         /** Date at which the position is valid. */
  338.         private final AbsoluteDate t0;
  339.         /** Initial point. */
  340.         private final Vector3D p0;
  341.         /** Velocity. */
  342.         private final Vector3D vel;
  343.         /** Frame in which the point and velocity are defined. */
  344.         private final Frame sourceFrame;

  345.         /** Class constructor. Aligns to the {@link InterpolationFactory} functional interface.
  346.          *
  347.          * @param date1 the first waypoint's date
  348.          * @param point1 the first waypoint's location
  349.          * @param date2 the second waypoint's date
  350.          * @param point2 the second waypoint's location
  351.          * @param body the body on which the waypoints are defined
  352.          * @see InterpolationFactory
  353.          */
  354.         CartesianWaypointPVProv(final AbsoluteDate date1, final GeodeticPoint point1,
  355.                                 final AbsoluteDate date2, final GeodeticPoint point2,
  356.                                 final OneAxisEllipsoid body) {
  357.             this.t0 = date1;
  358.             this.p0 = body.transform(point1);
  359.             this.vel = body.transform(point2).subtract(p0).scalarMultiply(1. / date2.durationFrom(t0));
  360.             this.sourceFrame = body.getBodyFrame();
  361.         }

  362.         @Override
  363.         public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame frame) {
  364.             final double d = date.durationFrom(t0);
  365.             final Vector3D p = p0.add(vel.scalarMultiply(d));
  366.             final TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(date, p, vel);
  367.             return sourceFrame.getTransformTo(frame, date).transformPVCoordinates(pv);
  368.         }
  369.     }
  370. }