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