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