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