1   /* Copyright 2002-2020 CS GROUP
2    * Licensed to CS GROUP (CS) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * CS licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *   http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  package org.orekit.utils;
18  
19  
20  import java.util.ArrayList;
21  import java.util.List;
22  
23  import org.hipparchus.geometry.euclidean.threed.Rotation;
24  import org.hipparchus.geometry.euclidean.threed.RotationConvention;
25  import org.hipparchus.geometry.euclidean.threed.Vector3D;
26  import org.hipparchus.ode.ODEIntegrator;
27  import org.hipparchus.ode.ODEState;
28  import org.hipparchus.ode.ODEStateAndDerivative;
29  import org.hipparchus.ode.OrdinaryDifferentialEquation;
30  import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
31  import org.hipparchus.ode.sampling.ODEFixedStepHandler;
32  import org.hipparchus.ode.sampling.StepNormalizer;
33  import org.hipparchus.random.RandomGenerator;
34  import org.hipparchus.random.Well1024a;
35  import org.hipparchus.util.FastMath;
36  import org.hipparchus.util.MathArrays;
37  import org.junit.Assert;
38  import org.junit.Test;
39  import org.orekit.errors.OrekitException;
40  import org.orekit.errors.OrekitMessages;
41  import org.orekit.time.AbsoluteDate;
42  import org.orekit.time.TimeScalesFactory;
43  
44  public class TimeStampedAngularCoordinatesTest {
45  
46      @Test
47      public void testZeroRate() {
48          TimeStampedAngularCoordinates ac =
49                  new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH,
50                                                    new Rotation(0.48, 0.64, 0.36, 0.48, false),
51                                                    Vector3D.ZERO, Vector3D.ZERO);
52          Assert.assertEquals(Vector3D.ZERO, ac.getRotationRate());
53          double dt = 10.0;
54          TimeStampedAngularCoordinates shifted = ac.shiftedBy(dt);
55          Assert.assertEquals(Vector3D.ZERO, shifted.getRotationAcceleration());
56          Assert.assertEquals(Vector3D.ZERO, shifted.getRotationRate());
57          Assert.assertEquals(0.0, Rotation.distance(ac.getRotation(), shifted.getRotation()), 1.0e-15);
58      }
59  
60      @Test
61      public void testOnePair() throws java.io.IOException {
62          RandomGenerator random = new Well1024a(0xed7dd911a44c5197l);
63  
64          for (int i = 0; i < 20; ++i) {
65  
66              Rotation r = randomRotation(random);
67              Vector3D o = randomVector(random, 1.0e-2);
68              Vector3D a = randomVector(random, 1.0e-2);
69              TimeStampedAngularCoordinates reference = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, a);
70  
71              PVCoordinates u = randomPVCoordinates(random, 1000, 1.0, 0.001);
72              PVCoordinates v = reference.applyTo(u);
73              TimeStampedAngularCoordinates ac =
74                      new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, u, v);
75  
76              Assert.assertEquals(0, Vector3D.distance(v.getPosition().normalize(), ac.applyTo(u).getPosition().normalize()), 1.0e-14);
77              Assert.assertEquals(0, Vector3D.distance(v.getVelocity().normalize(), ac.applyTo(u).getVelocity().normalize()), 4.0e-14);
78              Assert.assertEquals(0, Vector3D.distance(v.getAcceleration().normalize(), ac.applyTo(u).getAcceleration().normalize()), 1.0e-14);
79  
80          }
81  
82      }
83  
84      @Test
85      public void testTwoPairs() throws java.io.IOException {
86          RandomGenerator random = new Well1024a(0x976ad943966c9f00l);
87  
88          for (int i = 0; i < 20; ++i) {
89  
90              Rotation r = randomRotation(random);
91              Vector3D o = randomVector(random, 1.0e-2);
92              Vector3D a = randomVector(random, 1.0e-2);
93              TimeStampedAngularCoordinates reference = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, a);
94  
95              PVCoordinates u1 = randomPVCoordinates(random, 1000, 1.0, 0.001);
96              PVCoordinates u2 = randomPVCoordinates(random, 1000, 1.0, 0.001);
97              PVCoordinates v1 = reference.applyTo(u1);
98              PVCoordinates v2 = reference.applyTo(u2);
99              TimeStampedAngularCoordinates ac =
100                     new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, u1, u2, v1, v2, 1.0e-9);
101 
102             Assert.assertEquals(0, Vector3D.distance(v1.getPosition().normalize(), ac.applyTo(u1).getPosition().normalize()), 1.0e-14);
103             Assert.assertEquals(0, Vector3D.distance(v1.getVelocity().normalize(), ac.applyTo(u1).getVelocity().normalize()), 1.0e-14);
104             Assert.assertEquals(0, Vector3D.distance(v1.getAcceleration().normalize(), ac.applyTo(u1).getAcceleration().normalize()), 1.0e-14);
105             Assert.assertEquals(0, Vector3D.distance(v2.getPosition().normalize(), ac.applyTo(u2).getPosition().normalize()), 1.0e-14);
106             Assert.assertEquals(0, Vector3D.distance(v2.getVelocity().normalize(), ac.applyTo(u2).getVelocity().normalize()), 1.0e-14);
107             Assert.assertEquals(0, Vector3D.distance(v2.getAcceleration().normalize(), ac.applyTo(u2).getAcceleration().normalize()), 1.0e-14);
108 
109         }
110 
111     }
112 
113     @Test
114     public void testDerivativesStructures2() {
115         RandomGenerator random = new Well1024a(0x75fbebbdbf127b3dl);
116 
117         Rotation r    = randomRotation(random);
118         Vector3D o    = randomVector(random, 1.0e-2);
119         Vector3D oDot = randomVector(random, 1.0e-2);
120         TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH,
121                                                                              r, o, oDot);
122         TimeStampedAngularCoordinates rebuilt =
123                 new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH,
124                                                   ac.toDerivativeStructureRotation(2));
125         Assert.assertEquals(0.0, Rotation.distance(ac.getRotation(), rebuilt.getRotation()), 1.0e-15);
126         Assert.assertEquals(0.0, Vector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()), 1.0e-15);
127         Assert.assertEquals(0.0, Vector3D.distance(ac.getRotationAcceleration(), rebuilt.getRotationAcceleration()), 1.0e-15);
128     }
129 
130     @Test
131     public void testShift() {
132         double rate = 2 * FastMath.PI / (12 * 60);
133         TimeStampedAngularCoordinates ac =
134                 new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH,
135                                                   Rotation.IDENTITY,
136                                                   new Vector3D(rate, Vector3D.PLUS_K), Vector3D.ZERO);
137         Assert.assertEquals(rate, ac.getRotationRate().getNorm(), 1.0e-10);
138         double dt = 10.0;
139         double alpha = rate * dt;
140         TimeStampedAngularCoordinates shifted = ac.shiftedBy(dt);
141         Assert.assertEquals(rate, shifted.getRotationRate().getNorm(), 1.0e-10);
142         Assert.assertEquals(alpha, Rotation.distance(ac.getRotation(), shifted.getRotation()), 1.0e-10);
143 
144         Vector3D xSat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_I);
145         Assert.assertEquals(0.0, xSat.subtract(new Vector3D(FastMath.cos(alpha), FastMath.sin(alpha), 0)).getNorm(), 1.0e-10);
146         Vector3D ySat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_J);
147         Assert.assertEquals(0.0, ySat.subtract(new Vector3D(-FastMath.sin(alpha), FastMath.cos(alpha), 0)).getNorm(), 1.0e-10);
148         Vector3D zSat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_K);
149         Assert.assertEquals(0.0, zSat.subtract(Vector3D.PLUS_K).getNorm(), 1.0e-10);
150 
151     }
152 
153     @Test
154     public void testSpin() {
155         double rate = 2 * FastMath.PI / (12 * 60);
156         TimeStampedAngularCoordinates ac =
157                 new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH,
158                                                   new Rotation(0.48, 0.64, 0.36, 0.48, false),
159                                                   new Vector3D(rate, Vector3D.PLUS_K), Vector3D.ZERO);
160         Assert.assertEquals(rate, ac.getRotationRate().getNorm(), 1.0e-10);
161         double dt = 10.0;
162         TimeStampedAngularCoordinates shifted = ac.shiftedBy(dt);
163         Assert.assertEquals(rate, shifted.getRotationRate().getNorm(), 1.0e-10);
164         Assert.assertEquals(rate * dt, Rotation.distance(ac.getRotation(), shifted.getRotation()), 1.0e-10);
165 
166         Vector3D shiftedX  = shifted.getRotation().applyInverseTo(Vector3D.PLUS_I);
167         Vector3D shiftedY  = shifted.getRotation().applyInverseTo(Vector3D.PLUS_J);
168         Vector3D shiftedZ  = shifted.getRotation().applyInverseTo(Vector3D.PLUS_K);
169         Vector3D originalX = ac.getRotation().applyInverseTo(Vector3D.PLUS_I);
170         Vector3D originalY = ac.getRotation().applyInverseTo(Vector3D.PLUS_J);
171         Vector3D originalZ = ac.getRotation().applyInverseTo(Vector3D.PLUS_K);
172         Assert.assertEquals( FastMath.cos(rate * dt), Vector3D.dotProduct(shiftedX, originalX), 1.0e-10);
173         Assert.assertEquals( FastMath.sin(rate * dt), Vector3D.dotProduct(shiftedX, originalY), 1.0e-10);
174         Assert.assertEquals( 0.0,                 Vector3D.dotProduct(shiftedX, originalZ), 1.0e-10);
175         Assert.assertEquals(-FastMath.sin(rate * dt), Vector3D.dotProduct(shiftedY, originalX), 1.0e-10);
176         Assert.assertEquals( FastMath.cos(rate * dt), Vector3D.dotProduct(shiftedY, originalY), 1.0e-10);
177         Assert.assertEquals( 0.0,                 Vector3D.dotProduct(shiftedY, originalZ), 1.0e-10);
178         Assert.assertEquals( 0.0,                 Vector3D.dotProduct(shiftedZ, originalX), 1.0e-10);
179         Assert.assertEquals( 0.0,                 Vector3D.dotProduct(shiftedZ, originalY), 1.0e-10);
180         Assert.assertEquals( 1.0,                 Vector3D.dotProduct(shiftedZ, originalZ), 1.0e-10);
181 
182         Vector3D forward = TimeStampedAngularCoordinates.estimateRate(ac.getRotation(), shifted.getRotation(), dt);
183         Assert.assertEquals(0.0, forward.subtract(ac.getRotationRate()).getNorm(), 1.0e-10);
184 
185         Vector3D reversed = TimeStampedAngularCoordinates.estimateRate(shifted.getRotation(), ac.getRotation(), dt);
186         Assert.assertEquals(0.0, reversed.add(ac.getRotationRate()).getNorm(), 1.0e-10);
187 
188     }
189 
190     @Test
191     public void testReverseOffset() {
192         RandomGenerator random = new Well1024a(0x4ecca9d57a8f1611l);
193         for (int i = 0; i < 100; ++i) {
194             Rotation r = randomRotation(random);
195             Vector3D o = randomVector(random, 1.0e-3);
196             Vector3D a = randomVector(random, 1.0e-3);
197             TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, a);
198             TimeStampedAngularCoordinates sum = ac.addOffset(ac.revert());
199             Assert.assertEquals(0.0, sum.getRotation().getAngle(), 1.0e-15);
200             Assert.assertEquals(0.0, sum.getRotationRate().getNorm(), 1.0e-15);
201             Assert.assertEquals(0.0, sum.getRotationAcceleration().getNorm(), 1.0e-15);
202         }
203     }
204 
205     @Test
206     public void testNoCommute() {
207         TimeStampedAngularCoordinates ac1 =
208         new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, new Rotation(0.48,  0.64, 0.36, 0.48, false), Vector3D.ZERO, Vector3D.ZERO);
209         TimeStampedAngularCoordinates ac2 =
210         new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, new Rotation(0.36, -0.48, 0.48, 0.64, false), Vector3D.ZERO, Vector3D.ZERO);
211 
212         TimeStampedAngularCoordinates add12 = ac1.addOffset(ac2);
213         TimeStampedAngularCoordinates add21 = ac2.addOffset(ac1);
214 
215         // the rotations are really different from each other
216         Assert.assertEquals(2.574, Rotation.distance(add12.getRotation(), add21.getRotation()), 1.0e-3);
217 
218     }
219 
220     @Test
221     public void testRoundTripNoOp() {
222         RandomGenerator random = new Well1024a(0x1e610cfe89306669l);
223         for (int i = 0; i < 100; ++i) {
224 
225             Rotation r1 = randomRotation(random);
226             Vector3D o1 = randomVector(random, 1.0e-2);
227             Vector3D a1 = randomVector(random, 1.0e-2);
228             TimeStampedAngularCoordinates ac1 = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r1, o1, a1);
229             Rotation r2 = randomRotation(random);
230             Vector3D o2 = randomVector(random, 1.0e-2);
231             Vector3D a2 = randomVector(random, 1.0e-2);
232 
233             TimeStampedAngularCoordinates ac2 = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r2, o2, a2);
234             TimeStampedAngularCoordinates roundTripSA = ac1.subtractOffset(ac2).addOffset(ac2);
235             Assert.assertEquals(0.0, Rotation.distance(ac1.getRotation(), roundTripSA.getRotation()), 4.0e-16);
236             Assert.assertEquals(0.0, Vector3D.distance(ac1.getRotationRate(), roundTripSA.getRotationRate()), 2.0e-17);
237             Assert.assertEquals(0.0, Vector3D.distance(ac1.getRotationAcceleration(), roundTripSA.getRotationAcceleration()), 1.0e-17);
238 
239             TimeStampedAngularCoordinates roundTripAS = ac1.addOffset(ac2).subtractOffset(ac2);
240             Assert.assertEquals(0.0, Rotation.distance(ac1.getRotation(), roundTripAS.getRotation()), 6.0e-16);
241             Assert.assertEquals(0.0, Vector3D.distance(ac1.getRotationRate(), roundTripAS.getRotationRate()), 2.0e-17);
242             Assert.assertEquals(0.0, Vector3D.distance(ac1.getRotationAcceleration(), roundTripAS.getRotationAcceleration()), 2.0e-17);
243 
244         }
245     }
246 
247     @Test
248     public void testInterpolationAroundPI() {
249 
250         List<TimeStampedAngularCoordinates> sample = new ArrayList<TimeStampedAngularCoordinates>();
251 
252         // add angular coordinates at t0: 179.999 degrees rotation along X axis
253         AbsoluteDate t0 = new AbsoluteDate("2012-01-01T00:00:00.000", TimeScalesFactory.getTAI());
254         TimeStampedAngularCoordinates ac0 = new TimeStampedAngularCoordinates(t0,
255                                                                               new Rotation(Vector3D.PLUS_I,
256                                                                                            FastMath.toRadians(179.999),
257                                                                                            RotationConvention.VECTOR_OPERATOR),
258                                                                               new Vector3D(FastMath.toRadians(0), 0, 0),
259                                                                               Vector3D.ZERO);
260         sample.add(ac0);
261 
262         // add angular coordinates at t1: -179.999 degrees rotation (= 180.001 degrees) along X axis
263         AbsoluteDate t1 = new AbsoluteDate("2012-01-01T00:00:02.000", TimeScalesFactory.getTAI());
264         TimeStampedAngularCoordinates ac1 = new TimeStampedAngularCoordinates(t1,
265                                                                               new Rotation(Vector3D.PLUS_I,
266                                                                                            FastMath.toRadians(-179.999),
267                                                                                            RotationConvention.VECTOR_OPERATOR),
268                                                                               new Vector3D(FastMath.toRadians(0), 0, 0),
269                                                                               Vector3D.ZERO);
270         sample.add(ac1);
271 
272         // get interpolated angular coordinates at mid time between t0 and t1
273         AbsoluteDate t = new AbsoluteDate("2012-01-01T00:00:01.000", TimeScalesFactory.getTAI());
274         TimeStampedAngularCoordinates interpolated =
275                 TimeStampedAngularCoordinates.interpolate(t, AngularDerivativesFilter.USE_R, sample);
276 
277         Assert.assertEquals(FastMath.toRadians(180), interpolated.getRotation().getAngle(), 1.0e-12);
278 
279     }
280 
281     @Test
282     public void testInterpolationWithoutAcceleration() {
283         AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
284         double alpha0 = 0.5   * FastMath.PI;
285         double omega  = 0.05  * FastMath.PI;
286         final TimeStampedAngularCoordinates reference =
287                 new TimeStampedAngularCoordinates(date,
288                                                   new Rotation(Vector3D.PLUS_K, alpha0,
289                                                                RotationConvention.VECTOR_OPERATOR),
290                                                   new Vector3D(omega, Vector3D.MINUS_K),
291                                                   Vector3D.ZERO);
292         double[] errors = interpolationErrors(reference, 1.0);
293         Assert.assertEquals(0.0, errors[0], 1.4e-15);
294         Assert.assertEquals(0.0, errors[1], 3.0e-15);
295         Assert.assertEquals(0.0, errors[2], 3.0e-14);
296     }
297 
298     @Test
299     public void testInterpolationWithAcceleration() {
300         AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
301         double alpha0 = 0.5   * FastMath.PI;
302         double omega  = 0.05  * FastMath.PI;
303         double eta    = 0.005 * FastMath.PI;
304         final TimeStampedAngularCoordinates reference =
305                 new TimeStampedAngularCoordinates(date,
306                                                   new Rotation(Vector3D.PLUS_K, alpha0,
307                                                                RotationConvention.VECTOR_OPERATOR),
308                                                   new Vector3D(omega, Vector3D.MINUS_K),
309                                                   new Vector3D(eta,   Vector3D.PLUS_J));
310         double[] errors = interpolationErrors(reference, 1.0);
311         Assert.assertEquals(0.0, errors[0], 3.0e-5);
312         Assert.assertEquals(0.0, errors[1], 2.0e-4);
313         Assert.assertEquals(0.0, errors[2], 4.6e-3);
314     }
315 
316     private double[] interpolationErrors(final TimeStampedAngularCoordinates reference, double dt)
317         {
318 
319         final OrdinaryDifferentialEquation ode = new OrdinaryDifferentialEquation() {
320             public int getDimension() {
321                 return 4;
322             }
323             public double[] computeDerivatives(final double t, final double[] q) {
324                 final double omegaX = reference.getRotationRate().getX() + t * reference.getRotationAcceleration().getX();
325                 final double omegaY = reference.getRotationRate().getY() + t * reference.getRotationAcceleration().getY();
326                 final double omegaZ = reference.getRotationRate().getZ() + t * reference.getRotationAcceleration().getZ();
327                 return new double[] {
328                     0.5 * MathArrays.linearCombination(-q[1], omegaX, -q[2], omegaY, -q[3], omegaZ),
329                     0.5 * MathArrays.linearCombination( q[0], omegaX, -q[3], omegaY,  q[2], omegaZ),
330                     0.5 * MathArrays.linearCombination( q[3], omegaX,  q[0], omegaY, -q[1], omegaZ),
331                     0.5 * MathArrays.linearCombination(-q[2], omegaX,  q[1], omegaY,  q[0], omegaZ)
332                 };
333             }
334         };
335         final List<TimeStampedAngularCoordinates> complete = new ArrayList<TimeStampedAngularCoordinates>();
336         ODEIntegrator integrator = new DormandPrince853Integrator(1.0e-6, 1.0, 1.0e-12, 1.0e-12);
337         integrator.addStepHandler(new StepNormalizer(dt / 2000, new ODEFixedStepHandler() {
338             public void handleStep(ODEStateAndDerivative state, boolean isLast) {
339                 final double   t = state.getTime();
340                 final double[] q = state.getPrimaryState();
341                 complete.add(new TimeStampedAngularCoordinates(reference.getDate().shiftedBy(t),
342                                                                new Rotation(q[0], q[1], q[2], q[3], true),
343                                                                new Vector3D(1, reference.getRotationRate(),
344                                                                             t, reference.getRotationAcceleration()),
345                                                                reference.getRotationAcceleration()));
346             }
347         }));
348 
349         double[] y = new double[] {
350             reference.getRotation().getQ0(),
351             reference.getRotation().getQ1(),
352             reference.getRotation().getQ2(),
353             reference.getRotation().getQ3()
354         };
355         integrator.integrate(ode, new ODEState(0, y), dt);
356 
357         List<TimeStampedAngularCoordinates> sample = new ArrayList<TimeStampedAngularCoordinates>();
358         sample.add(complete.get(0));
359         sample.add(complete.get(complete.size() / 2));
360         sample.add(complete.get(complete.size() - 1));
361 
362         double maxRotationError     = 0;
363         double maxRateError         = 0;
364         double maxAccelerationError = 0;
365         for (TimeStampedAngularCoordinates acRef : complete) {
366             TimeStampedAngularCoordinates interpolated =
367                     TimeStampedAngularCoordinates.interpolate(acRef.getDate(), AngularDerivativesFilter.USE_RRA, sample);
368             double rotationError     = Rotation.distance(acRef.getRotation(), interpolated.getRotation());
369             double rateError         = Vector3D.distance(acRef.getRotationRate(), interpolated.getRotationRate());
370             double accelerationError = Vector3D.distance(acRef.getRotationAcceleration(), interpolated.getRotationAcceleration());
371             maxRotationError         = FastMath.max(maxRotationError,     rotationError);
372             maxRateError             = FastMath.max(maxRateError,         rateError);
373             maxAccelerationError     = FastMath.max(maxAccelerationError, accelerationError);
374         }
375 
376         return new double[] {
377           maxRotationError, maxRateError, maxAccelerationError
378         };
379 
380     }
381 
382     @Test
383     public void testInterpolationNeedOffsetWrongRate() {
384         AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
385         double omega  = 2.0 * FastMath.PI;
386         TimeStampedAngularCoordinates reference =
387                 new TimeStampedAngularCoordinates(date,
388                                                   Rotation.IDENTITY,
389                                                   new Vector3D(omega, Vector3D.MINUS_K),
390                                                   Vector3D.ZERO);
391 
392         List<TimeStampedAngularCoordinates> sample = new ArrayList<TimeStampedAngularCoordinates>();
393         for (double dt : new double[] { 0.0, 0.25, 0.5, 0.75, 1.0 }) {
394             TimeStampedAngularCoordinates shifted = reference.shiftedBy(dt);
395             sample.add(new TimeStampedAngularCoordinates(shifted.getDate(),
396                                                          shifted.getRotation(),
397                                                          Vector3D.ZERO, Vector3D.ZERO));
398         }
399 
400         for (TimeStampedAngularCoordinates s : sample) {
401             TimeStampedAngularCoordinates interpolated =
402                     TimeStampedAngularCoordinates.interpolate(s.getDate(), AngularDerivativesFilter.USE_RR, sample);
403             Rotation r    = interpolated.getRotation();
404             Vector3D rate = interpolated.getRotationRate();
405             Assert.assertEquals(0.0, Rotation.distance(s.getRotation(), r), 2.0e-14);
406             Assert.assertEquals(0.0, Vector3D.distance(s.getRotationRate(), rate), 2.0e-13);
407         }
408 
409     }
410 
411     @Test
412     public void testInterpolationRotationOnly() {
413         AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
414         double alpha0 = 0.5 * FastMath.PI;
415         double omega  = 0.5 * FastMath.PI;
416         TimeStampedAngularCoordinates reference =
417                 new TimeStampedAngularCoordinates(date,
418                                                   new Rotation(Vector3D.PLUS_K, alpha0,
419                                                                RotationConvention.VECTOR_OPERATOR),
420                                                   new Vector3D(omega, Vector3D.MINUS_K),
421                                                   Vector3D.ZERO);
422 
423         List<TimeStampedAngularCoordinates> sample = new ArrayList<TimeStampedAngularCoordinates>();
424         for (double dt : new double[] { 0.0, 0.2, 0.4, 0.6, 0.8, 1.0 }) {
425             Rotation r = reference.shiftedBy(dt).getRotation();
426             sample.add(new TimeStampedAngularCoordinates(date.shiftedBy(dt), r, Vector3D.ZERO, Vector3D.ZERO));
427         }
428 
429         for (double dt = 0; dt < 1.0; dt += 0.001) {
430             TimeStampedAngularCoordinates interpolated =
431                     TimeStampedAngularCoordinates.interpolate(date.shiftedBy(dt), AngularDerivativesFilter.USE_R, sample);
432             Rotation r    = interpolated.getRotation();
433             Vector3D rate = interpolated.getRotationRate();
434             Assert.assertEquals(0.0, Rotation.distance(reference.shiftedBy(dt).getRotation(), r), 3.0e-4);
435             Assert.assertEquals(0.0, Vector3D.distance(reference.shiftedBy(dt).getRotationRate(), rate), 1.0e-2);
436         }
437 
438     }
439 
440     @Test
441     public void testInterpolationTooSmallSample() {
442         AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
443         double alpha0 = 0.5 * FastMath.PI;
444         double omega  = 0.5 * FastMath.PI;
445         TimeStampedAngularCoordinates reference =
446                 new TimeStampedAngularCoordinates(date,
447                                                   new Rotation(Vector3D.PLUS_K, alpha0,
448                                                                RotationConvention.VECTOR_OPERATOR),
449                                                   new Vector3D(omega, Vector3D.MINUS_K),
450                                                   Vector3D.ZERO);
451 
452         List<TimeStampedAngularCoordinates> sample = new ArrayList<TimeStampedAngularCoordinates>();
453         Rotation r = reference.shiftedBy(0.2).getRotation();
454         sample.add(new TimeStampedAngularCoordinates(date.shiftedBy(0.2), r, Vector3D.ZERO, Vector3D.ZERO));
455 
456         try {
457             TimeStampedAngularCoordinates.interpolate(date.shiftedBy(0.3), AngularDerivativesFilter.USE_R, sample);
458             Assert.fail("an exception should have been thrown");
459         } catch (OrekitException oe) {
460             Assert.assertEquals(OrekitMessages.NOT_ENOUGH_DATA_FOR_INTERPOLATION, oe.getSpecifier());
461             Assert.assertEquals(1, ((Integer) oe.getParts()[0]).intValue());
462         }
463 
464     }
465 
466     @Test
467     public void testInterpolationGTODIssue() {
468         AbsoluteDate t0 = new AbsoluteDate("2004-04-06T19:59:28.000", TimeScalesFactory.getTAI());
469         double[][] params = new double[][] {
470             { 0.0, -0.3802356750911964, -0.9248896320037013, 7.292115030462892e-5 },
471             { 4.0,  0.1345716955788532, -0.990903859488413,  7.292115033301528e-5 },
472             { 8.0, -0.613127541102373,   0.7899839354960061, 7.292115037371062e-5 }
473         };
474         List<TimeStampedAngularCoordinates> sample = new ArrayList<TimeStampedAngularCoordinates>();
475         for (double[] row : params) {
476             AbsoluteDate t = t0.shiftedBy(row[0] * 3600.0);
477             Rotation     r = new Rotation(row[1], 0.0, 0.0, row[2], false);
478             Vector3D     o = new Vector3D(row[3], Vector3D.PLUS_K);
479             sample.add(new TimeStampedAngularCoordinates(t, r, o, Vector3D.ZERO));
480         }
481         for (double dt = 0; dt < 29000; dt += 120) {
482             TimeStampedAngularCoordinates shifted      = sample.get(0).shiftedBy(dt);
483             TimeStampedAngularCoordinates interpolated =
484                     TimeStampedAngularCoordinates.interpolate(t0.shiftedBy(dt), AngularDerivativesFilter.USE_RR, sample);
485             Assert.assertEquals(0.0,
486                                 Rotation.distance(shifted.getRotation(), interpolated.getRotation()),
487                                 1.3e-7);
488             Assert.assertEquals(0.0,
489                                 Vector3D.distance(shifted.getRotationRate(), interpolated.getRotationRate()),
490                                 1.0e-11);
491         }
492 
493     }
494 
495     private Vector3D randomVector(RandomGenerator random, double norm) {
496         double n = random.nextDouble() * norm;
497         double x = 2 * random.nextDouble() - 1;
498         double y = 2 * random.nextDouble() - 1;
499         double z = 2 * random.nextDouble() - 1;
500         return new Vector3D(n, new Vector3D(x, y, z).normalize());
501     }
502 
503     private PVCoordinates randomPVCoordinates(RandomGenerator random,
504                                               double norm0, double norm1, double norm2) {
505         Vector3D p0 = randomVector(random, norm0);
506         Vector3D p1 = randomVector(random, norm1);
507         Vector3D p2 = randomVector(random, norm2);
508         return new PVCoordinates(p0, p1, p2);
509     }
510 
511     private Rotation randomRotation(RandomGenerator random) {
512         double q0 = random.nextDouble() * 2 - 1;
513         double q1 = random.nextDouble() * 2 - 1;
514         double q2 = random.nextDouble() * 2 - 1;
515         double q3 = random.nextDouble() * 2 - 1;
516         double q  = FastMath.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
517         return new Rotation(q0 / q, q1 / q, q2 / q, q3 / q, false);
518     }
519 
520 }
521