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.geometry.euclidean.threed.FieldRotation;
32 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
33 import org.hipparchus.geometry.euclidean.threed.Rotation;
34 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
35 import org.hipparchus.geometry.euclidean.threed.Vector3D;
36 import org.hipparchus.random.RandomGenerator;
37 import org.hipparchus.random.Well1024a;
38 import org.hipparchus.util.Decimal64;
39 import org.hipparchus.util.Decimal64Field;
40 import org.hipparchus.util.FastMath;
41 import org.junit.Assert;
42 import org.junit.Test;
43 import org.orekit.errors.OrekitException;
44 import org.orekit.errors.OrekitMessages;
45 import org.orekit.time.AbsoluteDate;
46 import org.orekit.time.FieldAbsoluteDate;
47 import org.orekit.time.FieldTimeStamped;
48 import org.orekit.time.TimeScalesFactory;
49
50 public class TimeStampedFieldAngularCoordinatesTest {
51
52 @Test
53 public void testZeroRate() {
54 TimeStampedFieldAngularCoordinates<DerivativeStructure> angularCoordinates =
55 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH,
56 createRotation(0.48, 0.64, 0.36, 0.48, false),
57 createVector(0, 0, 0, 4),
58 createVector(0, 0, 0, 4));
59 Assert.assertEquals(createVector(0, 0, 0, 4), angularCoordinates.getRotationRate());
60 double dt = 10.0;
61 TimeStampedFieldAngularCoordinates<DerivativeStructure> shifted = angularCoordinates.shiftedBy(dt);
62 Assert.assertEquals(0.0, shifted.getRotationAcceleration().getNorm().getReal(), 1.0e-15);
63 Assert.assertEquals(0.0, shifted.getRotationRate().getNorm().getReal(), 1.0e-15);
64 Assert.assertEquals(0.0, FieldRotation.distance(angularCoordinates.getRotation(), shifted.getRotation()).getReal(), 1.0e-15);
65 }
66
67 @Test
68 public void testShift() {
69 double rate = 2 * FastMath.PI / (12 * 60);
70 TimeStampedFieldAngularCoordinates<DerivativeStructure> angularCoordinates =
71 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH,
72 createRotation(1, 0, 0, 0, false),
73 new FieldVector3D<>(rate, createVector(0, 0, 1, 4)),
74 createVector(0, 0, 0, 4));
75 Assert.assertEquals(rate, angularCoordinates.getRotationRate().getNorm().getReal(), 1.0e-10);
76 double dt = 10.0;
77 double alpha = rate * dt;
78 TimeStampedFieldAngularCoordinates<DerivativeStructure> shifted = angularCoordinates.shiftedBy(dt);
79 Assert.assertEquals(rate, shifted.getRotationRate().getNorm().getReal(), 1.0e-10);
80 Assert.assertEquals(alpha, FieldRotation.distance(angularCoordinates.getRotation(), shifted.getRotation()).getReal(), 1.0e-10);
81
82 FieldVector3D<DerivativeStructure> xSat = shifted.getRotation().applyInverseTo(createVector(1, 0, 0, 4));
83 Assert.assertEquals(0.0, xSat.subtract(createVector(FastMath.cos(alpha), FastMath.sin(alpha), 0, 4)).getNorm().getReal(), 1.0e-10);
84 FieldVector3D<DerivativeStructure> ySat = shifted.getRotation().applyInverseTo(createVector(0, 1, 0, 4));
85 Assert.assertEquals(0.0, ySat.subtract(createVector(-FastMath.sin(alpha), FastMath.cos(alpha), 0, 4)).getNorm().getReal(), 1.0e-10);
86 FieldVector3D<DerivativeStructure> zSat = shifted.getRotation().applyInverseTo(createVector(0, 0, 1, 4));
87 Assert.assertEquals(0.0, zSat.subtract(createVector(0, 0, 1, 4)).getNorm().getReal(), 1.0e-10);
88
89 }
90
91 @Test
92 public void testToAC() {
93 Random random = new Random(0xc9b4cf6c371108e0l);
94 for (int i = 0; i < 100; ++i) {
95 FieldRotation<DerivativeStructure> r = randomRotation(random);
96 FieldVector3D<DerivativeStructure> o = randomVector(random, 1.0e-3);
97 FieldVector3D<DerivativeStructure> a = randomVector(random, 1.0e-3);
98 TimeStampedFieldAngularCoordinates<DerivativeStructure> acds =
99 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH, r, o, a);
100 AngularCoordinates ac = acds.toAngularCoordinates();
101 Assert.assertEquals(0, Rotation.distance(r.toRotation(), ac.getRotation()), 1.0e-15);
102 Assert.assertEquals(0, FieldVector3D.distance(o, ac.getRotationRate()).getReal(), 1.0e-15);
103 Assert.assertEquals(0, FieldVector3D.distance(a, ac.getRotationAcceleration()).getReal(), 1.0e-15);
104 }
105 }
106
107 @Test
108 public void testSpin() {
109 double rate = 2 * FastMath.PI / (12 * 60);
110 TimeStampedFieldAngularCoordinates<DerivativeStructure> angularCoordinates =
111 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH,
112 createRotation(0.48, 0.64, 0.36, 0.48, false),
113 new FieldVector3D<>(rate, createVector(0, 0, 1, 4)),
114 createVector(0, 0, 0, 4));
115 Assert.assertEquals(rate, angularCoordinates.getRotationRate().getNorm().getReal(), 1.0e-10);
116 double dt = 10.0;
117 TimeStampedFieldAngularCoordinates<DerivativeStructure> shifted = angularCoordinates.shiftedBy(dt);
118 Assert.assertEquals(rate, shifted.getRotationRate().getNorm().getReal(), 1.0e-10);
119 Assert.assertEquals(rate * dt, FieldRotation.distance(angularCoordinates.getRotation(), shifted.getRotation()).getReal(), 1.0e-10);
120
121 FieldVector3D<DerivativeStructure> shiftedX = shifted.getRotation().applyInverseTo(createVector(1, 0, 0, 4));
122 FieldVector3D<DerivativeStructure> shiftedY = shifted.getRotation().applyInverseTo(createVector(0, 1, 0, 4));
123 FieldVector3D<DerivativeStructure> shiftedZ = shifted.getRotation().applyInverseTo(createVector(0, 0, 1, 4));
124 FieldVector3D<DerivativeStructure> originalX = angularCoordinates.getRotation().applyInverseTo(createVector(1, 0, 0, 4));
125 FieldVector3D<DerivativeStructure> originalY = angularCoordinates.getRotation().applyInverseTo(createVector(0, 1, 0, 4));
126 FieldVector3D<DerivativeStructure> originalZ = angularCoordinates.getRotation().applyInverseTo(createVector(0, 0, 1, 4));
127 Assert.assertEquals( FastMath.cos(rate * dt), FieldVector3D.dotProduct(shiftedX, originalX).getReal(), 1.0e-10);
128 Assert.assertEquals( FastMath.sin(rate * dt), FieldVector3D.dotProduct(shiftedX, originalY).getReal(), 1.0e-10);
129 Assert.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedX, originalZ).getReal(), 1.0e-10);
130 Assert.assertEquals(-FastMath.sin(rate * dt), FieldVector3D.dotProduct(shiftedY, originalX).getReal(), 1.0e-10);
131 Assert.assertEquals( FastMath.cos(rate * dt), FieldVector3D.dotProduct(shiftedY, originalY).getReal(), 1.0e-10);
132 Assert.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedY, originalZ).getReal(), 1.0e-10);
133 Assert.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedZ, originalX).getReal(), 1.0e-10);
134 Assert.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedZ, originalY).getReal(), 1.0e-10);
135 Assert.assertEquals( 1.0, FieldVector3D.dotProduct(shiftedZ, originalZ).getReal(), 1.0e-10);
136
137 FieldVector3D<DerivativeStructure> forward = FieldAngularCoordinates.estimateRate(angularCoordinates.getRotation(), shifted.getRotation(), dt);
138 Assert.assertEquals(0.0, forward.subtract(angularCoordinates.getRotationRate()).getNorm().getReal(), 1.0e-10);
139
140 FieldVector3D<DerivativeStructure> reversed = FieldAngularCoordinates.estimateRate(shifted.getRotation(), angularCoordinates.getRotation(), dt);
141 Assert.assertEquals(0.0, reversed.add(angularCoordinates.getRotationRate()).getNorm().getReal(), 1.0e-10);
142
143 }
144
145 @Test
146 public void testReverseOffset() {
147 Random random = new Random(0x4ecca9d57a8f1611l);
148 for (int i = 0; i < 100; ++i) {
149 FieldRotation<DerivativeStructure> r = randomRotation(random);
150 FieldVector3D<DerivativeStructure> o = randomVector(random, 1.0e-3);
151 FieldVector3D<DerivativeStructure> a = randomVector(random, 1.0e-3);
152 TimeStampedFieldAngularCoordinates<DerivativeStructure> ac =
153 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH, r, o, a);
154 TimeStampedFieldAngularCoordinates<DerivativeStructure> sum = ac.addOffset(ac.revert());
155 Assert.assertEquals(0.0, sum.getRotation().getAngle().getReal(), 1.0e-15);
156 Assert.assertEquals(0.0, sum.getRotationRate().getNorm().getReal(), 1.0e-15);
157 Assert.assertEquals(0.0, sum.getRotationAcceleration().getNorm().getReal(), 1.0e-15);
158 }
159 }
160
161 @Test
162 public void testNoCommute() {
163 TimeStampedFieldAngularCoordinates<DerivativeStructure> ac1 =
164 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH,
165 createRotation(0.48, 0.64, 0.36, 0.48, false),
166 createVector(0, 0, 0, 4),
167 createVector(0, 0, 0, 4));
168 TimeStampedFieldAngularCoordinates<DerivativeStructure> ac2 =
169 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH,
170 createRotation(0.36, -0.48, 0.48, 0.64, false),
171 createVector(0, 0, 0, 4),
172 createVector(0, 0, 0, 4));
173
174 TimeStampedFieldAngularCoordinates<DerivativeStructure> add12 = ac1.addOffset(ac2);
175 TimeStampedFieldAngularCoordinates<DerivativeStructure> add21 = ac2.addOffset(ac1);
176
177
178 Assert.assertEquals(2.574, FieldRotation.distance(add12.getRotation(), add21.getRotation()).getReal(), 1.0e-3);
179
180 }
181
182 @Test
183 public void testRoundTripNoOp() {
184 Random random = new Random(0x1e610cfe89306669l);
185 for (int i = 0; i < 100; ++i) {
186
187 FieldRotation<DerivativeStructure> r1 = randomRotation(random);
188 FieldVector3D<DerivativeStructure> o1 = randomVector(random, 1.0e-2);
189 FieldVector3D<DerivativeStructure> a1 = randomVector(random, 1.0e-2);
190 TimeStampedFieldAngularCoordinates<DerivativeStructure> ac1 =
191 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH, r1, o1, a1);
192 FieldRotation<DerivativeStructure> r2 = randomRotation(random);
193 FieldVector3D<DerivativeStructure> o2 = randomVector(random, 1.0e-2);
194 FieldVector3D<DerivativeStructure> a2 = randomVector(random, 1.0e-2);
195
196 TimeStampedFieldAngularCoordinates<DerivativeStructure> ac2 =
197 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH, r2, o2, a2);
198 TimeStampedFieldAngularCoordinates<DerivativeStructure> roundTripSA = ac1.subtractOffset(ac2).addOffset(ac2);
199 Assert.assertEquals(0.0, FieldRotation.distance(ac1.getRotation(), roundTripSA.getRotation()).getReal(), 4.0e-16);
200 Assert.assertEquals(0.0, FieldVector3D.distance(ac1.getRotationRate(), roundTripSA.getRotationRate()).getReal(), 2.0e-17);
201 Assert.assertEquals(0.0, FieldVector3D.distance(ac1.getRotationAcceleration(), roundTripSA.getRotationAcceleration()).getReal(), 1.0e-17);
202
203 TimeStampedFieldAngularCoordinates<DerivativeStructure> roundTripAS = ac1.addOffset(ac2).subtractOffset(ac2);
204 Assert.assertEquals(0.0, FieldRotation.distance(ac1.getRotation(), roundTripAS.getRotation()).getReal(), 6.0e-16);
205 Assert.assertEquals(0.0, FieldVector3D.distance(ac1.getRotationRate(), roundTripAS.getRotationRate()).getReal(), 2.0e-17);
206 Assert.assertEquals(0.0, FieldVector3D.distance(ac1.getRotationAcceleration(), roundTripAS.getRotationAcceleration()).getReal(), 2.0e-17);
207
208 }
209 }
210
211 @Test
212 public void testInterpolationNeedOffsetWrongRate() {
213 AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
214 double omega = 2.0 * FastMath.PI;
215 TimeStampedFieldAngularCoordinates<DerivativeStructure> reference =
216 new TimeStampedFieldAngularCoordinates<>(date,
217 createRotation(1, 0, 0, 0, false),
218 createVector(0, 0, -omega, 4),
219 createVector(0, 0, 0, 4));
220
221 List<TimeStampedFieldAngularCoordinates<DerivativeStructure>> sample =
222 new ArrayList<TimeStampedFieldAngularCoordinates<DerivativeStructure>>();
223 for (double dt : new double[] { 0.0, 0.25, 0.5, 0.75, 1.0 }) {
224 TimeStampedFieldAngularCoordinates<DerivativeStructure> shifted = reference.shiftedBy(dt);
225 sample.add(new TimeStampedFieldAngularCoordinates<>(shifted.getDate(),
226 shifted.getRotation(),
227 createVector(0, 0, 0, 4),
228 createVector(0, 0, 0, 4)));
229 }
230
231 for (TimeStampedFieldAngularCoordinates<DerivativeStructure> s : sample) {
232 TimeStampedFieldAngularCoordinates<DerivativeStructure> interpolated =
233 TimeStampedFieldAngularCoordinates.interpolate(s.getDate(), AngularDerivativesFilter.USE_RR, sample);
234 FieldRotation<DerivativeStructure> r = interpolated.getRotation();
235 FieldVector3D<DerivativeStructure> rate = interpolated.getRotationRate();
236 Assert.assertEquals(0.0, FieldRotation.distance(s.getRotation(), r).getReal(), 2.0e-14);
237 Assert.assertEquals(0.0, FieldVector3D.distance(s.getRotationRate(), rate).getReal(), 2.0e-13);
238 }
239
240 }
241
242 @Test
243 public void testInterpolationRotationOnly() {
244 AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
245 double alpha0 = 0.5 * FastMath.PI;
246 double omega = 0.5 * FastMath.PI;
247 TimeStampedFieldAngularCoordinates<DerivativeStructure> reference =
248 new TimeStampedFieldAngularCoordinates<>(date,
249 createRotation(createVector(0, 0, 1, 4), alpha0),
250 new FieldVector3D<>(omega, createVector(0, 0, -1, 4)),
251 createVector(0, 0, 0, 4));
252
253 List<TimeStampedFieldAngularCoordinates<DerivativeStructure>> sample =
254 new ArrayList<TimeStampedFieldAngularCoordinates<DerivativeStructure>>();
255 for (double dt : new double[] { 0.0, 0.2, 0.4, 0.6, 0.8, 1.0 }) {
256 FieldRotation<DerivativeStructure> r = reference.shiftedBy(dt).getRotation();
257 sample.add(new TimeStampedFieldAngularCoordinates<>(date.shiftedBy(dt),
258 r,
259 createVector(0, 0, 0, 4),
260 createVector(0, 0, 0, 4)));
261 }
262
263 for (double dt = 0; dt < 1.0; dt += 0.001) {
264 TimeStampedFieldAngularCoordinates<DerivativeStructure> interpolated =
265 TimeStampedFieldAngularCoordinates.interpolate(date.shiftedBy(dt), AngularDerivativesFilter.USE_R, sample);
266 FieldRotation<DerivativeStructure> r = interpolated.getRotation();
267 FieldVector3D<DerivativeStructure> rate = interpolated.getRotationRate();
268 FieldVector3D<DerivativeStructure> acceleration = interpolated.getRotationAcceleration();
269 Assert.assertEquals(0.0, FieldRotation.distance(reference.shiftedBy(dt).getRotation(), r).getReal(), 3.0e-4);
270 Assert.assertEquals(0.0, FieldVector3D.distance(reference.shiftedBy(dt).getRotationRate(), rate).getReal(), 1.0e-2);
271 Assert.assertEquals(0.0, FieldVector3D.distance(reference.shiftedBy(dt).getRotationAcceleration(), acceleration).getReal(), 1.0e-2);
272 }
273
274 }
275
276 @Test
277 public void testInterpolationAroundPI() {
278
279 DSFactory factory = new DSFactory(4, 1);
280 List<TimeStampedFieldAngularCoordinates<DerivativeStructure>> sample =
281 new ArrayList<TimeStampedFieldAngularCoordinates<DerivativeStructure>>();
282
283
284 AbsoluteDate t0 = new AbsoluteDate("2012-01-01T00:00:00.000", TimeScalesFactory.getTAI());
285 TimeStampedFieldAngularCoordinates<DerivativeStructure> ac0 =
286 new TimeStampedFieldAngularCoordinates<>(t0,
287 new FieldRotation<>(createVector(1, 0, 0, 4),
288 factory.variable(3, FastMath.toRadians(179.999)),
289 RotationConvention.VECTOR_OPERATOR),
290 createVector(FastMath.toRadians(0), 0, 0, 4),
291 createVector(0, 0, 0, 4));
292 sample.add(ac0);
293
294
295 AbsoluteDate t1 = new AbsoluteDate("2012-01-01T00:00:02.000", TimeScalesFactory.getTAI());
296 TimeStampedFieldAngularCoordinates<DerivativeStructure> ac1 =
297 new TimeStampedFieldAngularCoordinates<>(t1,
298 new FieldRotation<>(createVector(1, 0, 0, 4),
299 factory.variable(3, FastMath.toRadians(-179.999)),
300 RotationConvention.VECTOR_OPERATOR),
301 createVector(FastMath.toRadians(0), 0, 0, 4),
302 createVector(0, 0, 0, 4));
303 sample.add(ac1);
304
305
306 AbsoluteDate t = new AbsoluteDate("2012-01-01T00:00:01.000", TimeScalesFactory.getTAI());
307 TimeStampedFieldAngularCoordinates<DerivativeStructure> interpolated =
308 TimeStampedFieldAngularCoordinates.interpolate(t, AngularDerivativesFilter.USE_R, sample);
309
310 Assert.assertEquals(FastMath.toRadians(180), interpolated.getRotation().getAngle().getReal(), 1.0e-12);
311
312 }
313
314 @Test
315 public void testInterpolationTooSmallSample() {
316 DSFactory factory = new DSFactory(4, 1);
317 AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
318 double alpha0 = 0.5 * FastMath.PI;
319 double omega = 0.5 * FastMath.PI;
320 TimeStampedFieldAngularCoordinates<DerivativeStructure> reference =
321 new TimeStampedFieldAngularCoordinates<>(date,
322 new FieldRotation<>(createVector(0, 0, 1, 4),
323 factory.variable(3, alpha0),
324 RotationConvention.VECTOR_OPERATOR),
325 createVector(0, 0, -omega, 4),
326 createVector(0, 0, 0, 4));
327
328 List<TimeStampedFieldAngularCoordinates<DerivativeStructure> > sample =
329 new ArrayList<TimeStampedFieldAngularCoordinates<DerivativeStructure> >();
330 FieldRotation<DerivativeStructure> r = reference.shiftedBy(0.2).getRotation();
331 sample.add(new TimeStampedFieldAngularCoordinates<>(date.shiftedBy(0.2),
332 r,
333 createVector(0, 0, 0, 4),
334 createVector(0, 0, 0, 4)));
335
336 try {
337 TimeStampedFieldAngularCoordinates.interpolate(date.shiftedBy(0.3), AngularDerivativesFilter.USE_R, sample);
338 Assert.fail("an exception should have been thrown");
339 } catch (OrekitException oe) {
340 Assert.assertEquals(OrekitMessages.NOT_ENOUGH_DATA_FOR_INTERPOLATION, oe.getSpecifier());
341 Assert.assertEquals(1, ((Integer) oe.getParts()[0]).intValue());
342 }
343
344 }
345
346 @Test
347 public void testInterpolationGTODIssue() {
348 AbsoluteDate t0 = new AbsoluteDate("2004-04-06T19:59:28.000", TimeScalesFactory.getTAI());
349 double[][] params = new double[][] {
350 { 0.0, -0.3802356750911964, -0.9248896320037013, 7.292115030462892e-5 },
351 { 4.0, 0.1345716955788532, -0.990903859488413, 7.292115033301528e-5 },
352 { 8.0, -0.613127541102373, 0.7899839354960061, 7.292115037371062e-5 }
353 };
354 List<TimeStampedFieldAngularCoordinates<DerivativeStructure>> sample =
355 new ArrayList<TimeStampedFieldAngularCoordinates<DerivativeStructure>>();
356 for (double[] row : params) {
357 AbsoluteDate t = t0.shiftedBy(row[0] * 3600.0);
358 FieldRotation<DerivativeStructure> r = createRotation(row[1], 0.0, 0.0, row[2], false);
359 FieldVector3D<DerivativeStructure> o = new FieldVector3D<>(row[3], createVector(0, 0, 1, 4));
360 sample.add(new TimeStampedFieldAngularCoordinates<>(t, r, o, createVector(0, 0, 0, 4)));
361 }
362 for (double dt = 0; dt < 29000; dt += 120) {
363 TimeStampedFieldAngularCoordinates<DerivativeStructure> shifted = sample.get(0).shiftedBy(dt);
364 TimeStampedFieldAngularCoordinates<DerivativeStructure> interpolated =
365 TimeStampedFieldAngularCoordinates.interpolate(t0.shiftedBy(dt), AngularDerivativesFilter.USE_RR, sample);
366 Assert.assertEquals(0.0,
367 FieldRotation.distance(shifted.getRotation(), interpolated.getRotation()).getReal(),
368 1.3e-7);
369 Assert.assertEquals(0.0,
370 FieldVector3D.distance(shifted.getRotationRate(), interpolated.getRotationRate()).getReal(),
371 1.0e-11);
372 }
373
374 }
375
376 @Test
377 public void testDerivativesStructures0() {
378 RandomGenerator random = new Well1024a(0x18a0a08fd63f047al);
379
380 FieldRotation<Decimal64> r = randomRotation64(random);
381 FieldVector3D<Decimal64> o = randomVector64(random, 1.0e-2);
382 FieldVector3D<Decimal64> oDot = randomVector64(random, 1.0e-2);
383 TimeStampedFieldAngularCoordinates<Decimal64> ac =
384 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Decimal64Field.getInstance()),
385 r, o, oDot);
386 TimeStampedFieldAngularCoordinates<Decimal64> rebuilt =
387 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Decimal64Field.getInstance()),
388 ac.toDerivativeStructureRotation(0));
389 Assert.assertEquals(0.0, FieldRotation.distance(ac.getRotation(), rebuilt.getRotation()).getReal(), 1.0e-15);
390 Assert.assertEquals(0.0, rebuilt.getRotationRate().getNorm().getReal(), 1.0e-15);
391 Assert.assertEquals(0.0, rebuilt.getRotationAcceleration().getNorm().getReal(), 1.0e-15);
392 }
393
394 @Test
395 public void testDerivativesStructures1() {
396 RandomGenerator random = new Well1024a(0x8f8fc6d27bbdc46dl);
397
398 FieldRotation<Decimal64> r = randomRotation64(random);
399 FieldVector3D<Decimal64> o = randomVector64(random, 1.0e-2);
400 FieldVector3D<Decimal64> oDot = randomVector64(random, 1.0e-2);
401 TimeStampedFieldAngularCoordinates<Decimal64> ac =
402 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Decimal64Field.getInstance()),
403 r, o, oDot);
404 TimeStampedFieldAngularCoordinates<Decimal64> rebuilt =
405 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Decimal64Field.getInstance()),
406 ac.toDerivativeStructureRotation(1));
407 Assert.assertEquals(0.0, FieldRotation.distance(ac.getRotation(), rebuilt.getRotation()).getReal(), 1.0e-15);
408 Assert.assertEquals(0.0, FieldVector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()).getReal(), 1.0e-15);
409 Assert.assertEquals(0.0, rebuilt.getRotationAcceleration().getNorm().getReal(), 1.0e-15);
410 }
411
412 @Test
413 public void testDerivativesStructures2() {
414 RandomGenerator random = new Well1024a(0x1633878dddac047dl);
415
416 FieldRotation<Decimal64> r = randomRotation64(random);
417 FieldVector3D<Decimal64> o = randomVector64(random, 1.0e-2);
418 FieldVector3D<Decimal64> oDot = randomVector64(random, 1.0e-2);
419 TimeStampedFieldAngularCoordinates<Decimal64> ac =
420 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Decimal64Field.getInstance()),
421 r, o, oDot);
422 TimeStampedFieldAngularCoordinates<Decimal64> rebuilt =
423 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Decimal64Field.getInstance()),
424 ac.toDerivativeStructureRotation(2));
425 Assert.assertEquals(0.0, FieldRotation.distance(ac.getRotation(), rebuilt.getRotation()).getReal(), 1.0e-15);
426 Assert.assertEquals(0.0, FieldVector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()).getReal(), 1.0e-15);
427 Assert.assertEquals(0.0, FieldVector3D.distance(ac.getRotationAcceleration(), rebuilt.getRotationAcceleration()).getReal(), 1.0e-15);
428
429 }
430
431 @Test
432 public void testUnivariateDerivative1() {
433 RandomGenerator random = new Well1024a(0x6de8cce747539904l);
434
435 FieldRotation<Decimal64> r = randomRotation64(random);
436 FieldVector3D<Decimal64> o = randomVector64(random, 1.0e-2);
437 FieldVector3D<Decimal64> oDot = randomVector64(random, 1.0e-2);
438 TimeStampedFieldAngularCoordinates<Decimal64> ac =
439 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Decimal64Field.getInstance()),
440 r, o, oDot);
441 FieldRotation<FieldUnivariateDerivative1<Decimal64>> rotationUD = ac.toUnivariateDerivative1Rotation();
442 FieldRotation<FieldDerivativeStructure<Decimal64>> rotationDS = ac.toDerivativeStructureRotation(1);
443 Assert.assertEquals(rotationDS.getQ0().getReal(), rotationUD.getQ0().getReal(), 1.0e-15);
444 Assert.assertEquals(rotationDS.getQ1().getReal(), rotationUD.getQ1().getReal(), 1.0e-15);
445 Assert.assertEquals(rotationDS.getQ2().getReal(), rotationUD.getQ2().getReal(), 1.0e-15);
446 Assert.assertEquals(rotationDS.getQ3().getReal(), rotationUD.getQ3().getReal(), 1.0e-15);
447 Assert.assertEquals(rotationDS.getQ0().getPartialDerivative(1).getReal(), rotationUD.getQ0().getFirstDerivative().getReal(), 1.0e-15);
448 Assert.assertEquals(rotationDS.getQ1().getPartialDerivative(1).getReal(), rotationUD.getQ1().getFirstDerivative().getReal(), 1.0e-15);
449 Assert.assertEquals(rotationDS.getQ2().getPartialDerivative(1).getReal(), rotationUD.getQ2().getFirstDerivative().getReal(), 1.0e-15);
450 Assert.assertEquals(rotationDS.getQ3().getPartialDerivative(1).getReal(), rotationUD.getQ3().getFirstDerivative().getReal(), 1.0e-15);
451
452 TimeStampedFieldAngularCoordinates<Decimal64> rebuilt =
453 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Decimal64Field.getInstance()),
454 rotationUD);
455 Assert.assertEquals(0.0, FieldRotation.distance(ac.getRotation(), rebuilt.getRotation()).getReal(), 1.0e-15);
456 Assert.assertEquals(0.0, FieldVector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()).getReal(), 1.0e-15);
457
458 }
459
460 @Test
461 public void testUnivariateDerivative2() {
462 RandomGenerator random = new Well1024a(0x255710c8fa2247ecl);
463
464 FieldRotation<Decimal64> r = randomRotation64(random);
465 FieldVector3D<Decimal64> o = randomVector64(random, 1.0e-2);
466 FieldVector3D<Decimal64> oDot = randomVector64(random, 1.0e-2);
467 TimeStampedFieldAngularCoordinates<Decimal64> ac =
468 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Decimal64Field.getInstance()),
469 r, o, oDot);
470 FieldRotation<FieldUnivariateDerivative2<Decimal64>> rotationUD = ac.toUnivariateDerivative2Rotation();
471 FieldRotation<FieldDerivativeStructure<Decimal64>> rotationDS = ac.toDerivativeStructureRotation(2);
472 Assert.assertEquals(rotationDS.getQ0().getReal(), rotationUD.getQ0().getReal(), 1.0e-15);
473 Assert.assertEquals(rotationDS.getQ1().getReal(), rotationUD.getQ1().getReal(), 1.0e-15);
474 Assert.assertEquals(rotationDS.getQ2().getReal(), rotationUD.getQ2().getReal(), 1.0e-15);
475 Assert.assertEquals(rotationDS.getQ3().getReal(), rotationUD.getQ3().getReal(), 1.0e-15);
476 Assert.assertEquals(rotationDS.getQ0().getPartialDerivative(1).getReal(), rotationUD.getQ0().getFirstDerivative().getReal(), 1.0e-15);
477 Assert.assertEquals(rotationDS.getQ1().getPartialDerivative(1).getReal(), rotationUD.getQ1().getFirstDerivative().getReal(), 1.0e-15);
478 Assert.assertEquals(rotationDS.getQ2().getPartialDerivative(1).getReal(), rotationUD.getQ2().getFirstDerivative().getReal(), 1.0e-15);
479 Assert.assertEquals(rotationDS.getQ3().getPartialDerivative(1).getReal(), rotationUD.getQ3().getFirstDerivative().getReal(), 1.0e-15);
480 Assert.assertEquals(rotationDS.getQ0().getPartialDerivative(2).getReal(), rotationUD.getQ0().getSecondDerivative().getReal(), 1.0e-15);
481 Assert.assertEquals(rotationDS.getQ1().getPartialDerivative(2).getReal(), rotationUD.getQ1().getSecondDerivative().getReal(), 1.0e-15);
482 Assert.assertEquals(rotationDS.getQ2().getPartialDerivative(2).getReal(), rotationUD.getQ2().getSecondDerivative().getReal(), 1.0e-15);
483 Assert.assertEquals(rotationDS.getQ3().getPartialDerivative(2).getReal(), rotationUD.getQ3().getSecondDerivative().getReal(), 1.0e-15);
484
485 TimeStampedFieldAngularCoordinates<Decimal64> rebuilt =
486 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Decimal64Field.getInstance()),
487 rotationUD);
488 Assert.assertEquals(0.0, FieldRotation.distance(ac.getRotation(), rebuilt.getRotation()).getReal(), 1.0e-15);
489 Assert.assertEquals(0.0, FieldVector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()).getReal(), 1.0e-15);
490 Assert.assertEquals(0.0, FieldVector3D.distance(ac.getRotationAcceleration(), rebuilt.getRotationAcceleration()).getReal(), 1.0e-15);
491
492 }
493
494 @Test
495 public void testIssue773() {
496 doTestIssue773(Decimal64Field.getInstance());
497 }
498
499 private <T extends CalculusFieldElement<T>> void doTestIssue773(final Field<T> field) {
500
501 final AbsoluteDate date = new AbsoluteDate();
502
503
504 final TimeStampedAngularCoordinates angular =
505 new TimeStampedAngularCoordinates(date,
506 new Rotation(0., 0., 0., 0., false),
507 Vector3D.ZERO,
508 Vector3D.ZERO);
509
510
511 final FieldTimeStamped<T> timeStamped =
512 new TimeStampedFieldAngularCoordinates<>(field, angular);
513
514
515 Assert.assertEquals(0.0, date.durationFrom(timeStamped.getDate().toAbsoluteDate()), Double.MIN_VALUE);
516 }
517
518 private FieldVector3D<DerivativeStructure> randomVector(Random random, double norm) {
519 double n = random.nextDouble() * norm;
520 double x = random.nextDouble();
521 double y = random.nextDouble();
522 double z = random.nextDouble();
523 return new FieldVector3D<>(n, createVector(x, y, z, 4).normalize());
524 }
525
526 private FieldRotation<DerivativeStructure> randomRotation(Random random) {
527 double q0 = random.nextDouble() * 2 - 1;
528 double q1 = random.nextDouble() * 2 - 1;
529 double q2 = random.nextDouble() * 2 - 1;
530 double q3 = random.nextDouble() * 2 - 1;
531 double q = FastMath.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
532 return createRotation(q0 / q, q1 / q, q2 / q, q3 / q, false);
533 }
534
535 private FieldRotation<DerivativeStructure> createRotation(FieldVector3D<DerivativeStructure> axis, double angle) {
536 return new FieldRotation<>(axis,
537 new DSFactory(4, 1).constant(angle),
538 RotationConvention.VECTOR_OPERATOR);
539 }
540
541 private FieldRotation<DerivativeStructure> createRotation(double q0, double q1, double q2, double q3,
542 boolean needsNormalization) {
543 DSFactory factory = new DSFactory(4, 1);
544 return new FieldRotation<>(factory.variable(0, q0),
545 factory.variable(1, q1),
546 factory.variable(2, q2),
547 factory.variable(3, q3),
548 needsNormalization);
549 }
550
551 private FieldVector3D<DerivativeStructure> createVector(double x, double y, double z, int params) {
552 DSFactory factory = new DSFactory(params, 1);
553 return new FieldVector3D<>(factory.variable(0, x),
554 factory.variable(1, y),
555 factory.variable(2, z));
556 }
557
558 private FieldRotation<Decimal64> randomRotation64(RandomGenerator random) {
559 double q0 = random.nextDouble() * 2 - 1;
560 double q1 = random.nextDouble() * 2 - 1;
561 double q2 = random.nextDouble() * 2 - 1;
562 double q3 = random.nextDouble() * 2 - 1;
563 double q = FastMath.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
564 return new FieldRotation<>(new Decimal64(q0 / q),
565 new Decimal64(q1 / q),
566 new Decimal64(q2 / q),
567 new Decimal64(q3 / q),
568 false);
569 }
570
571 private FieldVector3D<Decimal64> randomVector64(RandomGenerator random, double norm) {
572 double n = random.nextDouble() * norm;
573 double x = random.nextDouble();
574 double y = random.nextDouble();
575 double z = random.nextDouble();
576 return new FieldVector3D<>(n, new FieldVector3D<>(new Decimal64(x), new Decimal64(y), new Decimal64(z)).normalize());
577 }
578
579 }
580