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.analysis.differentiation.DerivativeStructure;
20 import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
21 import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
22 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
23 import org.hipparchus.geometry.euclidean.threed.Rotation;
24 import org.hipparchus.geometry.euclidean.threed.Vector3D;
25 import org.hipparchus.random.RandomGenerator;
26 import org.hipparchus.random.Well1024a;
27 import org.hipparchus.util.FastMath;
28 import org.junit.jupiter.api.Assertions;
29 import org.junit.jupiter.api.BeforeEach;
30 import org.junit.jupiter.api.Test;
31 import org.orekit.Utils;
32 import org.orekit.time.AbsoluteDate;
33
34 public class TimeStampedAngularCoordinatesTest {
35
36 @Test
37 public void testZeroRate() {
38 TimeStampedAngularCoordinates ac =
39 new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH,
40 new Rotation(0.48, 0.64, 0.36, 0.48, false),
41 Vector3D.ZERO, Vector3D.ZERO);
42 Assertions.assertEquals(Vector3D.ZERO, ac.getRotationRate());
43 double dt = 10.0;
44 TimeStampedAngularCoordinates shifted = ac.shiftedBy(dt);
45 Assertions.assertEquals(Vector3D.ZERO, shifted.getRotationAcceleration());
46 Assertions.assertEquals(Vector3D.ZERO, shifted.getRotationRate());
47 Assertions.assertEquals(0.0, Rotation.distance(ac.getRotation(), shifted.getRotation()), 1.0e-15);
48 }
49
50 @Test
51 public void testOnePair() throws java.io.IOException {
52 RandomGenerator random = new Well1024a(0xed7dd911a44c5197l);
53
54 for (int i = 0; i < 20; ++i) {
55
56 Rotation r = randomRotation(random);
57 Vector3D o = randomVector(random, 1.0e-2);
58 Vector3D a = randomVector(random, 1.0e-2);
59 TimeStampedAngularCoordinates reference = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, a);
60
61 PVCoordinates u = randomPVCoordinates(random, 1000, 1.0, 0.001);
62 PVCoordinates v = reference.applyTo(u);
63 TimeStampedAngularCoordinates ac =
64 new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, u, v);
65
66 Assertions.assertEquals(0, Vector3D.distance(v.getPosition().normalize(), ac.applyTo(u).getPosition().normalize()), 1.0e-14);
67 Assertions.assertEquals(0, Vector3D.distance(v.getVelocity().normalize(), ac.applyTo(u).getVelocity().normalize()), 4.0e-14);
68 Assertions.assertEquals(0, Vector3D.distance(v.getAcceleration().normalize(), ac.applyTo(u).getAcceleration().normalize()), 1.0e-14);
69
70 }
71
72 }
73
74 @Test
75 public void testTwoPairs() throws java.io.IOException {
76 RandomGenerator random = new Well1024a(0x976ad943966c9f00l);
77
78 for (int i = 0; i < 20; ++i) {
79
80 Rotation r = randomRotation(random);
81 Vector3D o = randomVector(random, 1.0e-2);
82 Vector3D a = randomVector(random, 1.0e-2);
83 TimeStampedAngularCoordinates reference = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, a);
84
85 PVCoordinates u1 = randomPVCoordinates(random, 1000, 1.0, 0.001);
86 PVCoordinates u2 = randomPVCoordinates(random, 1000, 1.0, 0.001);
87 PVCoordinates v1 = reference.applyTo(u1);
88 PVCoordinates v2 = reference.applyTo(u2);
89 TimeStampedAngularCoordinates ac =
90 new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, u1, u2, v1, v2, 1.0e-9);
91
92 Assertions.assertEquals(0, Vector3D.distance(v1.getPosition().normalize(), ac.applyTo(u1).getPosition().normalize()), 1.0e-14);
93 Assertions.assertEquals(0, Vector3D.distance(v1.getVelocity().normalize(), ac.applyTo(u1).getVelocity().normalize()), 1.0e-14);
94 Assertions.assertEquals(0, Vector3D.distance(v1.getAcceleration().normalize(), ac.applyTo(u1).getAcceleration().normalize()), 1.0e-14);
95 Assertions.assertEquals(0, Vector3D.distance(v2.getPosition().normalize(), ac.applyTo(u2).getPosition().normalize()), 1.0e-14);
96 Assertions.assertEquals(0, Vector3D.distance(v2.getVelocity().normalize(), ac.applyTo(u2).getVelocity().normalize()), 1.0e-14);
97 Assertions.assertEquals(0, Vector3D.distance(v2.getAcceleration().normalize(), ac.applyTo(u2).getAcceleration().normalize()), 1.0e-14);
98
99 }
100
101 }
102
103 @Test
104 public void testDerivativesStructures0() {
105 RandomGenerator random = new Well1024a(0x18a0a08fd63f047al);
106
107 Rotation r = randomRotation(random);
108 Vector3D o = randomVector(random, 1.0e-2);
109 Vector3D oDot = randomVector(random, 1.0e-2);
110 TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, oDot);
111 TimeStampedAngularCoordinates rebuilt = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH,
112 ac.toDerivativeStructureRotation(0));
113 Assertions.assertEquals(0.0, Rotation.distance(ac.getRotation(), rebuilt.getRotation()), 1.0e-15);
114 Assertions.assertEquals(0.0, rebuilt.getRotationRate().getNorm(), 1.0e-15);
115 Assertions.assertEquals(0.0, rebuilt.getRotationAcceleration().getNorm(), 1.0e-15);
116 }
117
118 @Test
119 public void testDerivativesStructures1() {
120 RandomGenerator random = new Well1024a(0x8f8fc6d27bbdc46dl);
121
122 Rotation r = randomRotation(random);
123 Vector3D o = randomVector(random, 1.0e-2);
124 Vector3D oDot = randomVector(random, 1.0e-2);
125 TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, oDot);
126 TimeStampedAngularCoordinates rebuilt = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH,
127 ac.toDerivativeStructureRotation(1));
128 Assertions.assertEquals(0.0, Rotation.distance(ac.getRotation(), rebuilt.getRotation()), 1.0e-15);
129 Assertions.assertEquals(0.0, Vector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()), 1.0e-15);
130 Assertions.assertEquals(0.0, rebuilt.getRotationAcceleration().getNorm(), 1.0e-15);
131 }
132
133 @Test
134 public void testDerivativesStructures2() {
135 RandomGenerator random = new Well1024a(0x1633878dddac047dl);
136
137 Rotation r = randomRotation(random);
138 Vector3D o = randomVector(random, 1.0e-2);
139 Vector3D oDot = randomVector(random, 1.0e-2);
140 TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, oDot);
141 TimeStampedAngularCoordinates rebuilt = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH,
142 ac.toDerivativeStructureRotation(2));
143 Assertions.assertEquals(0.0, Rotation.distance(ac.getRotation(), rebuilt.getRotation()), 1.0e-15);
144 Assertions.assertEquals(0.0, Vector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()), 1.0e-15);
145 Assertions.assertEquals(0.0, Vector3D.distance(ac.getRotationAcceleration(), rebuilt.getRotationAcceleration()), 1.0e-15);
146 }
147
148 @Test
149 public void testUnivariateDerivative1() {
150 RandomGenerator random = new Well1024a(0x6de8cce747539904l);
151
152 Rotation r = randomRotation(random);
153 Vector3D o = randomVector(random, 1.0e-2);
154 Vector3D oDot = randomVector(random, 1.0e-2);
155 TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, oDot);
156 FieldRotation<UnivariateDerivative1> rotationUD = ac.toUnivariateDerivative1Rotation();
157 FieldRotation<DerivativeStructure> rotationDS = ac.toDerivativeStructureRotation(1);
158 Assertions.assertEquals(rotationDS.getQ0().getReal(), rotationUD.getQ0().getReal(), 1.0e-15);
159 Assertions.assertEquals(rotationDS.getQ1().getReal(), rotationUD.getQ1().getReal(), 1.0e-15);
160 Assertions.assertEquals(rotationDS.getQ2().getReal(), rotationUD.getQ2().getReal(), 1.0e-15);
161 Assertions.assertEquals(rotationDS.getQ3().getReal(), rotationUD.getQ3().getReal(), 1.0e-15);
162 Assertions.assertEquals(rotationDS.getQ0().getPartialDerivative(1), rotationUD.getQ0().getFirstDerivative(), 1.0e-15);
163 Assertions.assertEquals(rotationDS.getQ1().getPartialDerivative(1), rotationUD.getQ1().getFirstDerivative(), 1.0e-15);
164 Assertions.assertEquals(rotationDS.getQ2().getPartialDerivative(1), rotationUD.getQ2().getFirstDerivative(), 1.0e-15);
165 Assertions.assertEquals(rotationDS.getQ3().getPartialDerivative(1), rotationUD.getQ3().getFirstDerivative(), 1.0e-15);
166
167 TimeStampedAngularCoordinates rebuilt = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, rotationUD);
168 Assertions.assertEquals(0.0, Rotation.distance(ac.getRotation(), rebuilt.getRotation()), 1.0e-15);
169 Assertions.assertEquals(0.0, Vector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()), 1.0e-15);
170
171 }
172
173 @Test
174 public void testUnivariateDerivative2() {
175 RandomGenerator random = new Well1024a(0x255710c8fa2247ecl);
176
177 Rotation r = randomRotation(random);
178 Vector3D o = randomVector(random, 1.0e-2);
179 Vector3D oDot = randomVector(random, 1.0e-2);
180 TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, oDot);
181 FieldRotation<UnivariateDerivative2> rotationUD = ac.toUnivariateDerivative2Rotation();
182 FieldRotation<DerivativeStructure> rotationDS = ac.toDerivativeStructureRotation(2);
183 Assertions.assertEquals(rotationDS.getQ0().getReal(), rotationUD.getQ0().getReal(), 1.0e-15);
184 Assertions.assertEquals(rotationDS.getQ1().getReal(), rotationUD.getQ1().getReal(), 1.0e-15);
185 Assertions.assertEquals(rotationDS.getQ2().getReal(), rotationUD.getQ2().getReal(), 1.0e-15);
186 Assertions.assertEquals(rotationDS.getQ3().getReal(), rotationUD.getQ3().getReal(), 1.0e-15);
187 Assertions.assertEquals(rotationDS.getQ0().getPartialDerivative(1), rotationUD.getQ0().getFirstDerivative(), 1.0e-15);
188 Assertions.assertEquals(rotationDS.getQ1().getPartialDerivative(1), rotationUD.getQ1().getFirstDerivative(), 1.0e-15);
189 Assertions.assertEquals(rotationDS.getQ2().getPartialDerivative(1), rotationUD.getQ2().getFirstDerivative(), 1.0e-15);
190 Assertions.assertEquals(rotationDS.getQ3().getPartialDerivative(1), rotationUD.getQ3().getFirstDerivative(), 1.0e-15);
191 Assertions.assertEquals(rotationDS.getQ0().getPartialDerivative(2), rotationUD.getQ0().getSecondDerivative(), 1.0e-15);
192 Assertions.assertEquals(rotationDS.getQ1().getPartialDerivative(2), rotationUD.getQ1().getSecondDerivative(), 1.0e-15);
193 Assertions.assertEquals(rotationDS.getQ2().getPartialDerivative(2), rotationUD.getQ2().getSecondDerivative(), 1.0e-15);
194 Assertions.assertEquals(rotationDS.getQ3().getPartialDerivative(2), rotationUD.getQ3().getSecondDerivative(), 1.0e-15);
195
196 TimeStampedAngularCoordinates rebuilt = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, rotationUD);
197 Assertions.assertEquals(0.0, Rotation.distance(ac.getRotation(), rebuilt.getRotation()), 1.0e-15);
198 Assertions.assertEquals(0.0, Vector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()), 1.0e-15);
199 Assertions.assertEquals(0.0, Vector3D.distance(ac.getRotationAcceleration(), rebuilt.getRotationAcceleration()), 1.0e-15);
200
201 }
202
203 @Test
204 public void testShift() {
205 double rate = 2 * FastMath.PI / (12 * 60);
206 TimeStampedAngularCoordinates ac =
207 new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH,
208 Rotation.IDENTITY,
209 new Vector3D(rate, Vector3D.PLUS_K), Vector3D.ZERO);
210 Assertions.assertEquals(rate, ac.getRotationRate().getNorm(), 1.0e-10);
211 double dt = 10.0;
212 double alpha = rate * dt;
213 TimeStampedAngularCoordinates shifted = ac.shiftedBy(dt);
214 Assertions.assertEquals(rate, shifted.getRotationRate().getNorm(), 1.0e-10);
215 Assertions.assertEquals(alpha, Rotation.distance(ac.getRotation(), shifted.getRotation()), 1.0e-10);
216
217 Vector3D xSat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_I);
218 Assertions.assertEquals(0.0, xSat.subtract(new Vector3D(FastMath.cos(alpha), FastMath.sin(alpha), 0)).getNorm(), 1.0e-10);
219 Vector3D ySat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_J);
220 Assertions.assertEquals(0.0, ySat.subtract(new Vector3D(-FastMath.sin(alpha), FastMath.cos(alpha), 0)).getNorm(), 1.0e-10);
221 Vector3D zSat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_K);
222 Assertions.assertEquals(0.0, zSat.subtract(Vector3D.PLUS_K).getNorm(), 1.0e-10);
223
224 }
225
226 @Test
227 public void testSpin() {
228 double rate = 2 * FastMath.PI / (12 * 60);
229 TimeStampedAngularCoordinates ac =
230 new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH,
231 new Rotation(0.48, 0.64, 0.36, 0.48, false),
232 new Vector3D(rate, Vector3D.PLUS_K), Vector3D.ZERO);
233 Assertions.assertEquals(rate, ac.getRotationRate().getNorm(), 1.0e-10);
234 double dt = 10.0;
235 TimeStampedAngularCoordinates shifted = ac.shiftedBy(dt);
236 Assertions.assertEquals(rate, shifted.getRotationRate().getNorm(), 1.0e-10);
237 Assertions.assertEquals(rate * dt, Rotation.distance(ac.getRotation(), shifted.getRotation()), 1.0e-10);
238
239 Vector3D shiftedX = shifted.getRotation().applyInverseTo(Vector3D.PLUS_I);
240 Vector3D shiftedY = shifted.getRotation().applyInverseTo(Vector3D.PLUS_J);
241 Vector3D shiftedZ = shifted.getRotation().applyInverseTo(Vector3D.PLUS_K);
242 Vector3D originalX = ac.getRotation().applyInverseTo(Vector3D.PLUS_I);
243 Vector3D originalY = ac.getRotation().applyInverseTo(Vector3D.PLUS_J);
244 Vector3D originalZ = ac.getRotation().applyInverseTo(Vector3D.PLUS_K);
245 Assertions.assertEquals( FastMath.cos(rate * dt), Vector3D.dotProduct(shiftedX, originalX), 1.0e-10);
246 Assertions.assertEquals( FastMath.sin(rate * dt), Vector3D.dotProduct(shiftedX, originalY), 1.0e-10);
247 Assertions.assertEquals( 0.0, Vector3D.dotProduct(shiftedX, originalZ), 1.0e-10);
248 Assertions.assertEquals(-FastMath.sin(rate * dt), Vector3D.dotProduct(shiftedY, originalX), 1.0e-10);
249 Assertions.assertEquals( FastMath.cos(rate * dt), Vector3D.dotProduct(shiftedY, originalY), 1.0e-10);
250 Assertions.assertEquals( 0.0, Vector3D.dotProduct(shiftedY, originalZ), 1.0e-10);
251 Assertions.assertEquals( 0.0, Vector3D.dotProduct(shiftedZ, originalX), 1.0e-10);
252 Assertions.assertEquals( 0.0, Vector3D.dotProduct(shiftedZ, originalY), 1.0e-10);
253 Assertions.assertEquals( 1.0, Vector3D.dotProduct(shiftedZ, originalZ), 1.0e-10);
254
255 Vector3D forward = TimeStampedAngularCoordinates.estimateRate(ac.getRotation(), shifted.getRotation(), dt);
256 Assertions.assertEquals(0.0, forward.subtract(ac.getRotationRate()).getNorm(), 1.0e-10);
257
258 Vector3D reversed = TimeStampedAngularCoordinates.estimateRate(shifted.getRotation(), ac.getRotation(), dt);
259 Assertions.assertEquals(0.0, reversed.add(ac.getRotationRate()).getNorm(), 1.0e-10);
260
261 }
262
263 @Test
264 public void testReverseOffset() {
265 RandomGenerator random = new Well1024a(0x4ecca9d57a8f1611l);
266 for (int i = 0; i < 100; ++i) {
267 Rotation r = randomRotation(random);
268 Vector3D o = randomVector(random, 1.0e-3);
269 Vector3D a = randomVector(random, 1.0e-3);
270 TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, a);
271 TimeStampedAngularCoordinates sum = ac.addOffset(ac.revert());
272 Assertions.assertEquals(0.0, sum.getRotation().getAngle(), 1.0e-15);
273 Assertions.assertEquals(0.0, sum.getRotationRate().getNorm(), 1.0e-15);
274 Assertions.assertEquals(0.0, sum.getRotationAcceleration().getNorm(), 1.0e-15);
275 }
276 }
277
278 @Test
279 public void testNoCommute() {
280 TimeStampedAngularCoordinates ac1 =
281 new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, new Rotation(0.48, 0.64, 0.36, 0.48, false), Vector3D.ZERO, Vector3D.ZERO);
282 TimeStampedAngularCoordinates ac2 =
283 new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, new Rotation(0.36, -0.48, 0.48, 0.64, false), Vector3D.ZERO, Vector3D.ZERO);
284
285 TimeStampedAngularCoordinates add12 = ac1.addOffset(ac2);
286 TimeStampedAngularCoordinates add21 = ac2.addOffset(ac1);
287
288
289 Assertions.assertEquals(2.574, Rotation.distance(add12.getRotation(), add21.getRotation()), 1.0e-3);
290
291 }
292
293 @Test
294 public void testRoundTripNoOp() {
295 RandomGenerator random = new Well1024a(0x1e610cfe89306669l);
296 for (int i = 0; i < 100; ++i) {
297
298 Rotation r1 = randomRotation(random);
299 Vector3D o1 = randomVector(random, 1.0e-2);
300 Vector3D a1 = randomVector(random, 1.0e-2);
301 TimeStampedAngularCoordinates ac1 = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r1, o1, a1);
302 Rotation r2 = randomRotation(random);
303 Vector3D o2 = randomVector(random, 1.0e-2);
304 Vector3D a2 = randomVector(random, 1.0e-2);
305
306 TimeStampedAngularCoordinates ac2 = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r2, o2, a2);
307 TimeStampedAngularCoordinates roundTripSA = ac1.subtractOffset(ac2).addOffset(ac2);
308 Assertions.assertEquals(0.0, Rotation.distance(ac1.getRotation(), roundTripSA.getRotation()), 4.0e-16);
309 Assertions.assertEquals(0.0, Vector3D.distance(ac1.getRotationRate(), roundTripSA.getRotationRate()), 2.0e-17);
310 Assertions.assertEquals(0.0, Vector3D.distance(ac1.getRotationAcceleration(), roundTripSA.getRotationAcceleration()), 1.0e-17);
311
312 TimeStampedAngularCoordinates roundTripAS = ac1.addOffset(ac2).subtractOffset(ac2);
313 Assertions.assertEquals(0.0, Rotation.distance(ac1.getRotation(), roundTripAS.getRotation()), 6.0e-16);
314 Assertions.assertEquals(0.0, Vector3D.distance(ac1.getRotationRate(), roundTripAS.getRotationRate()), 2.0e-17);
315 Assertions.assertEquals(0.0, Vector3D.distance(ac1.getRotationAcceleration(), roundTripAS.getRotationAcceleration()), 2.0e-17);
316
317 }
318 }
319
320 private Vector3D randomVector(RandomGenerator random, double norm) {
321 double n = random.nextDouble() * norm;
322 double x = 2 * random.nextDouble() - 1;
323 double y = 2 * random.nextDouble() - 1;
324 double z = 2 * random.nextDouble() - 1;
325 return new Vector3D(n, new Vector3D(x, y, z).normalize());
326 }
327
328 private PVCoordinates randomPVCoordinates(RandomGenerator random,
329 double norm0, double norm1, double norm2) {
330 Vector3D p0 = randomVector(random, norm0);
331 Vector3D p1 = randomVector(random, norm1);
332 Vector3D p2 = randomVector(random, norm2);
333 return new PVCoordinates(p0, p1, p2);
334 }
335
336 private Rotation randomRotation(RandomGenerator random) {
337 double q0 = random.nextDouble() * 2 - 1;
338 double q1 = random.nextDouble() * 2 - 1;
339 double q2 = random.nextDouble() * 2 - 1;
340 double q3 = random.nextDouble() * 2 - 1;
341 double q = FastMath.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
342 return new Rotation(q0 / q, q1 / q, q2 / q, q3 / q, false);
343 }
344
345 @BeforeEach
346 public void setUp() {
347 Utils.setDataRoot("regular-data");
348 }
349
350 }
351