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  
18  package org.orekit.utils;
19  
20  import org.hipparchus.analysis.polynomials.PolynomialFunction;
21  import org.hipparchus.geometry.euclidean.threed.Vector3D;
22  import org.hipparchus.util.FastMath;
23  import org.junit.jupiter.api.Assertions;
24  import org.junit.jupiter.api.BeforeEach;
25  import org.junit.jupiter.api.DisplayName;
26  import org.junit.jupiter.api.Test;
27  import org.orekit.Utils;
28  import org.orekit.time.AbsoluteDate;
29  import org.orekit.time.AbstractTimeInterpolator;
30  import org.orekit.time.TimeInterpolator;
31  
32  import java.util.ArrayList;
33  import java.util.List;
34  import java.util.Random;
35  
36  class TimeStampedPVCoordinatesHermiteInterpolatorTest {
37  
38      public static PolynomialFunction randomPolynomial(int degree, Random random) {
39          double[] coeff = new double[1 + degree];
40          for (int j = 0; j < degree; ++j) {
41              coeff[j] = random.nextDouble();
42          }
43          return new PolynomialFunction(coeff);
44      }
45  
46      @Test
47      public void testInterpolatePolynomialPVA() {
48          Random       random = new Random(0xfe3945fcb8bf47ceL);
49          AbsoluteDate t0     = AbsoluteDate.J2000_EPOCH;
50          for (int i = 0; i < 20; ++i) {
51  
52              PolynomialFunction px       = randomPolynomial(5, random);
53              PolynomialFunction py       = randomPolynomial(5, random);
54              PolynomialFunction pz       = randomPolynomial(5, random);
55              PolynomialFunction pxDot    = px.polynomialDerivative();
56              PolynomialFunction pyDot    = py.polynomialDerivative();
57              PolynomialFunction pzDot    = pz.polynomialDerivative();
58              PolynomialFunction pxDotDot = pxDot.polynomialDerivative();
59              PolynomialFunction pyDotDot = pyDot.polynomialDerivative();
60              PolynomialFunction pzDotDot = pzDot.polynomialDerivative();
61  
62              List<TimeStampedPVCoordinates> sample = new ArrayList<>();
63              for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
64                  Vector3D position     = new Vector3D(px.value(dt), py.value(dt), pz.value(dt));
65                  Vector3D velocity     = new Vector3D(pxDot.value(dt), pyDot.value(dt), pzDot.value(dt));
66                  Vector3D acceleration = new Vector3D(pxDotDot.value(dt), pyDotDot.value(dt), pzDotDot.value(dt));
67                  sample.add(new TimeStampedPVCoordinates(t0.shiftedBy(dt), position, velocity, acceleration));
68              }
69  
70              // create interpolator
71              final TimeInterpolator<TimeStampedPVCoordinates> interpolator =
72                      new TimeStampedPVCoordinatesHermiteInterpolator(sample.size(), CartesianDerivativesFilter.USE_PVA);
73  
74              for (double dt = 0; dt < 1.0; dt += 0.01) {
75                  TimeStampedPVCoordinates interpolated = interpolator.interpolate(t0.shiftedBy(dt), sample);
76                  Vector3D                 p            = interpolated.getPosition();
77                  Vector3D                 v            = interpolated.getVelocity();
78                  Vector3D                 a            = interpolated.getAcceleration();
79                  Assertions.assertEquals(px.value(dt),       p.getX(), 5.0e-16 * p.getNorm());
80                  Assertions.assertEquals(py.value(dt),       p.getY(), 5.0e-16 * p.getNorm());
81                  Assertions.assertEquals(pz.value(dt),       p.getZ(), 5.0e-16 * p.getNorm());
82                  Assertions.assertEquals(pxDot.value(dt),    v.getX(), 2.0e-15 * v.getNorm());
83                  Assertions.assertEquals(pyDot.value(dt),    v.getY(), 2.0e-15 * v.getNorm());
84                  Assertions.assertEquals(pzDot.value(dt),    v.getZ(), 2.0e-15 * v.getNorm());
85                  Assertions.assertEquals(pxDotDot.value(dt), a.getX(), 9.0e-15 * a.getNorm());
86                  Assertions.assertEquals(pyDotDot.value(dt), a.getY(), 9.0e-15 * a.getNorm());
87                  Assertions.assertEquals(pzDotDot.value(dt), a.getZ(), 9.0e-15 * a.getNorm());
88              }
89  
90          }
91  
92      }
93  
94      @Test
95      public void testInterpolatePolynomialPV() {
96          Random       random = new Random(0xae7771c9933407bdL);
97          AbsoluteDate t0     = AbsoluteDate.J2000_EPOCH;
98          for (int i = 0; i < 20; ++i) {
99  
100             PolynomialFunction px       = randomPolynomial(5, random);
101             PolynomialFunction py       = randomPolynomial(5, random);
102             PolynomialFunction pz       = randomPolynomial(5, random);
103             PolynomialFunction pxDot    = px.polynomialDerivative();
104             PolynomialFunction pyDot    = py.polynomialDerivative();
105             PolynomialFunction pzDot    = pz.polynomialDerivative();
106             PolynomialFunction pxDotDot = pxDot.polynomialDerivative();
107             PolynomialFunction pyDotDot = pyDot.polynomialDerivative();
108             PolynomialFunction pzDotDot = pzDot.polynomialDerivative();
109 
110             List<TimeStampedPVCoordinates> sample = new ArrayList<>();
111             for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
112                 Vector3D position = new Vector3D(px.value(dt), py.value(dt), pz.value(dt));
113                 Vector3D velocity = new Vector3D(pxDot.value(dt), pyDot.value(dt), pzDot.value(dt));
114                 sample.add(new TimeStampedPVCoordinates(t0.shiftedBy(dt), position, velocity, Vector3D.ZERO));
115             }
116 
117             // create interpolator
118             final TimeInterpolator<TimeStampedPVCoordinates> interpolator =
119                     new TimeStampedPVCoordinatesHermiteInterpolator(sample.size(), CartesianDerivativesFilter.USE_PV);
120 
121             for (double dt = 0; dt < 1.0; dt += 0.01) {
122                 TimeStampedPVCoordinates interpolated = interpolator.interpolate(t0.shiftedBy(dt), sample);
123                 Vector3D                 p            = interpolated.getPosition();
124                 Vector3D                 v            = interpolated.getVelocity();
125                 Vector3D                 a            = interpolated.getAcceleration();
126                 Assertions.assertEquals(px.value(dt),       p.getX(), 4.0e-16 * p.getNorm());
127                 Assertions.assertEquals(py.value(dt),       p.getY(), 4.0e-16 * p.getNorm());
128                 Assertions.assertEquals(pz.value(dt),       p.getZ(), 4.0e-16 * p.getNorm());
129                 Assertions.assertEquals(pxDot.value(dt),    v.getX(), 2.0e-15 * v.getNorm());
130                 Assertions.assertEquals(pyDot.value(dt),    v.getY(), 2.0e-15 * v.getNorm());
131                 Assertions.assertEquals(pzDot.value(dt),    v.getZ(), 2.0e-15 * v.getNorm());
132                 Assertions.assertEquals(pxDotDot.value(dt), a.getX(), 1.0e-14 * a.getNorm());
133                 Assertions.assertEquals(pyDotDot.value(dt), a.getY(), 1.0e-14 * a.getNorm());
134                 Assertions.assertEquals(pzDotDot.value(dt), a.getZ(), 1.0e-14 * a.getNorm());
135             }
136 
137         }
138 
139     }
140 
141     @Test
142     public void testInterpolatePolynomialPositionOnly() {
143         Random       random = new Random(0x88740a12e4299003L);
144         AbsoluteDate t0     = AbsoluteDate.J2000_EPOCH;
145         for (int i = 0; i < 20; ++i) {
146 
147             PolynomialFunction px       = randomPolynomial(5, random);
148             PolynomialFunction py       = randomPolynomial(5, random);
149             PolynomialFunction pz       = randomPolynomial(5, random);
150             PolynomialFunction pxDot    = px.polynomialDerivative();
151             PolynomialFunction pyDot    = py.polynomialDerivative();
152             PolynomialFunction pzDot    = pz.polynomialDerivative();
153             PolynomialFunction pxDotDot = pxDot.polynomialDerivative();
154             PolynomialFunction pyDotDot = pyDot.polynomialDerivative();
155             PolynomialFunction pzDotDot = pzDot.polynomialDerivative();
156 
157             List<TimeStampedPVCoordinates> sample = new ArrayList<>();
158             for (double dt : new double[] { 0.0, 0.2, 0.4, 0.6, 0.8, 1.0 }) {
159                 Vector3D position = new Vector3D(px.value(dt), py.value(dt), pz.value(dt));
160                 sample.add(new TimeStampedPVCoordinates(t0.shiftedBy(dt), position, Vector3D.ZERO, Vector3D.ZERO));
161             }
162 
163             // create interpolator
164             final TimeInterpolator<TimeStampedPVCoordinates> interpolator =
165                     new TimeStampedPVCoordinatesHermiteInterpolator(sample.size(), CartesianDerivativesFilter.USE_P);
166 
167             for (double dt = 0; dt < 1.0; dt += 0.01) {
168                 TimeStampedPVCoordinates interpolated = interpolator.interpolate(t0.shiftedBy(dt), sample);
169                 Vector3D                 p            = interpolated.getPosition();
170                 Vector3D                 v            = interpolated.getVelocity();
171                 Vector3D                 a            = interpolated.getAcceleration();
172                 Assertions.assertEquals(px.value(dt),       p.getX(), 6.0e-16 * p.getNorm());
173                 Assertions.assertEquals(py.value(dt),       p.getY(), 6.0e-16 * p.getNorm());
174                 Assertions.assertEquals(pz.value(dt),       p.getZ(), 6.0e-16 * p.getNorm());
175                 Assertions.assertEquals(pxDot.value(dt),    v.getX(), 1.0e-14 * v.getNorm());
176                 Assertions.assertEquals(pyDot.value(dt),    v.getY(), 1.0e-14 * v.getNorm());
177                 Assertions.assertEquals(pzDot.value(dt),    v.getZ(), 1.0e-14 * v.getNorm());
178                 Assertions.assertEquals(pxDotDot.value(dt), a.getX(), 2.0e-13 * a.getNorm());
179                 Assertions.assertEquals(pyDotDot.value(dt), a.getY(), 2.0e-13 * a.getNorm());
180                 Assertions.assertEquals(pzDotDot.value(dt), a.getZ(), 2.0e-13 * a.getNorm());
181             }
182 
183         }
184     }
185 
186     @Test
187     public void testInterpolateNonPolynomial() {
188         AbsoluteDate t0 = AbsoluteDate.J2000_EPOCH;
189 
190         List<TimeStampedPVCoordinates> sample = new ArrayList<>();
191         for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
192             Vector3D position     = new Vector3D(FastMath.cos(dt), FastMath.sin(dt), 0.0);
193             Vector3D velocity     = new Vector3D(-FastMath.sin(dt), FastMath.cos(dt), 0.0);
194             Vector3D acceleration = new Vector3D(-FastMath.cos(dt), -FastMath.sin(dt), 0.0);
195             sample.add(new TimeStampedPVCoordinates(t0.shiftedBy(dt), position, velocity, acceleration));
196         }
197 
198         // create interpolator
199         final TimeInterpolator<TimeStampedPVCoordinates> interpolator =
200                 new TimeStampedPVCoordinatesHermiteInterpolator(sample.size(), CartesianDerivativesFilter.USE_PVA);
201 
202         for (double dt = 0; dt < 1.0; dt += 0.01) {
203             TimeStampedPVCoordinates interpolated = interpolator.interpolate(t0.shiftedBy(dt), sample);
204             Vector3D                 p            = interpolated.getPosition();
205             Vector3D                 v            = interpolated.getVelocity();
206             Vector3D                 a            = interpolated.getAcceleration();
207             Assertions.assertEquals(FastMath.cos(dt), p.getX(), 3.0e-10 * p.getNorm());
208             Assertions.assertEquals(FastMath.sin(dt), p.getY(), 3.0e-10 * p.getNorm());
209             Assertions.assertEquals(0, p.getZ(), 3.0e-10 * p.getNorm());
210             Assertions.assertEquals(-FastMath.sin(dt), v.getX(), 3.0e-9 * v.getNorm());
211             Assertions.assertEquals(FastMath.cos(dt), v.getY(), 3.0e-9 * v.getNorm());
212             Assertions.assertEquals(0, v.getZ(), 3.0e-9 * v.getNorm());
213             Assertions.assertEquals(-FastMath.cos(dt), a.getX(), 4.0e-8 * a.getNorm());
214             Assertions.assertEquals(-FastMath.sin(dt), a.getY(), 4.0e-8 * a.getNorm());
215             Assertions.assertEquals(0, a.getZ(), 4.0e-8 * a.getNorm());
216         }
217 
218     }
219 
220     @Test
221     void testConstructor() {
222         // WHEN
223         final TimeStampedPVCoordinatesHermiteInterpolator interpolator = new TimeStampedPVCoordinatesHermiteInterpolator();
224 
225         // THEN
226         Assertions.assertEquals(AbstractTimeInterpolator.DEFAULT_INTERPOLATION_POINTS,
227                                 interpolator.getNbInterpolationPoints());
228         Assertions.assertEquals(CartesianDerivativesFilter.USE_PVA, interpolator.getFilter());
229 
230     }
231 
232     @Test
233     @DisplayName("Test default constructor and getter")
234     void testDefaultConstructorAndGetter() {
235         // Given
236         final CartesianDerivativesFilter givenFilter = CartesianDerivativesFilter.USE_PVA;
237 
238         final TimeStampedPVCoordinatesHermiteInterpolator interpolator =
239                 new TimeStampedPVCoordinatesHermiteInterpolator(2, givenFilter);
240 
241         // When
242         final CartesianDerivativesFilter gottenFilter = interpolator.getFilter();
243 
244         // Then
245         Assertions.assertEquals(givenFilter, gottenFilter);
246     }
247 
248     @BeforeEach
249     public void setUp() {
250         Utils.setDataRoot("regular-data");
251     }
252 
253 }