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.Binary64;
34 import org.hipparchus.util.Binary64Field;
35 import org.hipparchus.util.FastMath;
36 import org.junit.jupiter.api.Assertions;
37 import org.junit.jupiter.api.BeforeEach;
38 import org.junit.jupiter.api.Test;
39 import org.orekit.Utils;
40 import org.orekit.time.AbsoluteDate;
41 import org.orekit.time.FieldAbsoluteDate;
42 import org.orekit.time.FieldTimeStamped;
43
44 import java.util.Random;
45
46 public class TimeStampedFieldAngularCoordinatesTest {
47
48 @Test
49 public void testZeroRate() {
50 TimeStampedFieldAngularCoordinates<DerivativeStructure> angularCoordinates =
51 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH,
52 createRotation(0.48, 0.64, 0.36, 0.48, false),
53 createVector(0, 0, 0, 4),
54 createVector(0, 0, 0, 4));
55 Assertions.assertEquals(createVector(0, 0, 0, 4), angularCoordinates.getRotationRate());
56 double dt = 10.0;
57 TimeStampedFieldAngularCoordinates<DerivativeStructure> shifted = angularCoordinates.shiftedBy(dt);
58 Assertions.assertEquals(0.0, shifted.getRotationAcceleration().getNorm().getReal(), 1.0e-15);
59 Assertions.assertEquals(0.0, shifted.getRotationRate().getNorm().getReal(), 1.0e-15);
60 Assertions.assertEquals(0.0, FieldRotation.distance(angularCoordinates.getRotation(), shifted.getRotation()).getReal(), 1.0e-15);
61 }
62
63 @Test
64 public void testShift() {
65 double rate = 2 * FastMath.PI / (12 * 60);
66 TimeStampedFieldAngularCoordinates<DerivativeStructure> angularCoordinates =
67 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH,
68 createRotation(1, 0, 0, 0, false),
69 new FieldVector3D<>(rate, createVector(0, 0, 1, 4)),
70 createVector(0, 0, 0, 4));
71 Assertions.assertEquals(rate, angularCoordinates.getRotationRate().getNorm().getReal(), 1.0e-10);
72 double dt = 10.0;
73 double alpha = rate * dt;
74 TimeStampedFieldAngularCoordinates<DerivativeStructure> shifted = angularCoordinates.shiftedBy(dt);
75 Assertions.assertEquals(rate, shifted.getRotationRate().getNorm().getReal(), 1.0e-10);
76 Assertions.assertEquals(alpha, FieldRotation.distance(angularCoordinates.getRotation(), shifted.getRotation()).getReal(), 1.0e-10);
77
78 FieldVector3D<DerivativeStructure> xSat = shifted.getRotation().applyInverseTo(createVector(1, 0, 0, 4));
79 Assertions.assertEquals(0.0, xSat.subtract(createVector(FastMath.cos(alpha), FastMath.sin(alpha), 0, 4)).getNorm().getReal(), 1.0e-10);
80 FieldVector3D<DerivativeStructure> ySat = shifted.getRotation().applyInverseTo(createVector(0, 1, 0, 4));
81 Assertions.assertEquals(0.0, ySat.subtract(createVector(-FastMath.sin(alpha), FastMath.cos(alpha), 0, 4)).getNorm().getReal(), 1.0e-10);
82 FieldVector3D<DerivativeStructure> zSat = shifted.getRotation().applyInverseTo(createVector(0, 0, 1, 4));
83 Assertions.assertEquals(0.0, zSat.subtract(createVector(0, 0, 1, 4)).getNorm().getReal(), 1.0e-10);
84
85 }
86
87 @Test
88 public void testToAC() {
89 Random random = new Random(0xc9b4cf6c371108e0l);
90 for (int i = 0; i < 100; ++i) {
91 FieldRotation<DerivativeStructure> r = randomRotation(random);
92 FieldVector3D<DerivativeStructure> o = randomVector(random, 1.0e-3);
93 FieldVector3D<DerivativeStructure> a = randomVector(random, 1.0e-3);
94 TimeStampedFieldAngularCoordinates<DerivativeStructure> acds =
95 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH, r, o, a);
96 AngularCoordinates ac = acds.toAngularCoordinates();
97 Assertions.assertEquals(0, Rotation.distance(r.toRotation(), ac.getRotation()), 1.0e-15);
98 Assertions.assertEquals(0, FieldVector3D.distance(o, ac.getRotationRate()).getReal(), 1.0e-15);
99 Assertions.assertEquals(0, FieldVector3D.distance(a, ac.getRotationAcceleration()).getReal(), 1.0e-15);
100 }
101 }
102
103 @Test
104 public void testSpin() {
105 double rate = 2 * FastMath.PI / (12 * 60);
106 TimeStampedFieldAngularCoordinates<DerivativeStructure> angularCoordinates =
107 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH,
108 createRotation(0.48, 0.64, 0.36, 0.48, false),
109 new FieldVector3D<>(rate, createVector(0, 0, 1, 4)),
110 createVector(0, 0, 0, 4));
111 Assertions.assertEquals(rate, angularCoordinates.getRotationRate().getNorm().getReal(), 1.0e-10);
112 double dt = 10.0;
113 TimeStampedFieldAngularCoordinates<DerivativeStructure> shifted = angularCoordinates.shiftedBy(dt);
114 Assertions.assertEquals(rate, shifted.getRotationRate().getNorm().getReal(), 1.0e-10);
115 Assertions.assertEquals(rate * dt, FieldRotation.distance(angularCoordinates.getRotation(), shifted.getRotation()).getReal(), 1.0e-10);
116
117 FieldVector3D<DerivativeStructure> shiftedX = shifted.getRotation().applyInverseTo(createVector(1, 0, 0, 4));
118 FieldVector3D<DerivativeStructure> shiftedY = shifted.getRotation().applyInverseTo(createVector(0, 1, 0, 4));
119 FieldVector3D<DerivativeStructure> shiftedZ = shifted.getRotation().applyInverseTo(createVector(0, 0, 1, 4));
120 FieldVector3D<DerivativeStructure> originalX = angularCoordinates.getRotation().applyInverseTo(createVector(1, 0, 0, 4));
121 FieldVector3D<DerivativeStructure> originalY = angularCoordinates.getRotation().applyInverseTo(createVector(0, 1, 0, 4));
122 FieldVector3D<DerivativeStructure> originalZ = angularCoordinates.getRotation().applyInverseTo(createVector(0, 0, 1, 4));
123 Assertions.assertEquals( FastMath.cos(rate * dt), FieldVector3D.dotProduct(shiftedX, originalX).getReal(), 1.0e-10);
124 Assertions.assertEquals( FastMath.sin(rate * dt), FieldVector3D.dotProduct(shiftedX, originalY).getReal(), 1.0e-10);
125 Assertions.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedX, originalZ).getReal(), 1.0e-10);
126 Assertions.assertEquals(-FastMath.sin(rate * dt), FieldVector3D.dotProduct(shiftedY, originalX).getReal(), 1.0e-10);
127 Assertions.assertEquals( FastMath.cos(rate * dt), FieldVector3D.dotProduct(shiftedY, originalY).getReal(), 1.0e-10);
128 Assertions.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedY, originalZ).getReal(), 1.0e-10);
129 Assertions.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedZ, originalX).getReal(), 1.0e-10);
130 Assertions.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedZ, originalY).getReal(), 1.0e-10);
131 Assertions.assertEquals( 1.0, FieldVector3D.dotProduct(shiftedZ, originalZ).getReal(), 1.0e-10);
132
133 FieldVector3D<DerivativeStructure> forward = FieldAngularCoordinates.estimateRate(angularCoordinates.getRotation(), shifted.getRotation(), dt);
134 Assertions.assertEquals(0.0, forward.subtract(angularCoordinates.getRotationRate()).getNorm().getReal(), 1.0e-10);
135
136 FieldVector3D<DerivativeStructure> reversed = FieldAngularCoordinates.estimateRate(shifted.getRotation(), angularCoordinates.getRotation(), dt);
137 Assertions.assertEquals(0.0, reversed.add(angularCoordinates.getRotationRate()).getNorm().getReal(), 1.0e-10);
138
139 }
140
141 @Test
142 public void testReverseOffset() {
143 Random random = new Random(0x4ecca9d57a8f1611l);
144 for (int i = 0; i < 100; ++i) {
145 FieldRotation<DerivativeStructure> r = randomRotation(random);
146 FieldVector3D<DerivativeStructure> o = randomVector(random, 1.0e-3);
147 FieldVector3D<DerivativeStructure> a = randomVector(random, 1.0e-3);
148 TimeStampedFieldAngularCoordinates<DerivativeStructure> ac =
149 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH, r, o, a);
150 TimeStampedFieldAngularCoordinates<DerivativeStructure> sum = ac.addOffset(ac.revert());
151 Assertions.assertEquals(0.0, sum.getRotation().getAngle().getReal(), 1.0e-15);
152 Assertions.assertEquals(0.0, sum.getRotationRate().getNorm().getReal(), 1.0e-15);
153 Assertions.assertEquals(0.0, sum.getRotationAcceleration().getNorm().getReal(), 1.0e-15);
154 }
155 }
156
157 @Test
158 public void testNoCommute() {
159 TimeStampedFieldAngularCoordinates<DerivativeStructure> ac1 =
160 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH,
161 createRotation(0.48, 0.64, 0.36, 0.48, false),
162 createVector(0, 0, 0, 4),
163 createVector(0, 0, 0, 4));
164 TimeStampedFieldAngularCoordinates<DerivativeStructure> ac2 =
165 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH,
166 createRotation(0.36, -0.48, 0.48, 0.64, false),
167 createVector(0, 0, 0, 4),
168 createVector(0, 0, 0, 4));
169
170 TimeStampedFieldAngularCoordinates<DerivativeStructure> add12 = ac1.addOffset(ac2);
171 TimeStampedFieldAngularCoordinates<DerivativeStructure> add21 = ac2.addOffset(ac1);
172
173
174 Assertions.assertEquals(2.574, FieldRotation.distance(add12.getRotation(), add21.getRotation()).getReal(), 1.0e-3);
175
176 }
177
178 @Test
179 public void testRoundTripNoOp() {
180 Random random = new Random(0x1e610cfe89306669l);
181 for (int i = 0; i < 100; ++i) {
182
183 FieldRotation<DerivativeStructure> r1 = randomRotation(random);
184 FieldVector3D<DerivativeStructure> o1 = randomVector(random, 1.0e-2);
185 FieldVector3D<DerivativeStructure> a1 = randomVector(random, 1.0e-2);
186 TimeStampedFieldAngularCoordinates<DerivativeStructure> ac1 =
187 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH, r1, o1, a1);
188 FieldRotation<DerivativeStructure> r2 = randomRotation(random);
189 FieldVector3D<DerivativeStructure> o2 = randomVector(random, 1.0e-2);
190 FieldVector3D<DerivativeStructure> a2 = randomVector(random, 1.0e-2);
191
192 TimeStampedFieldAngularCoordinates<DerivativeStructure> ac2 =
193 new TimeStampedFieldAngularCoordinates<>(AbsoluteDate.J2000_EPOCH, r2, o2, a2);
194 TimeStampedFieldAngularCoordinates<DerivativeStructure> roundTripSA = ac1.subtractOffset(ac2).addOffset(ac2);
195 Assertions.assertEquals(0.0, FieldRotation.distance(ac1.getRotation(), roundTripSA.getRotation()).getReal(), 4.0e-16);
196 Assertions.assertEquals(0.0, FieldVector3D.distance(ac1.getRotationRate(), roundTripSA.getRotationRate()).getReal(), 2.0e-17);
197 Assertions.assertEquals(0.0, FieldVector3D.distance(ac1.getRotationAcceleration(), roundTripSA.getRotationAcceleration()).getReal(), 1.0e-17);
198
199 TimeStampedFieldAngularCoordinates<DerivativeStructure> roundTripAS = ac1.addOffset(ac2).subtractOffset(ac2);
200 Assertions.assertEquals(0.0, FieldRotation.distance(ac1.getRotation(), roundTripAS.getRotation()).getReal(), 6.0e-16);
201 Assertions.assertEquals(0.0, FieldVector3D.distance(ac1.getRotationRate(), roundTripAS.getRotationRate()).getReal(), 2.0e-17);
202 Assertions.assertEquals(0.0, FieldVector3D.distance(ac1.getRotationAcceleration(), roundTripAS.getRotationAcceleration()).getReal(), 2.0e-17);
203
204 }
205 }
206
207 @Test
208 public void testDerivativesStructures0() {
209 RandomGenerator random = new Well1024a(0x18a0a08fd63f047al);
210
211 FieldRotation<Binary64> r = randomRotation64(random);
212 FieldVector3D<Binary64> o = randomVector64(random, 1.0e-2);
213 FieldVector3D<Binary64> oDot = randomVector64(random, 1.0e-2);
214 TimeStampedFieldAngularCoordinates<Binary64> ac =
215 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Binary64Field.getInstance()),
216 r, o, oDot);
217 TimeStampedFieldAngularCoordinates<Binary64> rebuilt =
218 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Binary64Field.getInstance()),
219 ac.toDerivativeStructureRotation(0));
220 Assertions.assertEquals(0.0, FieldRotation.distance(ac.getRotation(), rebuilt.getRotation()).getReal(), 1.0e-15);
221 Assertions.assertEquals(0.0, rebuilt.getRotationRate().getNorm().getReal(), 1.0e-15);
222 Assertions.assertEquals(0.0, rebuilt.getRotationAcceleration().getNorm().getReal(), 1.0e-15);
223 }
224
225 @Test
226 public void testDerivativesStructures1() {
227 RandomGenerator random = new Well1024a(0x8f8fc6d27bbdc46dl);
228
229 FieldRotation<Binary64> r = randomRotation64(random);
230 FieldVector3D<Binary64> o = randomVector64(random, 1.0e-2);
231 FieldVector3D<Binary64> oDot = randomVector64(random, 1.0e-2);
232 TimeStampedFieldAngularCoordinates<Binary64> ac =
233 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Binary64Field.getInstance()),
234 r, o, oDot);
235 TimeStampedFieldAngularCoordinates<Binary64> rebuilt =
236 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Binary64Field.getInstance()),
237 ac.toDerivativeStructureRotation(1));
238 Assertions.assertEquals(0.0, FieldRotation.distance(ac.getRotation(), rebuilt.getRotation()).getReal(), 1.0e-15);
239 Assertions.assertEquals(0.0, FieldVector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()).getReal(), 1.0e-15);
240 Assertions.assertEquals(0.0, rebuilt.getRotationAcceleration().getNorm().getReal(), 1.0e-15);
241 }
242
243 @Test
244 public void testDerivativesStructures2() {
245 RandomGenerator random = new Well1024a(0x1633878dddac047dl);
246
247 FieldRotation<Binary64> r = randomRotation64(random);
248 FieldVector3D<Binary64> o = randomVector64(random, 1.0e-2);
249 FieldVector3D<Binary64> oDot = randomVector64(random, 1.0e-2);
250 TimeStampedFieldAngularCoordinates<Binary64> ac =
251 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Binary64Field.getInstance()),
252 r, o, oDot);
253 TimeStampedFieldAngularCoordinates<Binary64> rebuilt =
254 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Binary64Field.getInstance()),
255 ac.toDerivativeStructureRotation(2));
256 Assertions.assertEquals(0.0, FieldRotation.distance(ac.getRotation(), rebuilt.getRotation()).getReal(), 1.0e-15);
257 Assertions.assertEquals(0.0, FieldVector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()).getReal(), 1.0e-15);
258 Assertions.assertEquals(0.0, FieldVector3D.distance(ac.getRotationAcceleration(), rebuilt.getRotationAcceleration()).getReal(), 1.0e-15);
259
260 }
261
262 @Test
263 public void testUnivariateDerivative1() {
264 RandomGenerator random = new Well1024a(0x6de8cce747539904l);
265
266 FieldRotation<Binary64> r = randomRotation64(random);
267 FieldVector3D<Binary64> o = randomVector64(random, 1.0e-2);
268 FieldVector3D<Binary64> oDot = randomVector64(random, 1.0e-2);
269 TimeStampedFieldAngularCoordinates<Binary64> ac =
270 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Binary64Field.getInstance()),
271 r, o, oDot);
272 FieldRotation<FieldUnivariateDerivative1<Binary64>> rotationUD = ac.toUnivariateDerivative1Rotation();
273 FieldRotation<FieldDerivativeStructure<Binary64>> rotationDS = ac.toDerivativeStructureRotation(1);
274 Assertions.assertEquals(rotationDS.getQ0().getReal(), rotationUD.getQ0().getReal(), 1.0e-15);
275 Assertions.assertEquals(rotationDS.getQ1().getReal(), rotationUD.getQ1().getReal(), 1.0e-15);
276 Assertions.assertEquals(rotationDS.getQ2().getReal(), rotationUD.getQ2().getReal(), 1.0e-15);
277 Assertions.assertEquals(rotationDS.getQ3().getReal(), rotationUD.getQ3().getReal(), 1.0e-15);
278 Assertions.assertEquals(rotationDS.getQ0().getPartialDerivative(1).getReal(), rotationUD.getQ0().getFirstDerivative().getReal(), 1.0e-15);
279 Assertions.assertEquals(rotationDS.getQ1().getPartialDerivative(1).getReal(), rotationUD.getQ1().getFirstDerivative().getReal(), 1.0e-15);
280 Assertions.assertEquals(rotationDS.getQ2().getPartialDerivative(1).getReal(), rotationUD.getQ2().getFirstDerivative().getReal(), 1.0e-15);
281 Assertions.assertEquals(rotationDS.getQ3().getPartialDerivative(1).getReal(), rotationUD.getQ3().getFirstDerivative().getReal(), 1.0e-15);
282
283 TimeStampedFieldAngularCoordinates<Binary64> rebuilt =
284 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Binary64Field.getInstance()),
285 rotationUD);
286 Assertions.assertEquals(0.0, FieldRotation.distance(ac.getRotation(), rebuilt.getRotation()).getReal(), 1.0e-15);
287 Assertions.assertEquals(0.0, FieldVector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()).getReal(), 1.0e-15);
288
289 }
290
291 @Test
292 public void testUnivariateDerivative2() {
293 RandomGenerator random = new Well1024a(0x255710c8fa2247ecl);
294
295 FieldRotation<Binary64> r = randomRotation64(random);
296 FieldVector3D<Binary64> o = randomVector64(random, 1.0e-2);
297 FieldVector3D<Binary64> oDot = randomVector64(random, 1.0e-2);
298 TimeStampedFieldAngularCoordinates<Binary64> ac =
299 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Binary64Field.getInstance()),
300 r, o, oDot);
301 FieldRotation<FieldUnivariateDerivative2<Binary64>> rotationUD = ac.toUnivariateDerivative2Rotation();
302 FieldRotation<FieldDerivativeStructure<Binary64>> rotationDS = ac.toDerivativeStructureRotation(2);
303 Assertions.assertEquals(rotationDS.getQ0().getReal(), rotationUD.getQ0().getReal(), 1.0e-15);
304 Assertions.assertEquals(rotationDS.getQ1().getReal(), rotationUD.getQ1().getReal(), 1.0e-15);
305 Assertions.assertEquals(rotationDS.getQ2().getReal(), rotationUD.getQ2().getReal(), 1.0e-15);
306 Assertions.assertEquals(rotationDS.getQ3().getReal(), rotationUD.getQ3().getReal(), 1.0e-15);
307 Assertions.assertEquals(rotationDS.getQ0().getPartialDerivative(1).getReal(), rotationUD.getQ0().getFirstDerivative().getReal(), 1.0e-15);
308 Assertions.assertEquals(rotationDS.getQ1().getPartialDerivative(1).getReal(), rotationUD.getQ1().getFirstDerivative().getReal(), 1.0e-15);
309 Assertions.assertEquals(rotationDS.getQ2().getPartialDerivative(1).getReal(), rotationUD.getQ2().getFirstDerivative().getReal(), 1.0e-15);
310 Assertions.assertEquals(rotationDS.getQ3().getPartialDerivative(1).getReal(), rotationUD.getQ3().getFirstDerivative().getReal(), 1.0e-15);
311 Assertions.assertEquals(rotationDS.getQ0().getPartialDerivative(2).getReal(), rotationUD.getQ0().getSecondDerivative().getReal(), 1.0e-15);
312 Assertions.assertEquals(rotationDS.getQ1().getPartialDerivative(2).getReal(), rotationUD.getQ1().getSecondDerivative().getReal(), 1.0e-15);
313 Assertions.assertEquals(rotationDS.getQ2().getPartialDerivative(2).getReal(), rotationUD.getQ2().getSecondDerivative().getReal(), 1.0e-15);
314 Assertions.assertEquals(rotationDS.getQ3().getPartialDerivative(2).getReal(), rotationUD.getQ3().getSecondDerivative().getReal(), 1.0e-15);
315
316 TimeStampedFieldAngularCoordinates<Binary64> rebuilt =
317 new TimeStampedFieldAngularCoordinates<>(FieldAbsoluteDate.getGalileoEpoch(Binary64Field.getInstance()),
318 rotationUD);
319 Assertions.assertEquals(0.0, FieldRotation.distance(ac.getRotation(), rebuilt.getRotation()).getReal(), 1.0e-15);
320 Assertions.assertEquals(0.0, FieldVector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()).getReal(), 1.0e-15);
321 Assertions.assertEquals(0.0, FieldVector3D.distance(ac.getRotationAcceleration(), rebuilt.getRotationAcceleration()).getReal(), 1.0e-15);
322
323 }
324
325 @Test
326 public void testIssue773() {
327 doTestIssue773(Binary64Field.getInstance());
328 }
329
330 private <T extends CalculusFieldElement<T>> void doTestIssue773(final Field<T> field) {
331
332 final AbsoluteDate date = new AbsoluteDate();
333
334
335 final TimeStampedAngularCoordinates angular =
336 new TimeStampedAngularCoordinates(date,
337 new Rotation(0., 0., 0., 0., false),
338 Vector3D.ZERO,
339 Vector3D.ZERO);
340
341
342 final FieldTimeStamped<T> timeStamped =
343 new TimeStampedFieldAngularCoordinates<>(field, angular);
344
345
346 Assertions.assertEquals(0.0, date.durationFrom(timeStamped.getDate().toAbsoluteDate()), Double.MIN_VALUE);
347 }
348
349 private FieldVector3D<DerivativeStructure> randomVector(Random random, double norm) {
350 double n = random.nextDouble() * norm;
351 double x = random.nextDouble();
352 double y = random.nextDouble();
353 double z = random.nextDouble();
354 return new FieldVector3D<>(n, createVector(x, y, z, 4).normalize());
355 }
356
357 private FieldRotation<DerivativeStructure> randomRotation(Random random) {
358 double q0 = random.nextDouble() * 2 - 1;
359 double q1 = random.nextDouble() * 2 - 1;
360 double q2 = random.nextDouble() * 2 - 1;
361 double q3 = random.nextDouble() * 2 - 1;
362 double q = FastMath.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
363 return createRotation(q0 / q, q1 / q, q2 / q, q3 / q, false);
364 }
365
366 public static FieldRotation<DerivativeStructure> createRotation(FieldVector3D<DerivativeStructure> axis, double angle) {
367 return new FieldRotation<>(axis,
368 new DSFactory(4, 1).constant(angle),
369 RotationConvention.VECTOR_OPERATOR);
370 }
371
372 public static FieldRotation<DerivativeStructure> createRotation(double q0, double q1, double q2, double q3,
373 boolean needsNormalization) {
374 DSFactory factory = new DSFactory(4, 1);
375 return new FieldRotation<>(factory.variable(0, q0),
376 factory.variable(1, q1),
377 factory.variable(2, q2),
378 factory.variable(3, q3),
379 needsNormalization);
380 }
381
382 public static FieldVector3D<DerivativeStructure> createVector(double x, double y, double z, int params) {
383 DSFactory factory = new DSFactory(params, 1);
384 return new FieldVector3D<>(factory.variable(0, x),
385 factory.variable(1, y),
386 factory.variable(2, z));
387 }
388
389 private FieldRotation<Binary64> randomRotation64(RandomGenerator random) {
390 double q0 = random.nextDouble() * 2 - 1;
391 double q1 = random.nextDouble() * 2 - 1;
392 double q2 = random.nextDouble() * 2 - 1;
393 double q3 = random.nextDouble() * 2 - 1;
394 double q = FastMath.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
395 return new FieldRotation<>(new Binary64(q0 / q),
396 new Binary64(q1 / q),
397 new Binary64(q2 / q),
398 new Binary64(q3 / q),
399 false);
400 }
401
402 private FieldVector3D<Binary64> randomVector64(RandomGenerator random, double norm) {
403 double n = random.nextDouble() * norm;
404 double x = random.nextDouble();
405 double y = random.nextDouble();
406 double z = random.nextDouble();
407 return new FieldVector3D<>(n, new FieldVector3D<>(new Binary64(x), new Binary64(y), new Binary64(z)).normalize());
408 }
409
410 @BeforeEach
411 public void setUp() {
412 Utils.setDataRoot("regular-data");
413 }
414
415 }
416