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