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