1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
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.DisplayName;
24 import org.junit.jupiter.api.Test;
25 import org.mockito.Mockito;
26 import org.orekit.frames.Frame;
27 import org.orekit.frames.FramesFactory;
28 import org.orekit.time.AbsoluteDate;
29 import org.orekit.time.AbstractTimeInterpolator;
30 import org.orekit.time.TimeInterpolator;
31
32 import java.util.ArrayList;
33 import java.util.List;
34 import java.util.Random;
35
36 class AbsolutePVCoordinatesHermiteInterpolatorTest {
37
38 @Test
39 public void testInterpolatePolynomialPVA() {
40 Random random = new Random(0xfe3945fcb8bf47ceL);
41 AbsoluteDate t0 = AbsoluteDate.J2000_EPOCH;
42 Frame frame = FramesFactory.getEME2000();
43 for (int i = 0; i < 20; ++i) {
44
45 PolynomialFunction px = randomPolynomial(5, random);
46 PolynomialFunction py = randomPolynomial(5, random);
47 PolynomialFunction pz = randomPolynomial(5, random);
48 PolynomialFunction pxDot = px.polynomialDerivative();
49 PolynomialFunction pyDot = py.polynomialDerivative();
50 PolynomialFunction pzDot = pz.polynomialDerivative();
51 PolynomialFunction pxDotDot = pxDot.polynomialDerivative();
52 PolynomialFunction pyDotDot = pyDot.polynomialDerivative();
53 PolynomialFunction pzDotDot = pzDot.polynomialDerivative();
54
55
56 List<AbsolutePVCoordinates> sample = new ArrayList<>();
57 for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
58 Vector3D position = new Vector3D(px.value(dt), py.value(dt), pz.value(dt));
59 Vector3D velocity = new Vector3D(pxDot.value(dt), pyDot.value(dt), pzDot.value(dt));
60 Vector3D acceleration = new Vector3D(pxDotDot.value(dt), pyDotDot.value(dt), pzDotDot.value(dt));
61 sample.add(new AbsolutePVCoordinates(frame, t0.shiftedBy(dt), position, velocity, acceleration));
62 }
63
64
65 final TimeInterpolator<AbsolutePVCoordinates> interpolator =
66 new AbsolutePVCoordinatesHermiteInterpolator(sample.size(), frame);
67
68
69 for (double dt = 0; dt < 1.0; dt += 0.01) {
70 AbsolutePVCoordinates interpolated = interpolator.interpolate(t0.shiftedBy(dt), sample);
71 Vector3D p = interpolated.getPosition();
72 Vector3D v = interpolated.getVelocity();
73 Vector3D a = interpolated.getAcceleration();
74 Assertions.assertEquals(px.value(dt), p.getX(), 5.0e-16 * p.getNorm());
75 Assertions.assertEquals(py.value(dt), p.getY(), 5.0e-16 * p.getNorm());
76 Assertions.assertEquals(pz.value(dt), p.getZ(), 5.0e-16 * p.getNorm());
77 Assertions.assertEquals(pxDot.value(dt), v.getX(), 2.0e-15 * v.getNorm());
78 Assertions.assertEquals(pyDot.value(dt), v.getY(), 2.0e-15 * v.getNorm());
79 Assertions.assertEquals(pzDot.value(dt), v.getZ(), 2.0e-15 * v.getNorm());
80 Assertions.assertEquals(pxDotDot.value(dt), a.getX(), 9.0e-15 * a.getNorm());
81 Assertions.assertEquals(pyDotDot.value(dt), a.getY(), 9.0e-15 * a.getNorm());
82 Assertions.assertEquals(pzDotDot.value(dt), a.getZ(), 9.0e-15 * a.getNorm());
83 }
84
85 }
86
87 }
88
89 @Test
90 public void testInterpolatePolynomialPV() {
91 Random random = new Random(0xae7771c9933407bdL);
92 AbsoluteDate t0 = AbsoluteDate.J2000_EPOCH;
93 Frame frame = FramesFactory.getEME2000();
94 for (int i = 0; i < 20; ++i) {
95
96 PolynomialFunction px = randomPolynomial(5, random);
97 PolynomialFunction py = randomPolynomial(5, random);
98 PolynomialFunction pz = randomPolynomial(5, random);
99 PolynomialFunction pxDot = px.polynomialDerivative();
100 PolynomialFunction pyDot = py.polynomialDerivative();
101 PolynomialFunction pzDot = pz.polynomialDerivative();
102 PolynomialFunction pxDotDot = pxDot.polynomialDerivative();
103 PolynomialFunction pyDotDot = pyDot.polynomialDerivative();
104 PolynomialFunction pzDotDot = pzDot.polynomialDerivative();
105
106
107 List<AbsolutePVCoordinates> sample = new ArrayList<>();
108 for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
109 Vector3D position = new Vector3D(px.value(dt), py.value(dt), pz.value(dt));
110 Vector3D velocity = new Vector3D(pxDot.value(dt), pyDot.value(dt), pzDot.value(dt));
111 sample.add(new AbsolutePVCoordinates(frame, t0.shiftedBy(dt), position, velocity, Vector3D.ZERO));
112 }
113
114
115 final TimeInterpolator<AbsolutePVCoordinates> interpolator =
116 new AbsolutePVCoordinatesHermiteInterpolator(sample.size(), frame, CartesianDerivativesFilter.USE_PV);
117
118
119 for (double dt = 0; dt < 1.0; dt += 0.01) {
120 AbsolutePVCoordinates 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 Frame frame = FramesFactory.getEME2000();
144 for (int i = 0; i < 20; ++i) {
145
146 PolynomialFunction px = randomPolynomial(5, random);
147 PolynomialFunction py = randomPolynomial(5, random);
148 PolynomialFunction pz = randomPolynomial(5, random);
149 PolynomialFunction pxDot = px.polynomialDerivative();
150 PolynomialFunction pyDot = py.polynomialDerivative();
151 PolynomialFunction pzDot = pz.polynomialDerivative();
152 PolynomialFunction pxDotDot = pxDot.polynomialDerivative();
153 PolynomialFunction pyDotDot = pyDot.polynomialDerivative();
154 PolynomialFunction pzDotDot = pzDot.polynomialDerivative();
155
156
157 List<AbsolutePVCoordinates> sample = new ArrayList<>();
158 for (double dt : new double[] { 0.0, 0.2, 0.4, 0.6, 0.8, 1.0 }) {
159 Vector3D position = new Vector3D(px.value(dt), py.value(dt), pz.value(dt));
160 sample.add(new AbsolutePVCoordinates(frame, t0.shiftedBy(dt), position, Vector3D.ZERO, Vector3D.ZERO));
161 }
162
163
164 final TimeInterpolator<AbsolutePVCoordinates> interpolator =
165 new AbsolutePVCoordinatesHermiteInterpolator(sample.size(), frame, CartesianDerivativesFilter.USE_P);
166
167
168 for (double dt = 0; dt < 1.0; dt += 0.01) {
169 AbsolutePVCoordinates interpolated = interpolator.interpolate(t0.shiftedBy(dt), sample);
170 Vector3D p = interpolated.getPosition();
171 Vector3D v = interpolated.getVelocity();
172 Vector3D a = interpolated.getAcceleration();
173 Assertions.assertEquals(px.value(dt), p.getX(), 6.0e-16 * p.getNorm());
174 Assertions.assertEquals(py.value(dt), p.getY(), 6.0e-16 * p.getNorm());
175 Assertions.assertEquals(pz.value(dt), p.getZ(), 6.0e-16 * p.getNorm());
176 Assertions.assertEquals(pxDot.value(dt), v.getX(), 1.0e-14 * v.getNorm());
177 Assertions.assertEquals(pyDot.value(dt), v.getY(), 1.0e-14 * v.getNorm());
178 Assertions.assertEquals(pzDot.value(dt), v.getZ(), 1.0e-14 * v.getNorm());
179 Assertions.assertEquals(pxDotDot.value(dt), a.getX(), 2.0e-13 * a.getNorm());
180 Assertions.assertEquals(pyDotDot.value(dt), a.getY(), 2.0e-13 * a.getNorm());
181 Assertions.assertEquals(pzDotDot.value(dt), a.getZ(), 2.0e-13 * a.getNorm());
182 }
183
184 }
185 }
186
187 @Test
188 public void testInterpolateNonPolynomial() {
189 AbsoluteDate t0 = AbsoluteDate.J2000_EPOCH;
190 Frame frame = FramesFactory.getEME2000();
191
192
193 List<AbsolutePVCoordinates> sample = new ArrayList<>();
194 for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
195 Vector3D position = new Vector3D(FastMath.cos(dt), FastMath.sin(dt), 0.0);
196 Vector3D velocity = new Vector3D(-FastMath.sin(dt), FastMath.cos(dt), 0.0);
197 Vector3D acceleration = new Vector3D(-FastMath.cos(dt), -FastMath.sin(dt), 0.0);
198 sample.add(new AbsolutePVCoordinates(frame, t0.shiftedBy(dt), position, velocity, acceleration));
199 }
200
201
202 final TimeInterpolator<AbsolutePVCoordinates> interpolator =
203 new AbsolutePVCoordinatesHermiteInterpolator(sample.size(), frame);
204
205
206 for (double dt = 0; dt < 1.0; dt += 0.01) {
207 AbsolutePVCoordinates interpolated = interpolator.interpolate(t0.shiftedBy(dt), sample);
208 Vector3D p = interpolated.getPosition();
209 Vector3D v = interpolated.getVelocity();
210 Vector3D a = interpolated.getAcceleration();
211 Assertions.assertEquals(FastMath.cos(dt), p.getX(), 3.0e-10 * p.getNorm());
212 Assertions.assertEquals(FastMath.sin(dt), p.getY(), 3.0e-10 * p.getNorm());
213 Assertions.assertEquals(0, p.getZ(), 3.0e-10 * p.getNorm());
214 Assertions.assertEquals(-FastMath.sin(dt), v.getX(), 3.0e-9 * v.getNorm());
215 Assertions.assertEquals(FastMath.cos(dt), v.getY(), 3.0e-9 * v.getNorm());
216 Assertions.assertEquals(0, v.getZ(), 3.0e-9 * v.getNorm());
217 Assertions.assertEquals(-FastMath.cos(dt), a.getX(), 4.0e-8 * a.getNorm());
218 Assertions.assertEquals(-FastMath.sin(dt), a.getY(), 4.0e-8 * a.getNorm());
219 Assertions.assertEquals(0, a.getZ(), 4.0e-8 * a.getNorm());
220 }
221
222 }
223
224 private PolynomialFunction randomPolynomial(int degree, Random random) {
225 double[] coeff = new double[1 + degree];
226 for (int j = 0; j < degree; ++j) {
227 coeff[j] = random.nextDouble();
228 }
229 return new PolynomialFunction(coeff);
230 }
231
232 @Test
233 @DisplayName("Test default constructor")
234 void testDefaultConstructor() {
235
236 final Frame frameMock = Mockito.mock(Frame.class);
237
238
239 final AbsolutePVCoordinatesHermiteInterpolator interpolator =
240 new AbsolutePVCoordinatesHermiteInterpolator(frameMock);
241
242
243 final CartesianDerivativesFilter expectedFilter = CartesianDerivativesFilter.USE_PVA;
244
245 Assertions.assertEquals(AbstractTimeInterpolator.DEFAULT_EXTRAPOLATION_THRESHOLD_SEC,
246 interpolator.getExtrapolationThreshold());
247 Assertions.assertEquals(frameMock, interpolator.getOutputFrame());
248 Assertions.assertEquals(expectedFilter, interpolator.getFilter());
249 }
250
251 }