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;
18  
19  import org.hipparchus.analysis.polynomials.PolynomialFunction;
20  import org.hipparchus.geometry.euclidean.threed.Vector3D;
21  import org.hipparchus.util.FastMath;
22  import org.junit.jupiter.api.Assertions;
23  import org.junit.jupiter.api.BeforeEach;
24  import org.junit.jupiter.api.DisplayName;
25  import org.junit.jupiter.api.Test;
26  import org.mockito.Mockito;
27  import org.orekit.Utils;
28  import org.orekit.frames.Frame;
29  import org.orekit.frames.FramesFactory;
30  import org.orekit.time.AbsoluteDate;
31  import org.orekit.time.AbstractTimeInterpolator;
32  import org.orekit.time.TimeInterpolator;
33  
34  import java.util.ArrayList;
35  import java.util.List;
36  import java.util.Random;
37  
38  class AbsolutePVCoordinatesHermiteInterpolatorTest {
39  
40      @Test
41      public void testInterpolatePolynomialPVA() {
42          Random       random = new Random(0xfe3945fcb8bf47ceL);
43          AbsoluteDate t0     = AbsoluteDate.J2000_EPOCH;
44          Frame        frame  = FramesFactory.getEME2000();
45          for (int i = 0; i < 20; ++i) {
46  
47              PolynomialFunction px       = randomPolynomial(5, random);
48              PolynomialFunction py       = randomPolynomial(5, random);
49              PolynomialFunction pz       = randomPolynomial(5, random);
50              PolynomialFunction pxDot    = px.polynomialDerivative();
51              PolynomialFunction pyDot    = py.polynomialDerivative();
52              PolynomialFunction pzDot    = pz.polynomialDerivative();
53              PolynomialFunction pxDotDot = pxDot.polynomialDerivative();
54              PolynomialFunction pyDotDot = pyDot.polynomialDerivative();
55              PolynomialFunction pzDotDot = pzDot.polynomialDerivative();
56  
57              // Create sample
58              List<AbsolutePVCoordinates> sample = new ArrayList<>();
59              for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
60                  Vector3D position     = new Vector3D(px.value(dt), py.value(dt), pz.value(dt));
61                  Vector3D velocity     = new Vector3D(pxDot.value(dt), pyDot.value(dt), pzDot.value(dt));
62                  Vector3D acceleration = new Vector3D(pxDotDot.value(dt), pyDotDot.value(dt), pzDotDot.value(dt));
63                  sample.add(new AbsolutePVCoordinates(frame, t0.shiftedBy(dt), position, velocity, acceleration));
64              }
65  
66              // Create interpolator
67              final TimeInterpolator<AbsolutePVCoordinates> interpolator =
68                      new AbsolutePVCoordinatesHermiteInterpolator(sample.size(), frame);
69  
70              // Interpolate
71              for (double dt = 0; dt < 1.0; dt += 0.01) {
72                  AbsolutePVCoordinates interpolated = interpolator.interpolate(t0.shiftedBy(dt), sample);
73                  Vector3D              p            = interpolated.getPosition();
74                  Vector3D              v            = interpolated.getVelocity();
75                  Vector3D              a            = interpolated.getAcceleration();
76                  Assertions.assertEquals(px.value(dt),       p.getX(), 5.0e-16 * p.getNorm());
77                  Assertions.assertEquals(py.value(dt),       p.getY(), 5.0e-16 * p.getNorm());
78                  Assertions.assertEquals(pz.value(dt),       p.getZ(), 5.0e-16 * p.getNorm());
79                  Assertions.assertEquals(pxDot.value(dt),    v.getX(), 2.0e-15 * v.getNorm());
80                  Assertions.assertEquals(pyDot.value(dt),    v.getY(), 2.0e-15 * v.getNorm());
81                  Assertions.assertEquals(pzDot.value(dt),    v.getZ(), 2.0e-15 * v.getNorm());
82                  Assertions.assertEquals(pxDotDot.value(dt), a.getX(), 9.0e-15 * a.getNorm());
83                  Assertions.assertEquals(pyDotDot.value(dt), a.getY(), 9.0e-15 * a.getNorm());
84                  Assertions.assertEquals(pzDotDot.value(dt), a.getZ(), 9.0e-15 * a.getNorm());
85              }
86  
87          }
88  
89      }
90  
91      @Test
92      public void testInterpolatePolynomialPV() {
93          Random       random = new Random(0xae7771c9933407bdL);
94          AbsoluteDate t0     = AbsoluteDate.J2000_EPOCH;
95          Frame        frame  = FramesFactory.getEME2000();
96          for (int i = 0; i < 20; ++i) {
97  
98              PolynomialFunction px       = randomPolynomial(5, random);
99              PolynomialFunction py       = randomPolynomial(5, random);
100             PolynomialFunction pz       = randomPolynomial(5, random);
101             PolynomialFunction pxDot    = px.polynomialDerivative();
102             PolynomialFunction pyDot    = py.polynomialDerivative();
103             PolynomialFunction pzDot    = pz.polynomialDerivative();
104             PolynomialFunction pxDotDot = pxDot.polynomialDerivative();
105             PolynomialFunction pyDotDot = pyDot.polynomialDerivative();
106             PolynomialFunction pzDotDot = pzDot.polynomialDerivative();
107 
108             // Create sample
109             List<AbsolutePVCoordinates> sample = new ArrayList<>();
110             for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
111                 Vector3D position = new Vector3D(px.value(dt), py.value(dt), pz.value(dt));
112                 Vector3D velocity = new Vector3D(pxDot.value(dt), pyDot.value(dt), pzDot.value(dt));
113                 sample.add(new AbsolutePVCoordinates(frame, t0.shiftedBy(dt), position, velocity, Vector3D.ZERO));
114             }
115 
116             // Create interpolator
117             final TimeInterpolator<AbsolutePVCoordinates> interpolator =
118                     new AbsolutePVCoordinatesHermiteInterpolator(sample.size(), frame, CartesianDerivativesFilter.USE_PV);
119 
120             // Interpolate
121             for (double dt = 0; dt < 1.0; dt += 0.01) {
122                 AbsolutePVCoordinates 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         Frame        frame  = FramesFactory.getEME2000();
146         for (int i = 0; i < 20; ++i) {
147 
148             PolynomialFunction px       = randomPolynomial(5, random);
149             PolynomialFunction py       = randomPolynomial(5, random);
150             PolynomialFunction pz       = randomPolynomial(5, random);
151             PolynomialFunction pxDot    = px.polynomialDerivative();
152             PolynomialFunction pyDot    = py.polynomialDerivative();
153             PolynomialFunction pzDot    = pz.polynomialDerivative();
154             PolynomialFunction pxDotDot = pxDot.polynomialDerivative();
155             PolynomialFunction pyDotDot = pyDot.polynomialDerivative();
156             PolynomialFunction pzDotDot = pzDot.polynomialDerivative();
157 
158             // Create sample
159             List<AbsolutePVCoordinates> sample = new ArrayList<>();
160             for (double dt : new double[] { 0.0, 0.2, 0.4, 0.6, 0.8, 1.0 }) {
161                 Vector3D position = new Vector3D(px.value(dt), py.value(dt), pz.value(dt));
162                 sample.add(new AbsolutePVCoordinates(frame, t0.shiftedBy(dt), position, Vector3D.ZERO, Vector3D.ZERO));
163             }
164 
165             // Create interpolator
166             final TimeInterpolator<AbsolutePVCoordinates> interpolator =
167                     new AbsolutePVCoordinatesHermiteInterpolator(sample.size(), frame, CartesianDerivativesFilter.USE_P);
168 
169             // Interpolate
170             for (double dt = 0; dt < 1.0; dt += 0.01) {
171                 AbsolutePVCoordinates interpolated = interpolator.interpolate(t0.shiftedBy(dt), sample);
172                 Vector3D              p            = interpolated.getPosition();
173                 Vector3D              v            = interpolated.getVelocity();
174                 Vector3D              a            = interpolated.getAcceleration();
175                 Assertions.assertEquals(px.value(dt),       p.getX(), 6.0e-16 * p.getNorm());
176                 Assertions.assertEquals(py.value(dt),       p.getY(), 6.0e-16 * p.getNorm());
177                 Assertions.assertEquals(pz.value(dt),       p.getZ(), 6.0e-16 * p.getNorm());
178                 Assertions.assertEquals(pxDot.value(dt),    v.getX(), 1.0e-14 * v.getNorm());
179                 Assertions.assertEquals(pyDot.value(dt),    v.getY(), 1.0e-14 * v.getNorm());
180                 Assertions.assertEquals(pzDot.value(dt),    v.getZ(), 1.0e-14 * v.getNorm());
181                 Assertions.assertEquals(pxDotDot.value(dt), a.getX(), 2.0e-13 * a.getNorm());
182                 Assertions.assertEquals(pyDotDot.value(dt), a.getY(), 2.0e-13 * a.getNorm());
183                 Assertions.assertEquals(pzDotDot.value(dt), a.getZ(), 2.0e-13 * a.getNorm());
184             }
185 
186         }
187     }
188 
189     @Test
190     public void testInterpolateNonPolynomial() {
191         AbsoluteDate t0    = AbsoluteDate.J2000_EPOCH;
192         Frame        frame = FramesFactory.getEME2000();
193 
194         // Create sample
195         List<AbsolutePVCoordinates> sample = new ArrayList<>();
196         for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
197             Vector3D position     = new Vector3D(FastMath.cos(dt), FastMath.sin(dt), 0.0);
198             Vector3D velocity     = new Vector3D(-FastMath.sin(dt), FastMath.cos(dt), 0.0);
199             Vector3D acceleration = new Vector3D(-FastMath.cos(dt), -FastMath.sin(dt), 0.0);
200             sample.add(new AbsolutePVCoordinates(frame, t0.shiftedBy(dt), position, velocity, acceleration));
201         }
202 
203         // Create interpolator
204         final TimeInterpolator<AbsolutePVCoordinates> interpolator =
205                 new AbsolutePVCoordinatesHermiteInterpolator(sample.size(), frame);
206 
207         // Interpolate
208         for (double dt = 0; dt < 1.0; dt += 0.01) {
209             AbsolutePVCoordinates interpolated = interpolator.interpolate(t0.shiftedBy(dt), sample);
210             Vector3D              p            = interpolated.getPosition();
211             Vector3D              v            = interpolated.getVelocity();
212             Vector3D              a            = interpolated.getAcceleration();
213             Assertions.assertEquals(FastMath.cos(dt), p.getX(), 3.0e-10 * p.getNorm());
214             Assertions.assertEquals(FastMath.sin(dt), p.getY(), 3.0e-10 * p.getNorm());
215             Assertions.assertEquals(0, p.getZ(), 3.0e-10 * p.getNorm());
216             Assertions.assertEquals(-FastMath.sin(dt), v.getX(), 3.0e-9 * v.getNorm());
217             Assertions.assertEquals(FastMath.cos(dt), v.getY(), 3.0e-9 * v.getNorm());
218             Assertions.assertEquals(0, v.getZ(), 3.0e-9 * v.getNorm());
219             Assertions.assertEquals(-FastMath.cos(dt), a.getX(), 4.0e-8 * a.getNorm());
220             Assertions.assertEquals(-FastMath.sin(dt), a.getY(), 4.0e-8 * a.getNorm());
221             Assertions.assertEquals(0, a.getZ(), 4.0e-8 * a.getNorm());
222         }
223 
224     }
225 
226     /**
227      * Test related to issue 1844.
228      *
229      * @see <a href="https://gitlab.orekit.org/orekit/orekit/-/issues/1844">Issue 1844</a>
230      */
231     @Test
232     void testOutputFrame() {
233         // GIVEN
234 
235         // Define output frame
236         final Frame outputFrame = FramesFactory.getEME2000();
237 
238         // Create samples
239         final Frame                       inputFrame = FramesFactory.getGCRF();
240         final List<AbsolutePVCoordinates> samples    = new ArrayList<>();
241 
242         final AbsoluteDate date1 = AbsoluteDate.J2000_EPOCH;
243         final AbsolutePVCoordinates sample1 = new AbsolutePVCoordinates(inputFrame,
244                                                                         date1,
245                                                                         Vector3D.PLUS_I,
246                                                                         Vector3D.PLUS_J);
247 
248         final AbsoluteDate date2 = AbsoluteDate.J2000_EPOCH.shiftedBy(1);
249         final AbsolutePVCoordinates sample2 = new AbsolutePVCoordinates(inputFrame,
250                                                                         date2,
251                                                                         Vector3D.PLUS_J,
252                                                                         Vector3D.MINUS_I);
253 
254         samples.add(sample1);
255         samples.add(sample2);
256 
257         // Create interpolator
258         final AbsolutePVCoordinatesHermiteInterpolator interpolator =
259                 new AbsolutePVCoordinatesHermiteInterpolator(2,
260                                                              outputFrame,
261                                                              CartesianDerivativesFilter.USE_P);
262 
263         // Define interpolation date
264         final AbsoluteDate interpolationDate = AbsoluteDate.J2000_EPOCH.shiftedBy(0.5);
265 
266 
267         // WHEN
268         final AbsolutePVCoordinates interpolated = interpolator.interpolate(interpolationDate, samples);
269 
270         // THEN
271         final PVCoordinates pvInInputFrame = interpolated.getPVCoordinates(inputFrame);
272 
273         // Assert actual output frame is respected
274         Assertions.assertEquals(outputFrame, interpolated.getFrame());
275 
276         // Assert that the interpolated result is correct
277         Assertions.assertEquals(0.5, pvInInputFrame.getPosition().getX(), 1e-15);
278         Assertions.assertEquals(0.5, pvInInputFrame.getPosition().getY(), 1e-15);
279         Assertions.assertEquals(0, pvInInputFrame.getPosition().getZ(), 1e-15);
280 
281         // Assert that input and output frame does not match
282         Assertions.assertNotEquals(inputFrame, outputFrame);
283     }
284 
285     private PolynomialFunction randomPolynomial(int degree, Random random) {
286         double[] coeff = new double[1 + degree];
287         for (int j = 0; j < degree; ++j) {
288             coeff[j] = random.nextDouble();
289         }
290         return new PolynomialFunction(coeff);
291     }
292 
293     @Test
294     @DisplayName("Test default constructor")
295     void testDefaultConstructor() {
296         // Given
297         final Frame frameMock = Mockito.mock(Frame.class);
298 
299         // When
300         final AbsolutePVCoordinatesHermiteInterpolator interpolator =
301                 new AbsolutePVCoordinatesHermiteInterpolator(frameMock);
302 
303         // Then
304         final CartesianDerivativesFilter expectedFilter = CartesianDerivativesFilter.USE_PVA;
305 
306         Assertions.assertEquals(AbstractTimeInterpolator.DEFAULT_EXTRAPOLATION_THRESHOLD_SEC,
307                                 interpolator.getExtrapolationThreshold());
308         Assertions.assertEquals(frameMock, interpolator.getOutputFrame());
309         Assertions.assertEquals(expectedFilter, interpolator.getFilter());
310     }
311 
312     @BeforeEach
313     public void setUp() {
314         Utils.setDataRoot("regular-data");
315     }
316 
317 }