TimeStampedFieldAngularCoordinatesHermiteInterpolator.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.CalculusFieldElement;
  20. import org.hipparchus.Field;
  21. import org.hipparchus.analysis.interpolation.FieldHermiteInterpolator;
  22. import org.hipparchus.geometry.euclidean.threed.FieldRotation;
  23. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  24. import org.hipparchus.geometry.euclidean.threed.RotationConvention;
  25. import org.hipparchus.util.FastMath;
  26. import org.orekit.errors.OrekitInternalError;
  27. import org.orekit.time.AbstractFieldTimeInterpolator;
  28. import org.orekit.time.FieldAbsoluteDate;

  30. import java.util.List;

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

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

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

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

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

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

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

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

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

  154.         // Get sample
  155.         final List<TimeStampedFieldAngularCoordinates<KK>> sample = interpolationData.getNeighborList();

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

  161.         // set up a linear model canceling mean rotation rate
  162.         final Field<KK> field = interpolationData.getField();
  163.         final FieldVector3D<KK> meanRate;
  164.         FieldVector3D<KK>       sum = FieldVector3D.getZero(field);
  165.         if (filter != AngularDerivativesFilter.USE_R) {
  166.             for (final TimeStampedFieldAngularCoordinates<KK> datedAC : sample) {
  167.                 sum = sum.add(datedAC.getRotationRate());
  168.             }
  169.             meanRate = new FieldVector3D<>(1.0 / sample.size(), sum);
  170.         }
  171.         else {
  172.             TimeStampedFieldAngularCoordinates<KK> previous = null;
  173.             for (final TimeStampedFieldAngularCoordinates<KK> datedAC : sample) {
  174.                 if (previous != null) {
  175.                     sum = sum.add(TimeStampedFieldAngularCoordinates.estimateRate(previous.getRotation(),
  176.                                                                                   datedAC.getRotation(),
  177.                                                                                   datedAC.getDate()
  178.                                                                                          .durationFrom(previous.getDate())));
  179.                 }
  180.                 previous = datedAC;
  181.             }
  182.             meanRate = new FieldVector3D<>(1.0 / (sample.size() - 1), sum);
  183.         }
  184.         TimeStampedFieldAngularCoordinates<KK> offset =
  185.                 new TimeStampedFieldAngularCoordinates<>(interpolationDate, FieldRotation.getIdentity(field),
  186.                                                          meanRate, FieldVector3D.getZero(field));

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

  191.             // offset adaptation parameters
  192.             restart = false;

  194.             // set up an interpolator taking derivatives into account
  195.             final FieldHermiteInterpolator<KK> interpolator = new FieldHermiteInterpolator<>();

  197.             // add sample points
  198.             final KK          one      = interpolationData.getOne();
  199.             double            sign     = 1.0;
  200.             FieldRotation<KK> previous = FieldRotation.getIdentity(field);

  202.             for (final TimeStampedFieldAngularCoordinates<KK> ac : sample) {

  204.                 // remove linear offset from the current coordinates
  205.                 final KK                                     dt    = ac.getDate().durationFrom(interpolationDate);
  206.                 final TimeStampedFieldAngularCoordinates<KK> fixed = ac.subtractOffset(offset.shiftedBy(dt));

  208.                 // make sure all interpolated points will be on the same branch
  209.                 final double dot = one.linearCombination(fixed.getRotation().getQ0(), previous.getQ0(),
  210.                                                          fixed.getRotation().getQ1(), previous.getQ1(),
  211.                                                          fixed.getRotation().getQ2(), previous.getQ2(),
  212.                                                          fixed.getRotation().getQ3(), previous.getQ3()).getReal();
  213.                 sign     = FastMath.copySign(1.0, dot * sign);
  214.                 previous = fixed.getRotation();

  216.                 // check modified Rodrigues vector singularity
  217.                 if (fixed.getRotation().getQ0().getReal() * sign < threshold) {
  218.                     // the sample point is close to a modified Rodrigues vector singularity
  219.                     // we need to change the linear offset model to avoid this
  220.                     restart = true;
  221.                     break;
  222.                 }

  224.                 final KK[][] rodrigues = fixed.getModifiedRodrigues(sign);
  225.                 switch (filter) {
  226.                     case USE_RRA:
  227.                         // populate sample with rotation, rotation rate and acceleration data
  228.                         interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1], rodrigues[2]);
  229.                         break;
  230.                     case USE_RR:
  231.                         // populate sample with rotation and rotation rate data
  232.                         interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1]);
  233.                         break;
  234.                     case USE_R:
  235.                         // populate sample with rotation data only
  236.                         interpolator.addSamplePoint(dt, rodrigues[0]);
  237.                         break;
  238.                     default:
  239.                         // this should never happen
  240.                         throw new OrekitInternalError(null);
  241.                 }
  242.             }

  244.             if (restart) {
  245.                 // interpolation failed, some intermediate rotation was too close to 2π
  246.                 // we need to offset all rotations to avoid the singularity
  247.                 offset = offset.addOffset(
  248.                         new FieldAngularCoordinates<>(new FieldRotation<>(FieldVector3D.getPlusI(field),
  249.                                                                           one.newInstance(epsilon),
  250.                                                                           RotationConvention.VECTOR_OPERATOR),
  251.                                                       FieldVector3D.getZero(field), FieldVector3D.getZero(field)));
  252.             } else {
  253.                 // interpolation succeeded with the current offset
  254.                 final KK                          zero = interpolationData.getZero();
  255.                 final KK[][]                      p    = interpolator.derivatives(zero, 2);
  256.                 final FieldAngularCoordinates<KK> ac   = FieldAngularCoordinates.createFromModifiedRodrigues(p);
  257.                 return new TimeStampedFieldAngularCoordinates<>(offset.getDate(),
  258.                                                                 ac.getRotation(),
  259.                                                                 ac.getRotationRate(),
  260.                                                                 ac.getRotationAcceleration()).addOffset(offset);
  261.             }

  263.         }

  265.         // this should never happen
  266.         throw new OrekitInternalError(null);
  267.     }
  268. }
Created with JaCoCo 0.8.12.202403310830