TimeStampedAngularCoordinatesHermiteInterpolator.java

  1. /* Copyright 2002-2025 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;

  19. import org.hipparchus.analysis.interpolation.HermiteInterpolator;
  20. import org.hipparchus.geometry.euclidean.threed.Rotation;
  21. import org.hipparchus.geometry.euclidean.threed.RotationConvention;
  22. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  23. import org.hipparchus.util.FastMath;
  24. import org.hipparchus.util.MathArrays;
  25. import org.orekit.errors.OrekitInternalError;
  26. import org.orekit.time.AbsoluteDate;
  27. import org.orekit.time.AbstractTimeInterpolator;

  29. import java.util.List;

  31. /**
  32.  * Class using Hermite interpolator to interpolate time stamped angular coordinates.
  33.  * <p>
  34.  * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation points
  35.  * (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  36.  * and numerical problems (including NaN appearing).
  37.  *
  38.  * @author Vincent Cucchietti
  39.  * @author Luc Maisonobe
  40.  * @see HermiteInterpolator
  41.  * @see TimeStampedAngularCoordinates
  42.  */
  43. public class TimeStampedAngularCoordinatesHermiteInterpolator
  44.         extends AbstractTimeInterpolator<TimeStampedAngularCoordinates> {

  46.     /** Filter for derivatives from the sample to use in interpolation. */
  47.     private final AngularDerivativesFilter filter;

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

  64.     /**
  65.      * /** Constructor with :
  66.      * <ul>
  67.      *     <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
  68.      *     <li>Use of angular and first time derivative for attitude interpolation</li>
  69.      * </ul>
  70.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  71.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  72.      * phenomenon</a> and numerical problems (including NaN appearing).
  73.      *
  74.      * @param interpolationPoints number of interpolation points
  75.      */
  76.     public TimeStampedAngularCoordinatesHermiteInterpolator(final int interpolationPoints) {
  77.         this(interpolationPoints, AngularDerivativesFilter.USE_RR);
  78.     }

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

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

  115.     /** Get filter for derivatives from the sample to use in interpolation.
  116.      * @return filter for derivatives from the sample to use in interpolation
  117.      */
  118.     public AngularDerivativesFilter getFilter() {
  119.         return filter;
  120.     }

  122.     /**
  123.      * {@inheritDoc}
  124.      * <p>
  125.      * The interpolated instance is created by polynomial Hermite interpolation on Rodrigues vector ensuring rotation rate
  126.      * remains the exact derivative of rotation.
  127.      * <p>
  128.      * This method is based on Sergei Tanygin's paper <a
  129.      * href="http://www.agi.com/resources/white-papers/attitude-interpolation">Attitude Interpolation</a>, changing the norm
  130.      * of the vector to match the modified Rodrigues vector as described in Malcolm D. Shuster's paper <a
  131.      * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A
  132.      * Survey of Attitude Representations</a>. This change avoids the singularity at π. There is still a singularity at 2π,
  133.      * which is handled by slightly offsetting all rotations when this singularity is detected. Another change is that the
  134.      * mean linear motion is first removed before interpolation and added back after interpolation. This allows to use
  135.      * interpolation even when the sample covers much more than one turn and even when sample points are separated by more
  136.      * than one turn.
  137.      * </p>
  138.      * <p>
  139.      * Note that even if first and second time derivatives (rotation rates and acceleration) from sample can be ignored, the
  140.      * interpolated instance always includes interpolated derivatives. This feature can be used explicitly to compute these
  141.      * derivatives when it would be too complex to compute them from an analytical formula: just compute a few sample points
  142.      * from the explicit formula and set the derivatives to zero in these sample points, then use interpolation to add
  143.      * derivatives consistent with the rotations.
  144.      */
  145.     @Override
  146.     protected TimeStampedAngularCoordinates interpolate(final InterpolationData interpolationData) {

  148.         // Get date
  149.         final AbsoluteDate date = interpolationData.getInterpolationDate();

  151.         // Get sample
  152.         final List<TimeStampedAngularCoordinates> sample = interpolationData.getNeighborList();

  154.         // set up safety elements for 2π singularity avoidance
  155.         final double epsilon   = 2 * FastMath.PI / sample.size();
  156.         final double threshold = FastMath.min(-(1.0 - 1.0e-4), -FastMath.cos(epsilon / 4));

  158.         // set up a linear model canceling mean rotation rate
  159.         final Vector3D meanRate;
  160.         Vector3D       sum = Vector3D.ZERO;
  161.         if (filter != AngularDerivativesFilter.USE_R) {
  162.             for (final TimeStampedAngularCoordinates datedAC : sample) {
  163.                 sum = sum.add(datedAC.getRotationRate());
  164.             }
  165.             meanRate = new Vector3D(1.0 / sample.size(), sum);
  166.         }
  167.         else {
  168.             TimeStampedAngularCoordinates previous = null;
  169.             for (final TimeStampedAngularCoordinates datedAC : sample) {
  170.                 if (previous != null) {
  171.                     sum = sum.add(TimeStampedAngularCoordinates.estimateRate(previous.getRotation(), datedAC.getRotation(),
  172.                                                                              datedAC.getDate()
  173.                                                                                     .durationFrom(previous.getDate())));
  174.                 }
  175.                 previous = datedAC;
  176.             }
  177.             meanRate = new Vector3D(1.0 / (sample.size() - 1), sum);
  178.         }
  179.         TimeStampedAngularCoordinates offset =
  180.                 new TimeStampedAngularCoordinates(date, Rotation.IDENTITY, meanRate, Vector3D.ZERO);

  182.         boolean restart = true;
  183.         for (int i = 0; restart && i < sample.size() + 2; ++i) {

  185.             // offset adaptation parameters
  186.             restart = false;

  188.             // set up an interpolator taking derivatives into account
  189.             final HermiteInterpolator interpolator = new HermiteInterpolator();

  191.             // add sample points
  192.             double   sign     = 1.0;
  193.             Rotation previous = Rotation.IDENTITY;

  195.             for (final TimeStampedAngularCoordinates ac : sample) {

  197.                 // remove linear offset from the current coordinates
  198.                 final double                        dt    = ac.getDate().durationFrom(date);
  199.                 final TimeStampedAngularCoordinates fixed = ac.subtractOffset(offset.shiftedBy(dt));

  201.                 // make sure all interpolated points will be on the same branch
  202.                 final double dot = MathArrays.linearCombination(fixed.getRotation().getQ0(), previous.getQ0(),
  203.                                                                 fixed.getRotation().getQ1(), previous.getQ1(),
  204.                                                                 fixed.getRotation().getQ2(), previous.getQ2(),
  205.                                                                 fixed.getRotation().getQ3(), previous.getQ3());
  206.                 sign     = FastMath.copySign(1.0, dot * sign);
  207.                 previous = fixed.getRotation();

  209.                 // check modified Rodrigues vector singularity
  210.                 if (fixed.getRotation().getQ0() * sign < threshold) {
  211.                     // the sample point is close to a modified Rodrigues vector singularity
  212.                     // we need to change the linear offset model to avoid this
  213.                     restart = true;
  214.                     break;
  215.                 }

  217.                 final double[][] rodrigues = fixed.getModifiedRodrigues(sign);
  218.                 switch (filter) {
  219.                     case USE_RRA:
  220.                         // populate sample with rotation, rotation rate and acceleration data
  221.                         interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1], rodrigues[2]);
  222.                         break;
  223.                     case USE_RR:
  224.                         // populate sample with rotation and rotation rate data
  225.                         interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1]);
  226.                         break;
  227.                     case USE_R:
  228.                         // populate sample with rotation data only
  229.                         interpolator.addSamplePoint(dt, rodrigues[0]);
  230.                         break;
  231.                     default:
  232.                         // this should never happen
  233.                         throw new OrekitInternalError(null);
  234.                 }
  235.             }

  237.             if (restart) {
  238.                 // interpolation failed, some intermediate rotation was too close to 2π
  239.                 // we need to offset all rotations to avoid the singularity
  240.                 offset = offset.addOffset(new AngularCoordinates(new Rotation(Vector3D.PLUS_I,
  241.                                                                               epsilon,
  242.                                                                               RotationConvention.VECTOR_OPERATOR),
  243.                                                                  Vector3D.ZERO, Vector3D.ZERO));
  244.             }
  245.             else {
  246.                 // interpolation succeeded with the current offset
  247.                 final double[][]         p  = interpolator.derivatives(0.0, 2);
  248.                 final AngularCoordinates ac = AngularCoordinates.createFromModifiedRodrigues(p);
  249.                 return new TimeStampedAngularCoordinates(offset.getDate(),
  250.                                                          ac.getRotation(),
  251.                                                          ac.getRotationRate(),
  252.                                                          ac.getRotationAcceleration()).addOffset(offset);
  253.             }

  255.         }

  257.         // this should never happen
  258.         throw new OrekitInternalError(null);
  259.     }
  260. }
Created with JaCoCo 0.8.12.202403310830