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