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