1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
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.util.Binary64;
24 import org.hipparchus.util.Binary64Field;
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.mockito.Mockito;
30 import org.orekit.frames.Frame;
31 import org.orekit.frames.FramesFactory;
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 FieldAbsolutePVCoordinatesHermiteInterpolatorTest {
41
42 private PolynomialFunction randomPolynomial(int degree, Random random) {
43 double[] coeff = new double[1 + degree];
44 for (int j = 0; j < degree; ++j) {
45 coeff[j] = random.nextDouble();
46 }
47 return new PolynomialFunction(coeff);
48 }
49
50 @Test
51 @DisplayName("Test default constructor")
52 void testDefaultConstructor() {
53
54 final Frame frameMock = Mockito.mock(Frame.class);
55
56
57 final FieldAbsolutePVCoordinatesHermiteInterpolator<Binary64> interpolator =
58 new FieldAbsolutePVCoordinatesHermiteInterpolator<>(frameMock);
59
60
61 final CartesianDerivativesFilter expectedFilter = CartesianDerivativesFilter.USE_PVA;
62
63 Assertions.assertEquals(AbstractTimeInterpolator.DEFAULT_EXTRAPOLATION_THRESHOLD_SEC,
64 interpolator.getExtrapolationThreshold());
65 Assertions.assertEquals(frameMock, interpolator.getOutputFrame());
66 Assertions.assertEquals(expectedFilter, interpolator.getFilter());
67 }
68
69 @Test
70 void testInterpolateNonPolynomial() {
71 final Field<Binary64> field = Binary64Field.getInstance();
72 final Binary64 one = field.getOne();
73 FieldAbsoluteDate<Binary64> t0 = FieldAbsoluteDate.getJ2000Epoch(field);
74 Frame frame = FramesFactory.getEME2000();
75
76
77 List<FieldAbsolutePVCoordinates<Binary64>> sample = new ArrayList<>();
78 for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
79 FieldVector3D<Binary64> position =
80 new FieldVector3D<>(one.multiply(FastMath.cos(dt)), one.multiply(FastMath.sin(dt)), one.multiply(0.0));
81 FieldVector3D<Binary64> velocity =
82 new FieldVector3D<>(one.multiply(-FastMath.sin(dt)), one.multiply(FastMath.cos(dt)), one.multiply(0.0));
83 FieldVector3D<Binary64> acceleration =
84 new FieldVector3D<>(one.multiply(-FastMath.cos(dt)), one.multiply(-FastMath.sin(dt)), one.multiply(0.0));
85 sample.add(new FieldAbsolutePVCoordinates<>(frame, t0.shiftedBy(one.multiply(dt)), position, velocity,
86 acceleration));
87 }
88
89
90 final FieldTimeInterpolator<FieldAbsolutePVCoordinates<Binary64>, Binary64> interpolator =
91 new FieldAbsolutePVCoordinatesHermiteInterpolator<>(sample.size(), frame,
92 CartesianDerivativesFilter.USE_PVA);
93
94
95 for (double dt = 0; dt < 1.0; dt += 0.01) {
96 FieldAbsolutePVCoordinates<Binary64> interpolated =
97 interpolator.interpolate(t0.shiftedBy(one.multiply(dt)), sample);
98 FieldVector3D<Binary64> p = interpolated.getPosition();
99 FieldVector3D<Binary64> v = interpolated.getVelocity();
100 FieldVector3D<Binary64> a = interpolated.getAcceleration();
101 Assertions.assertEquals(FastMath.cos(dt), p.getX().getReal(), 3.0e-10 * p.getNorm().getReal());
102 Assertions.assertEquals(FastMath.sin(dt), p.getY().getReal(), 3.0e-10 * p.getNorm().getReal());
103 Assertions.assertEquals(0, p.getZ().getReal(), 3.0e-10 * p.getNorm().getReal());
104 Assertions.assertEquals(-FastMath.sin(dt), v.getX().getReal(), 3.0e-9 * v.getNorm().getReal());
105 Assertions.assertEquals(FastMath.cos(dt), v.getY().getReal(), 3.0e-9 * v.getNorm().getReal());
106 Assertions.assertEquals(0, v.getZ().getReal(), 3.0e-9 * v.getNorm().getReal());
107 Assertions.assertEquals(-FastMath.cos(dt), a.getX().getReal(), 4.0e-8 * a.getNorm().getReal());
108 Assertions.assertEquals(-FastMath.sin(dt), a.getY().getReal(), 4.0e-8 * a.getNorm().getReal());
109 Assertions.assertEquals(0, a.getZ().getReal(), 4.0e-8 * a.getNorm().getReal());
110 }
111
112 }
113
114 @Test
115 void testInterpolatePolynomialPositionOnly() {
116 final Field<Binary64> field = Binary64Field.getInstance();
117 final Binary64 one = field.getOne();
118 Random random = new Random(0x88740a12e4299003L);
119 FieldAbsoluteDate<Binary64> t0 = FieldAbsoluteDate.getJ2000Epoch(field);
120 Frame frame = FramesFactory.getEME2000();
121 for (int i = 0; i < 20; ++i) {
122
123 PolynomialFunction px = randomPolynomial(5, random);
124 PolynomialFunction py = randomPolynomial(5, random);
125 PolynomialFunction pz = randomPolynomial(5, random);
126 PolynomialFunction pxDot = px.polynomialDerivative();
127 PolynomialFunction pyDot = py.polynomialDerivative();
128 PolynomialFunction pzDot = pz.polynomialDerivative();
129 PolynomialFunction pxDotDot = pxDot.polynomialDerivative();
130 PolynomialFunction pyDotDot = pyDot.polynomialDerivative();
131 PolynomialFunction pzDotDot = pzDot.polynomialDerivative();
132
133
134 List<FieldAbsolutePVCoordinates<Binary64>> sample = new ArrayList<>();
135 for (double dt : new double[] { 0.0, 0.2, 0.4, 0.6, 0.8, 1.0 }) {
136 FieldVector3D<Binary64> position =
137 new FieldVector3D<>(one.multiply(px.value(dt)), one.multiply(py.value(dt)),
138 one.multiply(pz.value(dt)));
139 sample.add(new FieldAbsolutePVCoordinates<>(frame, t0.shiftedBy(one.multiply(dt)), position,
140 FieldVector3D.getZero(field), FieldVector3D.getZero(field)));
141 }
142
143
144 final FieldTimeInterpolator<FieldAbsolutePVCoordinates<Binary64>, Binary64> interpolator =
145 new FieldAbsolutePVCoordinatesHermiteInterpolator<>(sample.size(), frame,
146 CartesianDerivativesFilter.USE_P);
147
148
149 for (double dt = 0; dt < 1.0; dt += 0.01) {
150 FieldAbsolutePVCoordinates<Binary64> interpolated =
151 interpolator.interpolate(t0.shiftedBy(one.multiply(dt)), sample);
152 FieldVector3D<Binary64> p = interpolated.getPosition();
153 FieldVector3D<Binary64> v = interpolated.getVelocity();
154 FieldVector3D<Binary64> a = interpolated.getAcceleration();
155 Assertions.assertEquals(px.value(dt), p.getX().getReal(), 6.0e-16 * p.getNorm().getReal());
156 Assertions.assertEquals(py.value(dt), p.getY().getReal(), 6.0e-16 * p.getNorm().getReal());
157 Assertions.assertEquals(pz.value(dt), p.getZ().getReal(), 6.0e-16 * p.getNorm().getReal());
158 Assertions.assertEquals(pxDot.value(dt), v.getX().getReal(), 1.0e-14 * v.getNorm().getReal());
159 Assertions.assertEquals(pyDot.value(dt), v.getY().getReal(), 1.0e-14 * v.getNorm().getReal());
160 Assertions.assertEquals(pzDot.value(dt), v.getZ().getReal(), 1.0e-14 * v.getNorm().getReal());
161 Assertions.assertEquals(pxDotDot.value(dt), a.getX().getReal(), 2.0e-13 * a.getNorm().getReal());
162 Assertions.assertEquals(pyDotDot.value(dt), a.getY().getReal(), 2.0e-13 * a.getNorm().getReal());
163 Assertions.assertEquals(pzDotDot.value(dt), a.getZ().getReal(), 2.0e-13 * a.getNorm().getReal());
164 }
165
166 }
167 }
168
169 @Test
170 void testInterpolatePolynomialPV() {
171 final Field<Binary64> field = Binary64Field.getInstance();
172 final Binary64 one = field.getOne();
173 Random random = new Random(0xae7771c9933407bdL);
174 FieldAbsoluteDate<Binary64> t0 = FieldAbsoluteDate.getJ2000Epoch(field);
175 Frame frame = FramesFactory.getEME2000();
176 for (int i = 0; i < 20; ++i) {
177
178 PolynomialFunction px = randomPolynomial(5, random);
179 PolynomialFunction py = randomPolynomial(5, random);
180 PolynomialFunction pz = randomPolynomial(5, random);
181 PolynomialFunction pxDot = px.polynomialDerivative();
182 PolynomialFunction pyDot = py.polynomialDerivative();
183 PolynomialFunction pzDot = pz.polynomialDerivative();
184 PolynomialFunction pxDotDot = pxDot.polynomialDerivative();
185 PolynomialFunction pyDotDot = pyDot.polynomialDerivative();
186 PolynomialFunction pzDotDot = pzDot.polynomialDerivative();
187
188
189 List<FieldAbsolutePVCoordinates<Binary64>> sample = new ArrayList<>();
190 for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
191 FieldVector3D<Binary64> position =
192 new FieldVector3D<>(one.multiply(px.value(dt)), one.multiply(py.value(dt)),
193 one.multiply(pz.value(dt)));
194 FieldVector3D<Binary64> velocity =
195 new FieldVector3D<>(one.multiply(pxDot.value(dt)), one.multiply(pyDot.value(dt)),
196 one.multiply(pzDot.value(dt)));
197 sample.add(new FieldAbsolutePVCoordinates<>(frame, t0.shiftedBy(one.multiply(dt)), position, velocity,
198 FieldVector3D.getZero(field)));
199 }
200
201
202 final FieldTimeInterpolator<FieldAbsolutePVCoordinates<Binary64>, Binary64> interpolator =
203 new FieldAbsolutePVCoordinatesHermiteInterpolator<>(sample.size(), frame,
204 CartesianDerivativesFilter.USE_PV);
205
206
207 for (double dt = 0; dt < 1.0; dt += 0.01) {
208 FieldAbsolutePVCoordinates<Binary64> interpolated =
209 interpolator.interpolate(t0.shiftedBy(one.multiply(dt)), sample);
210 FieldVector3D<Binary64> p = interpolated.getPosition();
211 FieldVector3D<Binary64> v = interpolated.getVelocity();
212 FieldVector3D<Binary64> a = interpolated.getAcceleration();
213 Assertions.assertEquals(px.value(dt), p.getX().getReal(), 4.0e-16 * p.getNorm().getReal());
214 Assertions.assertEquals(py.value(dt), p.getY().getReal(), 4.0e-16 * p.getNorm().getReal());
215 Assertions.assertEquals(pz.value(dt), p.getZ().getReal(), 4.0e-16 * p.getNorm().getReal());
216 Assertions.assertEquals(pxDot.value(dt), v.getX().getReal(), 2.0e-15 * v.getNorm().getReal());
217 Assertions.assertEquals(pyDot.value(dt), v.getY().getReal(), 2.0e-15 * v.getNorm().getReal());
218 Assertions.assertEquals(pzDot.value(dt), v.getZ().getReal(), 2.0e-15 * v.getNorm().getReal());
219 Assertions.assertEquals(pxDotDot.value(dt), a.getX().getReal(), 1.0e-14 * a.getNorm().getReal());
220 Assertions.assertEquals(pyDotDot.value(dt), a.getY().getReal(), 1.0e-14 * a.getNorm().getReal());
221 Assertions.assertEquals(pzDotDot.value(dt), a.getZ().getReal(), 1.0e-14 * a.getNorm().getReal());
222 }
223
224 }
225
226 }
227
228 @Test
229 void testInterpolatePolynomialPVA() {
230 final Field<Binary64> field = Binary64Field.getInstance();
231 final Binary64 one = field.getOne();
232 Random random = new Random(0xfe3945fcb8bf47ceL);
233 FieldAbsoluteDate<Binary64> t0 = FieldAbsoluteDate.getJ2000Epoch(field);
234 Frame frame = FramesFactory.getEME2000();
235 for (int i = 0; i < 20; ++i) {
236
237 PolynomialFunction px = randomPolynomial(5, random);
238 PolynomialFunction py = randomPolynomial(5, random);
239 PolynomialFunction pz = randomPolynomial(5, random);
240 PolynomialFunction pxDot = px.polynomialDerivative();
241 PolynomialFunction pyDot = py.polynomialDerivative();
242 PolynomialFunction pzDot = pz.polynomialDerivative();
243 PolynomialFunction pxDotDot = pxDot.polynomialDerivative();
244 PolynomialFunction pyDotDot = pyDot.polynomialDerivative();
245 PolynomialFunction pzDotDot = pzDot.polynomialDerivative();
246
247
248 List<FieldAbsolutePVCoordinates<Binary64>> sample = new ArrayList<>();
249 for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
250 FieldVector3D<Binary64> position =
251 new FieldVector3D<>(one.multiply(px.value(dt)),
252 one.multiply(py.value(dt)),
253 one.multiply(pz.value(dt)));
254 FieldVector3D<Binary64> velocity =
255 new FieldVector3D<>(one.multiply(pxDot.value(dt)),
256 one.multiply(pyDot.value(dt)),
257 one.multiply(pzDot.value(dt)));
258 FieldVector3D<Binary64> acceleration =
259 new FieldVector3D<>(one.multiply(pxDotDot.value(dt)),
260 one.multiply(pyDotDot.value(dt)),
261 one.multiply(pzDotDot.value(dt)));
262 sample.add(new FieldAbsolutePVCoordinates<>(frame, t0.shiftedBy(one.multiply(dt)),
263 position, velocity, acceleration));
264 }
265
266
267 final FieldTimeInterpolator<FieldAbsolutePVCoordinates<Binary64>, Binary64> interpolator =
268 new FieldAbsolutePVCoordinatesHermiteInterpolator<>(sample.size(), frame,
269 CartesianDerivativesFilter.USE_PVA);
270
271
272 for (double dt = 0; dt < 1.0; dt += 0.01) {
273 FieldAbsolutePVCoordinates<Binary64> interpolated =
274 interpolator.interpolate(t0.shiftedBy(one.multiply(dt)), sample);
275 FieldVector3D<Binary64> p = interpolated.getPosition();
276 FieldVector3D<Binary64> v = interpolated.getVelocity();
277 FieldVector3D<Binary64> a = interpolated.getAcceleration();
278 Assertions.assertEquals(px.value(dt), p.getX().getReal(), 5.0e-16 * p.getNorm().getReal());
279 Assertions.assertEquals(py.value(dt), p.getY().getReal(), 5.0e-16 * p.getNorm().getReal());
280 Assertions.assertEquals(pz.value(dt), p.getZ().getReal(), 5.0e-16 * p.getNorm().getReal());
281 Assertions.assertEquals(pxDot.value(dt), v.getX().getReal(), 2.0e-15 * v.getNorm().getReal());
282 Assertions.assertEquals(pyDot.value(dt), v.getY().getReal(), 2.0e-15 * v.getNorm().getReal());
283 Assertions.assertEquals(pzDot.value(dt), v.getZ().getReal(), 2.0e-15 * v.getNorm().getReal());
284 Assertions.assertEquals(pxDotDot.value(dt), a.getX().getReal(), 9.0e-15 * a.getNorm().getReal());
285 Assertions.assertEquals(pyDotDot.value(dt), a.getY().getReal(), 9.0e-15 * a.getNorm().getReal());
286 Assertions.assertEquals(pzDotDot.value(dt), a.getZ().getReal(), 9.0e-15 * a.getNorm().getReal());
287 }
288
289 }
290
291 }
292
293 }