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