1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.utils;
18
19 import static org.junit.Assert.assertEquals;
20
21 import java.util.ArrayList;
22 import java.util.List;
23 import java.util.Random;
24
25 import org.hipparchus.Field;
26 import org.hipparchus.CalculusFieldElement;
27 import org.hipparchus.analysis.differentiation.FieldDerivativeStructure;
28 import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative1;
29 import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
30 import org.hipparchus.analysis.polynomials.PolynomialFunction;
31 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
32 import org.hipparchus.geometry.euclidean.threed.Vector3D;
33 import org.hipparchus.util.Decimal64Field;
34 import org.hipparchus.util.FastMath;
35 import org.junit.Assert;
36 import org.junit.Before;
37 import org.junit.Test;
38 import org.orekit.Utils;
39 import org.orekit.frames.Frame;
40 import org.orekit.frames.FramesFactory;
41 import org.orekit.time.FieldAbsoluteDate;
42
43 public class FieldAbsolutePVCoordinatesTest {
44
45 @Before
46 public void setUp() {
47 Utils.setDataRoot("regular-data");
48 }
49
50 @Test
51 public void TestPVOnlyConstructor() {
52 doTestPVOnlyConstructor(Decimal64Field.getInstance());
53 }
54
55 @Test
56 public void testPVCoordinatesCopyConstructor() {
57 doTestPVCoordinatesCopyConstructor(Decimal64Field.getInstance());
58 }
59
60 @Test
61 public void testLinearConstructors() {
62 doTestLinearConstructors(Decimal64Field.getInstance());
63 }
64
65 @Test
66 public void testToDerivativeStructureVector1() {
67 doTestToDerivativeStructureVector1(Decimal64Field.getInstance());
68 }
69
70 @Test
71 public void testToDerivativeStructureVector2() {
72 doTestToDerivativeStructureVector2(Decimal64Field.getInstance());
73 }
74
75 @Test
76 public void testToUnivariateDerivative1Vector() {
77 doTestToUnivariateDerivative1Vector(Decimal64Field.getInstance());
78 }
79
80 @Test
81 public void testToUnivariateDerivative2Vector() {
82 doTestToUnivariateDerivative2Vector(Decimal64Field.getInstance());
83 }
84
85 @Test
86 public void testShift() {
87 doTestShift(Decimal64Field.getInstance());
88 }
89
90 @Test
91 public void testToString() {
92 doTestToString(Decimal64Field.getInstance());
93 }
94
95 @Test
96 public void testInterpolatePolynomialPVA() {
97 doTestInterpolatePolynomialPVA(Decimal64Field.getInstance());
98 }
99
100 @Test
101 public void testInterpolatePolynomialPV() {
102 doTestInterpolatePolynomialPV(Decimal64Field.getInstance());
103 }
104
105 @Test
106 public void testInterpolatePolynomialPositionOnly() {
107 doTestInterpolatePolynomialPositionOnly(Decimal64Field.getInstance());
108 }
109
110 @Test
111 public void testInterpolateNonPolynomial() {
112 doTestInterpolateNonPolynomial(Decimal64Field.getInstance());
113 }
114
115 @Test
116 public void testSamePV() {
117 doTestSamePV(Decimal64Field.getInstance());
118 }
119
120 @Test
121 public void testTaylorProvider() {
122 doTestTaylorProvider(Decimal64Field.getInstance());
123 }
124
125 private <T extends CalculusFieldElement<T>> void doTestPVOnlyConstructor(Field<T> field) {
126
127 FieldAbsoluteDate<T> date = FieldAbsoluteDate.getJ2000Epoch(field);
128 Frame frame = FramesFactory.getEME2000();
129 final T one = field.getOne();
130 FieldVector3D<T> p = new FieldVector3D<>(one, one.multiply(2.0), one.multiply(3.0));
131 FieldVector3D<T> v = new FieldVector3D<>(one.multiply(4.0), one.multiply(5.0), one.multiply(6.0));
132
133
134 FieldAbsolutePVCoordinates<T> actual = new FieldAbsolutePVCoordinates<>(frame, date, p, v);
135
136
137 Assert.assertEquals(date, actual.getDate());
138 Assert.assertEquals(1.0, actual.getPosition().getX().getReal(), 0.0);
139 Assert.assertEquals(2.0, actual.getPosition().getY().getReal(), 0.0);
140 Assert.assertEquals(3.0, actual.getPosition().getZ().getReal(), 0.0);
141 Assert.assertEquals(4.0, actual.getVelocity().getX().getReal(), 0.0);
142 Assert.assertEquals(5.0, actual.getVelocity().getY().getReal(), 0.0);
143 Assert.assertEquals(6.0, actual.getVelocity().getZ().getReal(), 0.0);
144 Assert.assertEquals(FieldVector3D.getZero(field), actual.getAcceleration());
145 }
146
147 private <T extends CalculusFieldElement<T>> void doTestPVCoordinatesCopyConstructor(Field<T> field) {
148
149 FieldAbsoluteDate<T> date = FieldAbsoluteDate.getJ2000Epoch(field);
150 Frame frame = FramesFactory.getEME2000();
151 final T one = field.getOne();
152 FieldPVCoordinates<T> pv = new FieldPVCoordinates<>(new FieldVector3D<>(one, one.multiply(2), one.multiply(3)), new FieldVector3D<>(one.multiply(4), one.multiply(5), one.multiply(6)));
153
154
155 FieldAbsolutePVCoordinates<T> actual = new FieldAbsolutePVCoordinates<>(frame, date, pv);
156
157
158 Assert.assertEquals(date, actual.getDate());
159 Assert.assertEquals(1.0, actual.getPosition().getX().getReal(), 0.0);
160 Assert.assertEquals(2.0, actual.getPosition().getY().getReal(), 0.0);
161 Assert.assertEquals(3.0, actual.getPosition().getZ().getReal(), 0.0);
162 Assert.assertEquals(4.0, actual.getVelocity().getX().getReal(), 0.0);
163 Assert.assertEquals(5.0, actual.getVelocity().getY().getReal(), 0.0);
164 Assert.assertEquals(6.0, actual.getVelocity().getZ().getReal(), 0.0);
165 Assert.assertEquals(FieldVector3D.getZero(field), actual.getAcceleration());
166 }
167
168 private <T extends CalculusFieldElement<T>> void doTestLinearConstructors(Field<T> field) {
169 Frame frame = FramesFactory.getEME2000();
170 final T one = field.getOne();
171 FieldAbsolutePVCoordinates<T> pv1 = new FieldAbsolutePVCoordinates<>(frame,FieldAbsoluteDate.getCCSDSEpoch(field),
172 new FieldVector3D<>(one, one.multiply(0.1), one.multiply(10.0)),
173 new FieldVector3D<>(one.multiply(-1.0), one.multiply(-0.1), one.multiply(-10.0)),
174 new FieldVector3D<>(one.multiply(10.0), one.multiply(-1.0), one.multiply(-100.0)));
175 FieldAbsolutePVCoordinates<T> pv2 = new FieldAbsolutePVCoordinates<>(frame,FieldAbsoluteDate.getFiftiesEpoch(field),
176 new FieldVector3D<>(one.multiply(2.0), one.multiply(0.2), one.multiply(20.0)),
177 new FieldVector3D<>(one.multiply(-2.0), one.multiply(-0.2), one.multiply(-20.0)),
178 new FieldVector3D<>(one.multiply(20.0), one.multiply(-2.0), one.multiply(-200.0)));
179 FieldAbsolutePVCoordinates<T> pv3 = new FieldAbsolutePVCoordinates<>(frame,FieldAbsoluteDate.getGalileoEpoch(field),
180 new FieldVector3D<>(one.multiply(3.0), one.multiply(0.3), one.multiply(30.0)),
181 new FieldVector3D<>(one.multiply(-3.0), one.multiply(-0.3), one.multiply(-30.0)),
182 new FieldVector3D<>(one.multiply(30.0), one.multiply(-3.0), one.multiply(-300.0)));
183 FieldAbsolutePVCoordinates<T> pv4 = new FieldAbsolutePVCoordinates<>(frame,FieldAbsoluteDate.getJulianEpoch(field),
184 new FieldVector3D<>(one.multiply(4.0), one.multiply(0.4), one.multiply(40.0)),
185 new FieldVector3D<>(one.multiply(-4.0), one.multiply(-0.4), one.multiply(-40.0)),
186 new FieldVector3D<>(one.multiply(40.0), one.multiply(-4.0), one.multiply(-400.0)));
187 checkPV(pv4, new FieldAbsolutePVCoordinates<T>(FieldAbsoluteDate.getJulianEpoch(field), one.multiply(4.0), pv1), 1.0e-15);
188 checkPV(pv2, new FieldAbsolutePVCoordinates<T>(FieldAbsoluteDate.getFiftiesEpoch(field), pv1, pv3), 1.0e-15);
189 checkPV(pv3, new FieldAbsolutePVCoordinates<T>(FieldAbsoluteDate.getGalileoEpoch(field), one, pv1, one, pv2), 1.0e-15);
190 checkPV(new FieldAbsolutePVCoordinates<T>(FieldAbsoluteDate.getJ2000Epoch(field), one.multiply(2.0), pv4),
191 new FieldAbsolutePVCoordinates<T>(FieldAbsoluteDate.getJ2000Epoch(field), one.multiply(3.0), pv1, one, pv2, one, pv3),
192 1.0e-15);
193 checkPV(new FieldAbsolutePVCoordinates<T>(FieldAbsoluteDate.getJ2000Epoch(field), one.multiply(3.0), pv3),
194 new FieldAbsolutePVCoordinates<T>(FieldAbsoluteDate.getJ2000Epoch(field), one.multiply(3.0), pv1, one, pv2, one, pv4),
195 1.0e-15);
196 checkPV(new FieldAbsolutePVCoordinates<T>(FieldAbsoluteDate.getJ2000Epoch(field), one.multiply(5.0), pv4),
197 new FieldAbsolutePVCoordinates<T>(FieldAbsoluteDate.getJ2000Epoch(field), one.multiply(4.0), pv1, one.multiply(3.0), pv2, one.multiply(2.0), pv3, one, pv4),
198 1.0e-15);
199 }
200
201 private <T extends CalculusFieldElement<T>> void doTestToDerivativeStructureVector1(Field<T> field) {
202 final T one = field.getOne();
203 FieldVector3D<FieldDerivativeStructure<T>> fv =
204 new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(),
205 FieldAbsoluteDate.getGalileoEpoch(field),
206 new FieldVector3D<>(one, one.multiply(0.1), one.multiply(10.0)),
207 new FieldVector3D<>(one.multiply(-1.0), one.multiply(-0.1), one.multiply(-10.0)),
208 new FieldVector3D<>(one.multiply(10.0), one.multiply(-1.0), one.multiply(-100.0))).toDerivativeStructureVector(1);
209
210 Assert.assertEquals(1, fv.getX().getFreeParameters());
211 Assert.assertEquals(1, fv.getX().getOrder());
212 Assert.assertEquals( 1.0, fv.getX().getReal(), 1.0e-10);
213 Assert.assertEquals( 0.1, fv.getY().getReal(), 1.0e-10);
214 Assert.assertEquals( 10.0, fv.getZ().getReal(), 1.0e-10);
215 Assert.assertEquals( -1.0, fv.getX().getPartialDerivative(1).getReal(), 1.0e-15);
216 Assert.assertEquals( -0.1, fv.getY().getPartialDerivative(1).getReal(), 1.0e-15);
217 Assert.assertEquals( -10.0, fv.getZ().getPartialDerivative(1).getReal(), 1.0e-15);
218 checkPV(new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(),
219 FieldAbsoluteDate.getGalileoEpoch(field),
220 new FieldVector3D<>(one, one.multiply(0.1), one.multiply(10.0)),
221 new FieldVector3D<>(one.multiply(-1.0), one.multiply(-0.1), one.multiply(-10.0)),
222 FieldVector3D.getZero(field)),
223 new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(),
224 FieldAbsoluteDate.getGalileoEpoch(field), fv), 1.0e-15);
225
226 for (double dt = 0; dt < 10; dt += 0.125) {
227 Vector3D p = new PVCoordinates(new Vector3D( 1, 0.1, 10),
228 new Vector3D(-1, -0.1, -10)).shiftedBy(dt).getPosition();
229 Assert.assertEquals(p.getX(), fv.getX().taylor(dt).getReal(), 1.0e-14);
230 Assert.assertEquals(p.getY(), fv.getY().taylor(dt).getReal(), 1.0e-14);
231 Assert.assertEquals(p.getZ(), fv.getZ().taylor(dt).getReal(), 1.0e-14);
232 }
233
234 FieldAbsolutePVCoordinates<T> pv =
235 new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(),
236 FieldAbsoluteDate.getGalileoEpoch(field),
237 fv);
238 Assert.assertEquals( 1.0, pv.getPosition().getX().getReal(), 1.0e-10);
239 Assert.assertEquals( 0.1, pv.getPosition().getY().getReal(), 1.0e-10);
240 Assert.assertEquals( 10.0, pv.getPosition().getZ().getReal(), 1.0e-10);
241 Assert.assertEquals( -1.0, pv.getVelocity().getX().getReal(), 1.0e-15);
242 Assert.assertEquals( -0.1, pv.getVelocity().getY().getReal(), 1.0e-15);
243 Assert.assertEquals( -10.0, pv.getVelocity().getZ().getReal(), 1.0e-15);
244
245 }
246
247 private <T extends CalculusFieldElement<T>> void doTestToDerivativeStructureVector2(Field<T> field) {
248 final T one = field.getOne();
249 FieldVector3D<FieldDerivativeStructure<T>> fv =
250 new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(),
251 FieldAbsoluteDate.getGalileoEpoch(field),
252 new FieldVector3D<>(one, one.multiply(0.1), one.multiply(10.0)),
253 new FieldVector3D<>(one.multiply(-1.0), one.multiply(-0.1), one.multiply(-10.0)),
254 new FieldVector3D<>(one.multiply(10.0), one.multiply(-1.0), one.multiply(-100.0))).toDerivativeStructureVector(2);
255
256 Assert.assertEquals(1, fv.getX().getFreeParameters());
257 Assert.assertEquals(2, fv.getX().getOrder());
258 Assert.assertEquals( 1.0, fv.getX().getReal(), 1.0e-10);
259 Assert.assertEquals( 0.1, fv.getY().getReal(), 1.0e-10);
260 Assert.assertEquals( 10.0, fv.getZ().getReal(), 1.0e-10);
261 Assert.assertEquals( -1.0, fv.getX().getPartialDerivative(1).getReal(), 1.0e-15);
262 Assert.assertEquals( -0.1, fv.getY().getPartialDerivative(1).getReal(), 1.0e-15);
263 Assert.assertEquals( -10.0, fv.getZ().getPartialDerivative(1).getReal(), 1.0e-15);
264 Assert.assertEquals( 10.0, fv.getX().getPartialDerivative(2).getReal(), 1.0e-15);
265 Assert.assertEquals( -1.0, fv.getY().getPartialDerivative(2).getReal(), 1.0e-15);
266 Assert.assertEquals(-100.0, fv.getZ().getPartialDerivative(2).getReal(), 1.0e-15);
267 checkPV(new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(),
268 FieldAbsoluteDate.getGalileoEpoch(field),
269 new FieldVector3D<>(one, one.multiply(0.1), one.multiply(10.0)),
270 new FieldVector3D<>(one.multiply(-1.0), one.multiply(-0.1), one.multiply(-10.0)),
271 new FieldVector3D<>(one.multiply(10.0), one.multiply(-1.0), one.multiply(-100.0))),
272 new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(),
273 FieldAbsoluteDate.getGalileoEpoch(field), fv), 1.0e-15);
274
275 for (double dt = 0; dt < 10; dt += 0.125) {
276 Vector3D p = new PVCoordinates(new Vector3D( 1, 0.1, 10),
277 new Vector3D(-1, -0.1, -10),
278 new Vector3D(10, -1.0, -100)).shiftedBy(dt).getPosition();
279 Assert.assertEquals(p.getX(), fv.getX().taylor(dt).getReal(), 1.0e-14);
280 Assert.assertEquals(p.getY(), fv.getY().taylor(dt).getReal(), 1.0e-14);
281 Assert.assertEquals(p.getZ(), fv.getZ().taylor(dt).getReal(), 1.0e-14);
282 }
283
284 FieldAbsolutePVCoordinates<T> pv =
285 new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(),
286 FieldAbsoluteDate.getGalileoEpoch(field),
287 fv);
288 Assert.assertEquals( 1.0, pv.getPosition().getX().getReal(), 1.0e-10);
289 Assert.assertEquals( 0.1, pv.getPosition().getY().getReal(), 1.0e-10);
290 Assert.assertEquals( 10.0, pv.getPosition().getZ().getReal(), 1.0e-10);
291 Assert.assertEquals( -1.0, pv.getVelocity().getX().getReal(), 1.0e-15);
292 Assert.assertEquals( -0.1, pv.getVelocity().getY().getReal(), 1.0e-15);
293 Assert.assertEquals( -10.0, pv.getVelocity().getZ().getReal(), 1.0e-15);
294 Assert.assertEquals( 10.0, pv.getAcceleration().getX().getReal(), 1.0e-15);
295 Assert.assertEquals( -1.0, pv.getAcceleration().getY().getReal(), 1.0e-15);
296 Assert.assertEquals(-100.0, pv.getAcceleration().getZ().getReal(), 1.0e-15);
297
298 }
299
300 private <T extends CalculusFieldElement<T>> void doTestToUnivariateDerivative1Vector(Field<T> field) {
301 final T one = field.getOne();
302 FieldVector3D<FieldUnivariateDerivative1<T>> fv =
303 new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(),
304 FieldAbsoluteDate.getGalileoEpoch(field),
305 new FieldVector3D<>(one, one.multiply(0.1), one.multiply(10.0)),
306 new FieldVector3D<>(one.multiply(-1.0), one.multiply(-0.1), one.multiply(-10.0)),
307 new FieldVector3D<>(one.multiply(10.0), one.multiply(-1.0), one.multiply(-100.0))).toUnivariateDerivative1Vector();
308
309 Assert.assertEquals(1, fv.getX().getFreeParameters());
310 Assert.assertEquals(1, fv.getX().getOrder());
311 Assert.assertEquals( 1.0, fv.getX().getReal(), 1.0e-10);
312 Assert.assertEquals( 0.1, fv.getY().getReal(), 1.0e-10);
313 Assert.assertEquals( 10.0, fv.getZ().getReal(), 1.0e-10);
314 Assert.assertEquals( -1.0, fv.getX().getPartialDerivative(1).getReal(), 1.0e-15);
315 Assert.assertEquals( -0.1, fv.getY().getPartialDerivative(1).getReal(), 1.0e-15);
316 Assert.assertEquals( -10.0, fv.getZ().getPartialDerivative(1).getReal(), 1.0e-15);
317 checkPV(new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(),
318 FieldAbsoluteDate.getGalileoEpoch(field),
319 new FieldVector3D<>(one, one.multiply(0.1), one.multiply(10.0)),
320 new FieldVector3D<>(one.multiply(-1.0), one.multiply(-0.1), one.multiply(-10.0)),
321 FieldVector3D.getZero(field)),
322 new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(),
323 FieldAbsoluteDate.getGalileoEpoch(field), fv), 1.0e-15);
324
325 for (double dt = 0; dt < 10; dt += 0.125) {
326 Vector3D p = new PVCoordinates(new Vector3D( 1, 0.1, 10),
327 new Vector3D(-1, -0.1, -10)).shiftedBy(dt).getPosition();
328 Assert.assertEquals(p.getX(), fv.getX().taylor(dt).getReal(), 1.0e-14);
329 Assert.assertEquals(p.getY(), fv.getY().taylor(dt).getReal(), 1.0e-14);
330 Assert.assertEquals(p.getZ(), fv.getZ().taylor(dt).getReal(), 1.0e-14);
331 }
332
333 FieldAbsolutePVCoordinates<T> pv =
334 new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(),
335 FieldAbsoluteDate.getGalileoEpoch(field),
336 fv);
337 Assert.assertEquals( 1.0, pv.getPosition().getX().getReal(), 1.0e-10);
338 Assert.assertEquals( 0.1, pv.getPosition().getY().getReal(), 1.0e-10);
339 Assert.assertEquals( 10.0, pv.getPosition().getZ().getReal(), 1.0e-10);
340 Assert.assertEquals( -1.0, pv.getVelocity().getX().getReal(), 1.0e-15);
341 Assert.assertEquals( -0.1, pv.getVelocity().getY().getReal(), 1.0e-15);
342 Assert.assertEquals( -10.0, pv.getVelocity().getZ().getReal(), 1.0e-15);
343
344 }
345
346 private <T extends CalculusFieldElement<T>> void doTestToUnivariateDerivative2Vector(Field<T> field) {
347 final T one = field.getOne();
348 FieldVector3D<FieldUnivariateDerivative2<T>> fv =
349 new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(),
350 FieldAbsoluteDate.getGalileoEpoch(field),
351 new FieldVector3D<>(one, one.multiply(0.1), one.multiply(10.0)),
352 new FieldVector3D<>(one.multiply(-1.0), one.multiply(-0.1), one.multiply(-10.0)),
353 new FieldVector3D<>(one.multiply(10.0), one.multiply(-1.0), one.multiply(-100.0))).toUnivariateDerivative2Vector();
354
355 Assert.assertEquals(1, fv.getX().getFreeParameters());
356 Assert.assertEquals(2, fv.getX().getOrder());
357 Assert.assertEquals( 1.0, fv.getX().getReal(), 1.0e-10);
358 Assert.assertEquals( 0.1, fv.getY().getReal(), 1.0e-10);
359 Assert.assertEquals( 10.0, fv.getZ().getReal(), 1.0e-10);
360 Assert.assertEquals( -1.0, fv.getX().getPartialDerivative(1).getReal(), 1.0e-15);
361 Assert.assertEquals( -0.1, fv.getY().getPartialDerivative(1).getReal(), 1.0e-15);
362 Assert.assertEquals( -10.0, fv.getZ().getPartialDerivative(1).getReal(), 1.0e-15);
363 Assert.assertEquals( 10.0, fv.getX().getPartialDerivative(2).getReal(), 1.0e-15);
364 Assert.assertEquals( -1.0, fv.getY().getPartialDerivative(2).getReal(), 1.0e-15);
365 Assert.assertEquals(-100.0, fv.getZ().getPartialDerivative(2).getReal(), 1.0e-15);
366 checkPV(new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(),
367 FieldAbsoluteDate.getGalileoEpoch(field),
368 new FieldVector3D<>(one, one.multiply(0.1), one.multiply(10.0)),
369 new FieldVector3D<>(one.multiply(-1.0), one.multiply(-0.1), one.multiply(-10.0)),
370 new FieldVector3D<>(one.multiply(10.0), one.multiply(-1.0), one.multiply(-100.0))),
371 new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(),
372 FieldAbsoluteDate.getGalileoEpoch(field), fv), 1.0e-15);
373
374 for (double dt = 0; dt < 10; dt += 0.125) {
375 Vector3D p = new PVCoordinates(new Vector3D( 1, 0.1, 10),
376 new Vector3D(-1, -0.1, -10),
377 new Vector3D(10, -1.0, -100)).shiftedBy(dt).getPosition();
378 Assert.assertEquals(p.getX(), fv.getX().taylor(dt).getReal(), 1.0e-14);
379 Assert.assertEquals(p.getY(), fv.getY().taylor(dt).getReal(), 1.0e-14);
380 Assert.assertEquals(p.getZ(), fv.getZ().taylor(dt).getReal(), 1.0e-14);
381 }
382
383 FieldAbsolutePVCoordinates<T> pv =
384 new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(),
385 FieldAbsoluteDate.getGalileoEpoch(field),
386 fv);
387 Assert.assertEquals( 1.0, pv.getPosition().getX().getReal(), 1.0e-10);
388 Assert.assertEquals( 0.1, pv.getPosition().getY().getReal(), 1.0e-10);
389 Assert.assertEquals( 10.0, pv.getPosition().getZ().getReal(), 1.0e-10);
390 Assert.assertEquals( -1.0, pv.getVelocity().getX().getReal(), 1.0e-15);
391 Assert.assertEquals( -0.1, pv.getVelocity().getY().getReal(), 1.0e-15);
392 Assert.assertEquals( -10.0, pv.getVelocity().getZ().getReal(), 1.0e-15);
393 Assert.assertEquals( 10.0, pv.getAcceleration().getX().getReal(), 1.0e-15);
394 Assert.assertEquals( -1.0, pv.getAcceleration().getY().getReal(), 1.0e-15);
395 Assert.assertEquals(-100.0, pv.getAcceleration().getZ().getReal(), 1.0e-15);
396
397 }
398
399 private <T extends CalculusFieldElement<T>> void doTestShift(Field<T> field) {
400 final T one = field.getOne();
401 FieldVector3D<T> p1 = new FieldVector3D<>(one, one.multiply(0.1), one.multiply(10.0));
402 FieldVector3D<T> v1 = new FieldVector3D<>(one.multiply(-1.0), one.multiply(-0.1), one.multiply(-10));
403 FieldVector3D<T> a1 = new FieldVector3D<>(one.multiply(10.0), one, one.multiply(100.0));
404 FieldVector3D<T> p2 = new FieldVector3D<>(one.multiply(7.0), one.multiply(0.7), one.multiply(70.0));
405 FieldVector3D<T> v2 = new FieldVector3D<>(one.multiply(-11.0), one.multiply(-1.1), one.multiply(-110.0));
406 FieldVector3D<T> a2 = new FieldVector3D<>(one.multiply(10.0), one, one.multiply(100.0));
407 checkPV(new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), FieldAbsoluteDate.getJ2000Epoch(field), p2, v2, a2),
408 new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), FieldAbsoluteDate.getJ2000Epoch(field).shiftedBy(1.0), p1, v1, a1).shiftedBy(one.multiply(-1.0)), 1.0e-15);
409 Assert.assertEquals(0.0, FieldAbsolutePVCoordinates.estimateVelocity(p1, p2, -1.0).subtract(new Vector3D(-6, -0.6, -60)).getNorm().getReal(), 1.0e-15);
410 }
411
412 private <T extends CalculusFieldElement<T>> void doTestToString(Field<T> field) {
413 final T one = field.getOne();
414 FieldAbsolutePVCoordinates<T> pv =
415 new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(),
416 FieldAbsoluteDate.getJ2000Epoch(field),
417 new FieldVector3D<>(one.multiply(1.0), one.multiply(0.1), one.multiply(10.0)),
418 new FieldVector3D<>(one.multiply(-1.0), one.multiply(-0.1), one.multiply(-10.0)),
419 new FieldVector3D<>(one.multiply(10.0), one.multiply(1.0), one.multiply(100.0)));
420 Assert.assertEquals("{2000-01-01T11:58:55.816, P(1.0, 0.1, 10.0), V(-1.0, -0.1, -10.0), A(10.0, 1.0, 100.0)}", pv.toString());
421 }
422
423 private <T extends CalculusFieldElement<T>> void doTestInterpolatePolynomialPVA(Field<T> field) {
424 final T one = field.getOne();
425 Random random = new Random(0xfe3945fcb8bf47cel);
426 FieldAbsoluteDate<T> t0 = FieldAbsoluteDate.getJ2000Epoch(field);
427 Frame frame = FramesFactory.getEME2000();
428 for (int i = 0; i < 20; ++i) {
429
430 PolynomialFunction px = randomPolynomial(5, random);
431 PolynomialFunction py = randomPolynomial(5, random);
432 PolynomialFunction pz = randomPolynomial(5, random);
433 PolynomialFunction pxDot = px.polynomialDerivative();
434 PolynomialFunction pyDot = py.polynomialDerivative();
435 PolynomialFunction pzDot = pz.polynomialDerivative();
436 PolynomialFunction pxDotDot = pxDot.polynomialDerivative();
437 PolynomialFunction pyDotDot = pyDot.polynomialDerivative();
438 PolynomialFunction pzDotDot = pzDot.polynomialDerivative();
439
440 List<FieldAbsolutePVCoordinates<T>> sample = new ArrayList<FieldAbsolutePVCoordinates<T>>();
441 for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
442 FieldVector3D<T> position = new FieldVector3D<>(one.multiply(px.value(dt)), one.multiply(py.value(dt)), one.multiply(pz.value(dt)));
443 FieldVector3D<T> velocity = new FieldVector3D<>(one.multiply(pxDot.value(dt)), one.multiply(pyDot.value(dt)), one.multiply(pzDot.value(dt)));
444 FieldVector3D<T> acceleration = new FieldVector3D<>(one.multiply(pxDotDot.value(dt)), one.multiply(pyDotDot.value(dt)), one.multiply(pzDotDot.value(dt)));
445 sample.add(new FieldAbsolutePVCoordinates<>(frame, t0.shiftedBy(one.multiply(dt)), position, velocity, acceleration));
446 }
447
448 for (double dt = 0; dt < 1.0; dt += 0.01) {
449 FieldAbsolutePVCoordinates<T> interpolated =
450 FieldAbsolutePVCoordinates.interpolate(frame, t0.shiftedBy(one.multiply(dt)), CartesianDerivativesFilter.USE_PVA, sample.stream());
451 FieldVector3D<T> p = interpolated.getPosition();
452 FieldVector3D<T> v = interpolated.getVelocity();
453 FieldVector3D<T> a = interpolated.getAcceleration();
454 Assert.assertEquals(px.value(dt), p.getX().getReal(), 4.0e-16 * p.getNorm().getReal());
455 Assert.assertEquals(py.value(dt), p.getY().getReal(), 4.0e-16 * p.getNorm().getReal());
456 Assert.assertEquals(pz.value(dt), p.getZ().getReal(), 4.0e-16 * p.getNorm().getReal());
457 Assert.assertEquals(pxDot.value(dt), v.getX().getReal(), 9.0e-16 * v.getNorm().getReal());
458 Assert.assertEquals(pyDot.value(dt), v.getY().getReal(), 9.0e-16 * v.getNorm().getReal());
459 Assert.assertEquals(pzDot.value(dt), v.getZ().getReal(), 9.0e-16 * v.getNorm().getReal());
460 Assert.assertEquals(pxDotDot.value(dt), a.getX().getReal(), 9.0e-15 * a.getNorm().getReal());
461 Assert.assertEquals(pyDotDot.value(dt), a.getY().getReal(), 9.0e-15 * a.getNorm().getReal());
462 Assert.assertEquals(pzDotDot.value(dt), a.getZ().getReal(), 9.0e-15 * a.getNorm().getReal());
463 }
464
465 }
466
467 }
468
469 private <T extends CalculusFieldElement<T>> void doTestInterpolatePolynomialPV(Field<T> field) {
470 final T one = field.getOne();
471 Random random = new Random(0xae7771c9933407bdl);
472 FieldAbsoluteDate<T> t0 = FieldAbsoluteDate.getJ2000Epoch(field);
473 Frame frame = FramesFactory.getEME2000();
474 for (int i = 0; i < 20; ++i) {
475
476 PolynomialFunction px = randomPolynomial(5, random);
477 PolynomialFunction py = randomPolynomial(5, random);
478 PolynomialFunction pz = randomPolynomial(5, random);
479 PolynomialFunction pxDot = px.polynomialDerivative();
480 PolynomialFunction pyDot = py.polynomialDerivative();
481 PolynomialFunction pzDot = pz.polynomialDerivative();
482 PolynomialFunction pxDotDot = pxDot.polynomialDerivative();
483 PolynomialFunction pyDotDot = pyDot.polynomialDerivative();
484 PolynomialFunction pzDotDot = pzDot.polynomialDerivative();
485
486 List<FieldAbsolutePVCoordinates<T>> sample = new ArrayList<FieldAbsolutePVCoordinates<T>>();
487 for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
488 FieldVector3D<T> position = new FieldVector3D<>(one.multiply(px.value(dt)), one.multiply(py.value(dt)), one.multiply(pz.value(dt)));
489 FieldVector3D<T> velocity = new FieldVector3D<>(one.multiply(pxDot.value(dt)), one.multiply(pyDot.value(dt)), one.multiply(pzDot.value(dt)));
490 sample.add(new FieldAbsolutePVCoordinates<>(frame, t0.shiftedBy(one.multiply(dt)), position, velocity, FieldVector3D.getZero(field)));
491 }
492
493 for (double dt = 0; dt < 1.0; dt += 0.01) {
494 FieldAbsolutePVCoordinates<T> interpolated =
495 FieldAbsolutePVCoordinates.interpolate(frame, t0.shiftedBy(dt), CartesianDerivativesFilter.USE_PV, sample.stream());
496 FieldVector3D<T> p = interpolated.getPosition();
497 FieldVector3D<T> v = interpolated.getVelocity();
498 FieldVector3D<T> a = interpolated.getAcceleration();
499 Assert.assertEquals(px.value(dt), p.getX().getReal(), 4.0e-16 * p.getNorm().getReal());
500 Assert.assertEquals(py.value(dt), p.getY().getReal(), 4.0e-16 * p.getNorm().getReal());
501 Assert.assertEquals(pz.value(dt), p.getZ().getReal(), 4.0e-16 * p.getNorm().getReal());
502 Assert.assertEquals(pxDot.value(dt), v.getX().getReal(), 9.0e-16 * v.getNorm().getReal());
503 Assert.assertEquals(pyDot.value(dt), v.getY().getReal(), 9.0e-16 * v.getNorm().getReal());
504 Assert.assertEquals(pzDot.value(dt), v.getZ().getReal(), 9.0e-16 * v.getNorm().getReal());
505 Assert.assertEquals(pxDotDot.value(dt), a.getX().getReal(), 1.0e-14 * a.getNorm().getReal());
506 Assert.assertEquals(pyDotDot.value(dt), a.getY().getReal(), 1.0e-14 * a.getNorm().getReal());
507 Assert.assertEquals(pzDotDot.value(dt), a.getZ().getReal(), 1.0e-14 * a.getNorm().getReal());
508 }
509
510 }
511
512 }
513
514
515 private <T extends CalculusFieldElement<T>> void doTestInterpolatePolynomialPositionOnly(Field<T> field) {
516 final T one = field.getOne();
517 Random random = new Random(0x88740a12e4299003l);
518 FieldAbsoluteDate<T> t0 = FieldAbsoluteDate.getJ2000Epoch(field);
519 Frame frame = FramesFactory.getEME2000();
520 for (int i = 0; i < 20; ++i) {
521
522 PolynomialFunction px = randomPolynomial(5, random);
523 PolynomialFunction py = randomPolynomial(5, random);
524 PolynomialFunction pz = randomPolynomial(5, random);
525 PolynomialFunction pxDot = px.polynomialDerivative();
526 PolynomialFunction pyDot = py.polynomialDerivative();
527 PolynomialFunction pzDot = pz.polynomialDerivative();
528 PolynomialFunction pxDotDot = pxDot.polynomialDerivative();
529 PolynomialFunction pyDotDot = pyDot.polynomialDerivative();
530 PolynomialFunction pzDotDot = pzDot.polynomialDerivative();
531
532 List<FieldAbsolutePVCoordinates<T>> sample = new ArrayList<FieldAbsolutePVCoordinates<T>>();
533 for (double dt : new double[] { 0.0, 0.2, 0.4, 0.6, 0.8, 1.0 }) {
534 FieldVector3D<T>position = new FieldVector3D<>(one.multiply(px.value(dt)), one.multiply(py.value(dt)), one.multiply(pz.value(dt)));
535 sample.add(new FieldAbsolutePVCoordinates<>(frame, t0.shiftedBy(one.multiply(dt)), position, FieldVector3D.getZero(field), FieldVector3D.getZero(field)));
536 }
537
538 for (double dt = 0; dt < 1.0; dt += 0.01) {
539 FieldAbsolutePVCoordinates<T> interpolated =
540 FieldAbsolutePVCoordinates.interpolate(frame, t0.shiftedBy(one.multiply(dt)), CartesianDerivativesFilter.USE_P, sample.stream());
541 FieldVector3D<T> p = interpolated.getPosition();
542 FieldVector3D<T> v = interpolated.getVelocity();
543 FieldVector3D<T> a = interpolated.getAcceleration();
544 Assert.assertEquals(px.value(dt), p.getX().getReal(), 5.0e-16 * p.getNorm().getReal());
545 Assert.assertEquals(py.value(dt), p.getY().getReal(), 5.0e-16 * p.getNorm().getReal());
546 Assert.assertEquals(pz.value(dt), p.getZ().getReal(), 5.0e-16 * p.getNorm().getReal());
547 Assert.assertEquals(pxDot.value(dt), v.getX().getReal(), 7.0e-15 * v.getNorm().getReal());
548 Assert.assertEquals(pyDot.value(dt), v.getY().getReal(), 7.0e-15 * v.getNorm().getReal());
549 Assert.assertEquals(pzDot.value(dt), v.getZ().getReal(), 7.0e-15 * v.getNorm().getReal());
550 Assert.assertEquals(pxDotDot.value(dt), a.getX().getReal(), 2.0e-13 * a.getNorm().getReal());
551 Assert.assertEquals(pyDotDot.value(dt), a.getY().getReal(), 2.0e-13 * a.getNorm().getReal());
552 Assert.assertEquals(pzDotDot.value(dt), a.getZ().getReal(), 2.0e-13 * a.getNorm().getReal());
553 }
554
555 }
556 }
557
558 private <T extends CalculusFieldElement<T>> void doTestInterpolateNonPolynomial(Field<T> field) {
559 final T one = field.getOne();
560 FieldAbsoluteDate<T> t0 = FieldAbsoluteDate.getJ2000Epoch(field);
561 Frame frame = FramesFactory.getEME2000();
562
563 List<FieldAbsolutePVCoordinates<T>> sample = new ArrayList<FieldAbsolutePVCoordinates<T>>();
564 for (double dt : new double[] { 0.0, 0.5, 1.0 }) {
565 FieldVector3D<T> position = new FieldVector3D<>( one.multiply(FastMath.cos(dt)), one.multiply(FastMath.sin(dt)), one.multiply(0.0));
566 FieldVector3D<T> velocity = new FieldVector3D<>( one.multiply(-FastMath.sin(dt)), one.multiply(FastMath.cos(dt)), one.multiply(0.0));
567 FieldVector3D<T> acceleration = new FieldVector3D<>( one.multiply(-FastMath.cos(dt)), one.multiply(-FastMath.sin(dt)), one.multiply(0.0));
568 sample.add(new FieldAbsolutePVCoordinates<>(frame, t0.shiftedBy(one.multiply(dt)), position, velocity, acceleration));
569 }
570
571 for (double dt = 0; dt < 1.0; dt += 0.01) {
572 FieldAbsolutePVCoordinates<T> interpolated =
573 FieldAbsolutePVCoordinates.interpolate(frame, t0.shiftedBy(one.multiply(dt)), CartesianDerivativesFilter.USE_PVA, sample.stream());
574 FieldVector3D<T> p = interpolated.getPosition();
575 FieldVector3D<T> v = interpolated.getVelocity();
576 FieldVector3D<T> a = interpolated.getAcceleration();
577 Assert.assertEquals( FastMath.cos(dt), p.getX().getReal(), 3.0e-10 * p.getNorm().getReal());
578 Assert.assertEquals( FastMath.sin(dt), p.getY().getReal(), 3.0e-10 * p.getNorm().getReal());
579 Assert.assertEquals(0, p.getZ().getReal(), 3.0e-10 * p.getNorm().getReal());
580 Assert.assertEquals(-FastMath.sin(dt), v.getX().getReal(), 3.0e-9 * v.getNorm().getReal());
581 Assert.assertEquals( FastMath.cos(dt), v.getY().getReal(), 3.0e-9 * v.getNorm().getReal());
582 Assert.assertEquals(0, v.getZ().getReal(), 3.0e-9 * v.getNorm().getReal());
583 Assert.assertEquals(-FastMath.cos(dt), a.getX().getReal(), 4.0e-8 * a.getNorm().getReal());
584 Assert.assertEquals(-FastMath.sin(dt), a.getY().getReal(), 4.0e-8 * a.getNorm().getReal());
585 Assert.assertEquals(0, a.getZ().getReal(), 4.0e-8 * a.getNorm().getReal());
586 }
587
588 }
589
590 private <T extends CalculusFieldElement<T>> void doTestSamePV(Field<T> field) {
591
592 final T one = field.getOne();
593 FieldAbsoluteDate<T> date = FieldAbsoluteDate.getJ2000Epoch(field);
594 Frame frame = FramesFactory.getEME2000();
595 FieldVector3D<T> p = new FieldVector3D<>(one.multiply(1), one.multiply(2), one.multiply(3));
596 FieldVector3D<T> v = new FieldVector3D<>(one.multiply(4), one.multiply(5), one.multiply(6));
597
598
599 FieldAbsolutePVCoordinates<T> actual = new FieldAbsolutePVCoordinates<>(frame, date, p, v);
600
601
602 assertEquals(actual.getPVCoordinates().toString(), actual.getPVCoordinates(frame).toString());
603 assertEquals(actual.getPVCoordinates(frame).toString(), actual.getPVCoordinates(date, frame).toString());
604 }
605
606
607 private <T extends CalculusFieldElement<T>> void doTestTaylorProvider(Field<T> field) {
608
609 final T one = field.getOne();
610 FieldAbsoluteDate<T> date = FieldAbsoluteDate.getJ2000Epoch(field);
611 Frame frame = FramesFactory.getEME2000();
612 FieldVector3D<T> p = new FieldVector3D<>(one.multiply(1), one.multiply(2), one.multiply(3));
613 FieldVector3D<T> v = new FieldVector3D<>(one.multiply(4), one.multiply(5), one.multiply(6));
614
615
616 FieldAbsolutePVCoordinates<T> actual = new FieldAbsolutePVCoordinates<>(frame, date, p, v);
617 final FieldPVCoordinatesProvider<T> pv = actual.toTaylorProvider();
618
619
620 Assert.assertEquals(actual.getPVCoordinates(date, frame).toString(), pv.getPVCoordinates(date, frame).toString());
621 }
622
623 private PolynomialFunction randomPolynomial(int degree, Random random) {
624 double[] coeff = new double[ 1 + degree];
625 for (int j = 0; j < degree; ++j) {
626 coeff[j] = random.nextDouble();
627 }
628 return new PolynomialFunction(coeff);
629 }
630
631 private <T extends CalculusFieldElement<T>> void checkPV(FieldAbsolutePVCoordinates<T> expected, FieldAbsolutePVCoordinates<T> real, double epsilon) {
632 Assert.assertEquals(expected.getDate(), real.getDate());
633 Assert.assertEquals(expected.getPosition().getX().getReal(), real.getPosition().getX().getReal(), epsilon);
634 Assert.assertEquals(expected.getPosition().getY().getReal(), real.getPosition().getY().getReal(), epsilon);
635 Assert.assertEquals(expected.getPosition().getZ().getReal(), real.getPosition().getZ().getReal(), epsilon);
636 Assert.assertEquals(expected.getVelocity().getX().getReal(), real.getVelocity().getX().getReal(), epsilon);
637 Assert.assertEquals(expected.getVelocity().getY().getReal(), real.getVelocity().getY().getReal(), epsilon);
638 Assert.assertEquals(expected.getVelocity().getZ().getReal(), real.getVelocity().getZ().getReal(), epsilon);
639 Assert.assertEquals(expected.getAcceleration().getX().getReal(), real.getAcceleration().getX().getReal(), epsilon);
640 Assert.assertEquals(expected.getAcceleration().getY().getReal(), real.getAcceleration().getY().getReal(), epsilon);
641 Assert.assertEquals(expected.getAcceleration().getZ().getReal(), real.getAcceleration().getZ().getReal(), epsilon);
642 }
643
644 }