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