TimeStampedFieldPVCoordinatesHermiteInterpolator.java

  1. /* Copyright 2002-2024 CS GROUP
  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.  * 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.utils;

  18. import org.hipparchus.CalculusFieldElement;
  19. import org.hipparchus.analysis.interpolation.FieldHermiteInterpolator;
  20. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  21. import org.orekit.errors.OrekitInternalError;
  22. import org.orekit.time.AbstractFieldTimeInterpolator;
  23. import org.orekit.time.FieldAbsoluteDate;

  24. import java.util.stream.Stream;

  25. /**
  26.  * Class using a Hermite interpolator to interpolate time stamped position-velocity-acceleration coordinates.
  27.  * <p>
  28.  * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation points
  29.  * (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  30.  * and numerical problems (including NaN appearing).
  31.  *
  32.  * @param <KK> type of the field element
  33.  *
  34.  * @author Luc Maisonobe
  35.  * @author Vincent Cucchietti
  36.  * @see FieldHermiteInterpolator
  37.  * @see TimeStampedFieldPVCoordinates
  38.  */
  39. public class TimeStampedFieldPVCoordinatesHermiteInterpolator<KK extends CalculusFieldElement<KK>>
  40.         extends AbstractFieldTimeInterpolator<TimeStampedFieldPVCoordinates<KK>, KK> {

  41.     /** Filter for derivatives from the sample to use in interpolation. */
  42.     private final CartesianDerivativesFilter filter;

  43.     /**
  44.      * Constructor with :
  45.      * <ul>
  46.      *     <li>Default number of interpolation points of {@code DEFAULT_INTERPOLATION_POINTS}</li>
  47.      *     <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
  48.      *     <li>Use of angular and first time derivative for attitude interpolation</li>
  49.      * </ul>
  50.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  51.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  52.      * phenomenon</a> and numerical problems (including NaN appearing).
  53.      */
  54.     public TimeStampedFieldPVCoordinatesHermiteInterpolator() {
  55.         this(DEFAULT_INTERPOLATION_POINTS);
  56.     }

  57.     /**
  58.      * Constructor with :
  59.      * <ul>
  60.      *     <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
  61.      *     <li>Use of position and both time derivatives for attitude interpolation</li>
  62.      * </ul>
  63.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  64.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  65.      * phenomenon</a> and numerical problems (including NaN appearing).
  66.      *
  67.      * @param interpolationPoints number of interpolation points
  68.      */
  69.     public TimeStampedFieldPVCoordinatesHermiteInterpolator(final int interpolationPoints) {

  70.         this(interpolationPoints, CartesianDerivativesFilter.USE_PVA);
  71.     }

  72.     /**
  73.      * Constructor with :
  74.      * <ul>
  75.      *     <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
  76.      * </ul>
  77.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  78.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  79.      * phenomenon</a> and numerical problems (including NaN appearing).
  80.      *
  81.      * @param interpolationPoints number of interpolation points
  82.      * @param filter filter for derivatives from the sample to use in interpolation
  83.      */
  84.     public TimeStampedFieldPVCoordinatesHermiteInterpolator(final int interpolationPoints,
  85.                                                             final CartesianDerivativesFilter filter) {
  86.         this(interpolationPoints, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC, filter);
  87.     }

  88.     /**
  89.      * Constructor.
  90.      * <p>
  91.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  92.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  93.      * phenomenon</a> and numerical problems (including NaN appearing).
  94.      *
  95.      * @param interpolationPoints number of interpolation points
  96.      * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
  97.      * @param filter filter for derivatives from the sample to use in interpolation
  98.      */
  99.     public TimeStampedFieldPVCoordinatesHermiteInterpolator(final int interpolationPoints,
  100.                                                             final double extrapolationThreshold,
  101.                                                             final CartesianDerivativesFilter filter) {
  102.         super(interpolationPoints, extrapolationThreshold);
  103.         this.filter = filter;
  104.     }

  105.     /** filter for derivatives from the sample to use in interpolation.
  106.      * @return filter for derivatives from the sample to use in interpolation
  107.      */
  108.     public CartesianDerivativesFilter getFilter() {
  109.         return filter;
  110.     }

  111.     /**
  112.      * {@inheritDoc}
  113.      * <p>
  114.      * The interpolated instance is created by polynomial Hermite interpolation ensuring velocity remains the exact
  115.      * derivative of position.
  116.      * <p>
  117.      * Note that even if first time derivatives (velocities) from sample can be ignored, the interpolated instance always
  118.      * includes interpolated derivatives. This feature can be used explicitly to compute these derivatives when it would be
  119.      * too complex to compute them from an analytical formula: just compute a few sample points from the explicit formula and
  120.      * set the derivatives to zero in these sample points, then use interpolation to add derivatives consistent with the
  121.      * positions.
  122.      */
  123.     @Override
  124.     protected TimeStampedFieldPVCoordinates<KK> interpolate(final InterpolationData interpolationData) {

  125.         // Get interpolation date
  126.         final FieldAbsoluteDate<KK> interpolationDate = interpolationData.getInterpolationDate();

  127.         // Convert sample to stream
  128.         final Stream<TimeStampedFieldPVCoordinates<KK>> sample = interpolationData.getNeighborList().stream();

  129.         // Set up an interpolator taking derivatives into account
  130.         final FieldHermiteInterpolator<KK> interpolator = new FieldHermiteInterpolator<>();

  131.         // Add sample points
  132.         switch (filter) {
  133.             case USE_P:
  134.                 // populate sample with position data, ignoring velocity
  135.                 sample.forEach(pv -> {
  136.                     final FieldVector3D<KK> position = pv.getPosition();
  137.                     interpolator.addSamplePoint(pv.getDate().durationFrom(interpolationDate),
  138.                                                 position.toArray());
  139.                 });
  140.                 break;
  141.             case USE_PV:
  142.                 // populate sample with position and velocity data
  143.                 sample.forEach(pv -> {
  144.                     final FieldVector3D<KK> position = pv.getPosition();
  145.                     final FieldVector3D<KK> velocity = pv.getVelocity();
  146.                     interpolator.addSamplePoint(pv.getDate().durationFrom(interpolationDate),
  147.                                                 position.toArray(), velocity.toArray());
  148.                 });
  149.                 break;
  150.             case USE_PVA:
  151.                 // populate sample with position, velocity and acceleration data
  152.                 sample.forEach(pv -> {
  153.                     final FieldVector3D<KK> position     = pv.getPosition();
  154.                     final FieldVector3D<KK> velocity     = pv.getVelocity();
  155.                     final FieldVector3D<KK> acceleration = pv.getAcceleration();
  156.                     interpolator.addSamplePoint(pv.getDate().durationFrom(interpolationDate),
  157.                                                 position.toArray(), velocity.toArray(), acceleration.toArray());
  158.                 });
  159.                 break;
  160.             default:
  161.                 // this should never happen
  162.                 throw new OrekitInternalError(null);
  163.         }

  164.         // Interpolate
  165.         final KK[][] pva = interpolator.derivatives(interpolationDate.getField().getZero(), 2);

  166.         // Build a new interpolated instance
  167.         return new TimeStampedFieldPVCoordinates<>(interpolationDate, new FieldVector3D<>(pva[0]), new FieldVector3D<>(pva[1]),
  168.                                                    new FieldVector3D<>(pva[2]));
  169.     }
  170. }