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.frames;
18  
19  
20  import java.io.ByteArrayInputStream;
21  import java.io.ByteArrayOutputStream;
22  import java.io.FileNotFoundException;
23  import java.io.IOException;
24  import java.io.ObjectInputStream;
25  import java.io.ObjectOutputStream;
26  
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.util.FastMath;
31  import org.junit.Assert;
32  import org.junit.Before;
33  import org.junit.Test;
34  import org.orekit.Utils;
35  import org.orekit.data.DataContext;
36  import org.orekit.time.AbsoluteDate;
37  import org.orekit.time.DateComponents;
38  import org.orekit.time.TimeComponents;
39  import org.orekit.time.TimeScalesFactory;
40  import org.orekit.utils.AngularDerivativesFilter;
41  import org.orekit.utils.CartesianDerivativesFilter;
42  import org.orekit.utils.Constants;
43  import org.orekit.utils.IERSConventions;
44  import org.orekit.utils.OrekitConfiguration;
45  import org.orekit.utils.PVCoordinates;
46  
47  
48  public class TODProviderTest {
49  
50      @Test
51      public void testRotationRate() {
52          TransformProvider provider =
53                  new InterpolatingTransformProvider(new TODProvider(IERSConventions.IERS_1996, null, DataContext.getDefault().getTimeScales()),
54                                                     CartesianDerivativesFilter.USE_PVA,
55                                                     AngularDerivativesFilter.USE_R,
56                                                     3, 1.0, 5, Constants.JULIAN_DAY, 100.0);
57          AbsoluteDate tMin = new AbsoluteDate(2035, 3, 2, 15, 58, 59, TimeScalesFactory.getUTC());
58          double minRate = provider.getTransform(tMin).getRotationRate().getNorm();
59          Assert.assertEquals(6.4e-14, minRate, 1.0e-15);
60          AbsoluteDate tMax = new AbsoluteDate(2043, 12, 16, 14, 18, 9, TimeScalesFactory.getUTC());
61          double maxRate = provider.getTransform(tMax).getRotationRate().getNorm();
62          Assert.assertEquals(1.4e-11, maxRate, 1.0e-12);
63      }
64  
65      @Test
66      public void testAASReferenceLEO() {
67  
68          // this reference test has been extracted from the following paper:
69          // Implementation Issues Surrounding the New IAU Reference Systems for Astrodynamics
70          // David A. Vallado, John H. Seago, P. Kenneth Seidelmann
71          // http://www.centerforspace.com/downloads/files/pubs/AAS-06-134.pdf
72          Utils.setLoaders(IERSConventions.IERS_1996,
73                           Utils.buildEOPList(IERSConventions.IERS_1996, ITRFVersion.ITRF_2008, new double[][] {
74                               { 53098, -0.4399619, 0.0015563, -0.140682, 0.333309, -0.052195, -0.003875, Double.NaN, Double.NaN },
75                               { 53099, -0.4399619, 0.0015563, -0.140682, 0.333309, -0.052195, -0.003875, Double.NaN, Double.NaN },
76                               { 53100, -0.4399619, 0.0015563, -0.140682, 0.333309, -0.052195, -0.003875, Double.NaN, Double.NaN },
77                               { 53101, -0.4399619, 0.0015563, -0.140682, 0.333309, -0.052195, -0.003875, Double.NaN, Double.NaN },
78                               { 53102, -0.4399619, 0.0015563, -0.140682, 0.333309, -0.052195, -0.003875, Double.NaN, Double.NaN },
79                               { 53103, -0.4399619, 0.0015563, -0.140682, 0.333309, -0.052195, -0.003875, Double.NaN, Double.NaN },
80                               { 53104, -0.4399619, 0.0015563, -0.140682, 0.333309, -0.052195, -0.003875, Double.NaN, Double.NaN },
81                               { 53105, -0.4399619, 0.0015563, -0.140682, 0.333309, -0.052195, -0.003875, Double.NaN, Double.NaN }
82                           }));
83          AbsoluteDate t0 = new AbsoluteDate(new DateComponents(2004, 04, 06),
84                                             new TimeComponents(07, 51, 28.386009),
85                                             TimeScalesFactory.getUTC());
86  
87          Transform tt = FramesFactory.getMOD(IERSConventions.IERS_1996).
88                  getTransformTo(FramesFactory.getTOD(IERSConventions.IERS_1996, true), t0);
89          Transform ff = FramesFactory.getMOD(false).getTransformTo(FramesFactory.getTOD(false), t0);
90  
91          //TOD iau76
92          PVCoordinates pvTODiau76 =
93              new PVCoordinates(new Vector3D(5094514.7804, 6127366.4612, 6380344.5328),
94                                new Vector3D(-4746.088567, 786.077222, 5531.931288));
95          //MOD iau76
96          PVCoordinates pvMODiau76WithoutNutCorr =
97              new PVCoordinates(new Vector3D(5094029.0167, 6127870.9363, 6380247.8885),
98                                new Vector3D(-4746.262495, 786.014149, 5531.791025));
99          //MOD iau76
100         PVCoordinates pvMODiau76 =
101             new PVCoordinates(new Vector3D(5094028.3745, 6127870.8164, 6380248.5164),
102                               new Vector3D(-4746.263052, 786.014045, 5531.790562));
103 
104         // it seems the induced effect of pole nutation correction δΔψ on the equation of the equinoxes
105         // was not taken into account in the reference paper, so we fix it here for the test
106         final double dDeltaPsi =
107                 FramesFactory.getEOPHistory(IERSConventions.IERS_1996, true).getEquinoxNutationCorrection(t0)[0];
108         final double epsilonA = IERSConventions.IERS_1996.getMeanObliquityFunction().value(t0);
109         final Transform fix =
110                 new Transform(t0, new Rotation(Vector3D.PLUS_K,
111                                                dDeltaPsi * FastMath.cos(epsilonA),
112                                                RotationConvention.FRAME_TRANSFORM));
113 
114         checkPV(pvTODiau76, fix.transformPVCoordinates(tt.transformPVCoordinates(pvMODiau76)), 1.13e-3, 5.3e-5);
115         checkPV(pvTODiau76, ff.transformPVCoordinates(pvMODiau76WithoutNutCorr), 1.07e-3, 5.3e-5);
116 
117     }
118 
119     @Test
120     public void testAASReferenceGEO() {
121 
122         // this reference test has been extracted from the following paper:
123         // Implementation Issues Surrounding the New IAU Reference Systems for Astrodynamics
124         // David A. Vallado, John H. Seago, P. Kenneth Seidelmann
125         // http://www.centerforspace.com/downloads/files/pubs/AAS-06-134.pdf
126         Utils.setLoaders(IERSConventions.IERS_1996,
127                          Utils.buildEOPList(IERSConventions.IERS_1996, ITRFVersion.ITRF_2008, new double[][] {
128                              { 53153, -0.4709050,  0.0000000, -0.083853,  0.467217, -0.053614, -0.004494, Double.NaN, Double.NaN },
129                              { 53154, -0.4709050,  0.0000000, -0.083853,  0.467217, -0.053614, -0.004494, Double.NaN, Double.NaN },
130                              { 53155, -0.4709050,  0.0000000, -0.083853,  0.467217, -0.053614, -0.004494, Double.NaN, Double.NaN },
131                              { 53156, -0.4709050,  0.0000000, -0.083853,  0.467217, -0.053614, -0.004494, Double.NaN, Double.NaN },
132                              { 53157, -0.4709050,  0.0000000, -0.083853,  0.467217, -0.053614, -0.004494, Double.NaN, Double.NaN },
133                              { 53158, -0.4709050,  0.0000000, -0.083853,  0.467217, -0.053614, -0.004494, Double.NaN, Double.NaN },
134                              { 53159, -0.4709050,  0.0000000, -0.083853,  0.467217, -0.053614, -0.004494, Double.NaN, Double.NaN },
135                              { 53160, -0.4709050,  0.0000000, -0.083853,  0.467217, -0.053614, -0.004494, Double.NaN, Double.NaN }
136                          }));
137         AbsoluteDate t0 = new AbsoluteDate(new DateComponents(2004, 06, 01),
138                                            TimeComponents.H00,
139                                            TimeScalesFactory.getUTC());
140 
141         Transform tt = FramesFactory.getMOD(IERSConventions.IERS_1996).
142                 getTransformTo(FramesFactory.getTOD(IERSConventions.IERS_1996, true), t0);
143         Transform ff = FramesFactory.getMOD(false).getTransformTo(FramesFactory.getTOD(false), t0);
144 
145         // TOD iau76
146         PVCoordinates pvTODiau76 =
147             new PVCoordinates(new Vector3D(-40577427.7501, -11500096.1306, 10293.2583),
148                               new Vector3D(837.552338, -2957.524176, -0.928772));
149         // MOD iau76
150         PVCoordinates pvMODiau76WithoutNutCorr =
151             new PVCoordinates(new Vector3D(-40576822.6385, -11502231.5013, 9738.2304),
152                               new Vector3D(837.708020, -2957.480118, -0.814275));
153 
154         // MOD iau76
155         PVCoordinates pvMODiau76 =
156             new PVCoordinates(new Vector3D(-40576822.6395, -11502231.5015, 9733.7842),
157                               new Vector3D(837.708020, -2957.480117, -0.814253));
158 
159 
160         // it seems the induced effect of pole nutation correction δΔψ on the equation of the equinoxes
161         // was not taken into account in the reference paper, so we fix it here for the test
162         final double dDeltaPsi =
163                 FramesFactory.getEOPHistory(IERSConventions.IERS_1996, true).getEquinoxNutationCorrection(t0)[0];
164         final double epsilonA = IERSConventions.IERS_1996.getMeanObliquityFunction().value(t0);
165         final Transform fix =
166                 new Transform(t0, new Rotation(Vector3D.PLUS_K,
167                                                dDeltaPsi * FastMath.cos(epsilonA),
168                                                RotationConvention.FRAME_TRANSFORM));
169 
170         checkPV(pvTODiau76, fix.transformPVCoordinates(tt.transformPVCoordinates(pvMODiau76)), 4.86e-4, 6.2e-5);
171         checkPV(pvTODiau76, ff.transformPVCoordinates(pvMODiau76WithoutNutCorr), 4.87e-4, 6.31e-5);
172 
173     }
174 
175     @Test
176     public void testInterpolationAccuracyWithEOP() throws FileNotFoundException {
177 
178         // max interpolation error observed on a one month period with 60 seconds step
179         //
180         // number of sample points    time between sample points    max error
181         //        6                          86400s /  8 =  3h       19.56e-12 rad
182         //        6                          86400s / 12 =  2h       13.02e-12 rad
183         //        6                          86400s / 16 =  1h30      9.75e-12 rad
184         //        6                          86400s / 20 =  1h12      7.79e-12 rad
185         //        6                          86400s / 24 =  1h        6.48e-12 rad
186         //        8                          86400s /  8 =  3h       20.91e-12 rad
187         //        8                          86400s / 12 =  2h       13.91e-12 rad
188         //        8                          86400s / 16 =  1h30     10.42e-12 rad
189         //        8                          86400s / 20 =  1h12      8.32e-12 rad
190         //        8                          86400s / 24 =  1h        6.92e-12 rad
191         //       10                          86400s /  8 =  3h       21.65e-12 rad
192         //       10                          86400s / 12 =  2h       14.41e-12 rad
193         //       10                          86400s / 16 =  1h30     10.78e-12 rad
194         //       10                          86400s / 20 =  1h12      8.61e-12 rad
195         //       10                          86400s / 24 =  1h        7.16e-12 rad
196         //       12                          86400s /  8 =  3h       22.12e-12 rad
197         //       12                          86400s / 12 =  2h       14.72e-12 rad
198         //       12                          86400s / 16 =  1h30     11.02e-12 rad
199         //       12                          86400s / 20 =  1h12      8.80e-12 rad
200         //       12                          86400s / 24 =  1h        7.32e-12 rad
201         //
202         // looking at error behavior during along the sample show the max error is
203         // a peak at 00h00 each day for all curves, which matches the EOP samples
204         // points used for correction (eopHistoru is set to non null at construction here).
205         // So looking only at max error does not allow to select an interpolation
206         // setting as they all fall in a similar 6e-12 to 8e-12 range. Looking at
207         // the error behavior between these peaks however shows that there is still
208         // some signal if the time interval is between sample points is too large,
209         // in order to get only numerical noise, we have to go as far as 1h between
210         // the points.
211         // We finally select 6 interpolation points separated by 1 hour each
212         EOPHistory eopHistory = FramesFactory.getEOPHistory(IERSConventions.IERS_1996, false);
213         TransformProvider nonInterpolating = new TODProvider(IERSConventions.IERS_1996, eopHistory, DataContext.getDefault().getTimeScales());
214         final TransformProvider interpolating =
215                 new InterpolatingTransformProvider(nonInterpolating,
216                                                    CartesianDerivativesFilter.USE_PVA,
217                                                    AngularDerivativesFilter.USE_R,
218                                                    6, Constants.JULIAN_DAY / 24,
219                                                    OrekitConfiguration.getCacheSlotsNumber(),
220                                                    Constants.JULIAN_YEAR, 30 * Constants.JULIAN_DAY);
221 
222         // the following time range is located around the maximal observed error
223         AbsoluteDate start = new AbsoluteDate(2002, 11, 11, 0, 0, 0.0, TimeScalesFactory.getTAI());
224         AbsoluteDate end   = new AbsoluteDate(2002, 11, 15, 6, 0, 0.0, TimeScalesFactory.getTAI());
225         double maxError = 0.0;
226         for (AbsoluteDate date = start; date.compareTo(end) < 0; date = date.shiftedBy(60)) {
227             final Transform transform =
228                     new Transform(date,
229                                   interpolating.getTransform(date),
230                                   nonInterpolating.getTransform(date).getInverse());
231             final double error = transform.getRotation().getAngle();
232             maxError = FastMath.max(maxError, error);
233         }
234 
235         Assert.assertTrue(maxError < 7e-12);
236 
237     }
238 
239     @Test
240     public void testInterpolationAccuracyWithoutEOP() throws FileNotFoundException {
241 
242         // max interpolation error observed on a one month period with 60 seconds step
243         //
244         // number of sample points    time between sample points    max error
245         //        5                          86400s /  3 =  8h     3286.90e-15 rad
246         //        5                          86400s /  6 =  4h      103.90e-15 rad
247         //        5                          86400s /  8 =  3h       24.74e-15 rad
248         //        5                          86400s / 12 =  2h        4.00e-15 rad
249         //        6                          86400s /  3 =  8h      328.91e-15 rad
250         //        6                          86400s /  6 =  4h        5.92e-15 rad
251         //        6                          86400s /  8 =  3h        3.95e-15 rad
252         //        6                          86400s / 12 =  2h        3.94e-15 rad
253         //        8                          86400s /  3 =  8h        5.87e-15 rad
254         //        8                          86400s /  6 =  4h        4.73e-15 rad
255         //        8                          86400s /  8 =  3h        4.45e-15 rad
256         //        8                          86400s / 12 =  2h        3.87e-15 rad
257         //       10                          86400s /  3 =  8h        5.29e-15 rad
258         //       10                          86400s /  6 =  4h        5.36e-15 rad
259         //       10                          86400s /  8 =  3h        5.86e-15 rad
260         //       10                          86400s / 12 =  2h        5.76e-15 rad
261         //
262         //
263         // We don't see anymore the peak at 00h00 so this confirms it is related to EOP
264         // sampling. All values between 3e-15 and 6e-15 are really equivalent: it is
265         // mostly numerical noise. The best settings are 6 or 8 points every 2 or 3 hours.
266         // We finally select 6 interpolation points separated by 3 hours each
267         TransformProvider nonInterpolating = new TODProvider(IERSConventions.IERS_1996, null, DataContext.getDefault().getTimeScales());
268                 final TransformProvider interpolating =
269                         new InterpolatingTransformProvider(nonInterpolating,
270                                                            CartesianDerivativesFilter.USE_PVA,
271                                                            AngularDerivativesFilter.USE_R,
272                                                            6, Constants.JULIAN_DAY / 8,
273                                                            OrekitConfiguration.getCacheSlotsNumber(),
274                                                            Constants.JULIAN_YEAR, 30 * Constants.JULIAN_DAY);
275 
276                 // the following time range is located around the maximal observed error
277                 AbsoluteDate start = new AbsoluteDate(2002, 11, 11, 0, 0, 0.0, TimeScalesFactory.getTAI());
278                 AbsoluteDate end   = new AbsoluteDate(2002, 11, 15, 6, 0, 0.0, TimeScalesFactory.getTAI());
279                 double maxError = 0.0;
280                 for (AbsoluteDate date = start; date.compareTo(end) < 0; date = date.shiftedBy(60)) {
281                     final Transform transform =
282                             new Transform(date,
283                                           interpolating.getTransform(date),
284                                           nonInterpolating.getTransform(date).getInverse());
285                     final double error = transform.getRotation().getAngle();
286                     maxError = FastMath.max(maxError, error);
287                 }
288 
289                 Assert.assertTrue(maxError < 4.0e-15);
290 
291     }
292 
293     @Test
294     public void testSofaPnm80() {
295 
296         // the reference value has been computed using the March 2012 version of the SOFA library
297         // http://www.iausofa.org/2012_0301_C.html, with the following code
298         //
299         //        double utc1, utc2, tai1, tai2, tt1, tt2, rmatpn[3][3];
300         //
301         //        // 2004-02-14:00:00:00Z, MJD = 53049, UT1-UTC = -0.4093509
302         //        utc1  = DJM0 + 53049.0;
303         //        utc2  = 0.0;
304         //        iauUtctai(utc1, utc2, &tai1, &tai2);
305         //        iauTaitt(tai1, tai2, &tt1, &tt2);
306         //
307         //        iauPnm80(tt1, tt2, rmatpn);
308         //
309         //        printf("iauPnm80(%.20g, %.20g, rmatpn)\n"
310         //               "  --> %.20g %.20g %.20g\n"
311         //               "      %.20g %.20g %.20g\n"
312         //               "      %.20g %.20g %.20g\n",
313         //               tt1, tt2,
314         //               rmatpn[0][0], rmatpn[0][1], rmatpn[0][2],
315         //               rmatpn[1][0], rmatpn[1][1], rmatpn[1][2],
316         //               rmatpn[2][0], rmatpn[2][1], rmatpn[2][2]);
317         //
318         // the output of this test reads:
319         //        iauNutm80(2453049.5, 0.00074287037037037029902, nut)
320         //         --> 0.99999999859236310407 4.8681019508684473249e-05 2.1105264333587349032e-05
321         //            -4.8680343021901595118e-05 0.99999999830143670998 -3.205231683600651138e-05
322         //            -2.1106824637199909505e-05 3.2051289379386727063e-05 0.99999999926360838565
323         //        iauPnm80(2453049.5, 0.00074287037037037029902, rmatpn)
324         //         --> 0.99999954755358466674 -0.00087243169070689370777 -0.00037915111913272635073
325         //            0.0008724195377896877112 0.99999961892302935418 -3.2217171614061089913e-05
326         //            0.00037917908192846747854 3.1886378193416632805e-05 0.99999992760323874741
327 
328         // As the iauNutm80 and iauPnm80 do not allow user to specify EOP corrections,
329         // the test is done with Predefined.TOD_WITHOUT_EOP_CORRECTIONS.
330 
331         AbsoluteDate date = new AbsoluteDate(2004, 2, 14, TimeScalesFactory.getUTC());
332         Frame tod  = FramesFactory.getFrame(Predefined.TOD_WITHOUT_EOP_CORRECTIONS);
333         checkRotation(new double[][] {
334             { 0.99999999859236310407, 4.8681019508684473249e-05, 2.1105264333587349032e-05 },
335             { -4.8680343021901595118e-05, 0.99999999830143670998, -3.205231683600651138e-05 },
336             { -2.1106824637199909505e-05, 3.2051289379386727063e-05, 0.99999999926360838565    }
337 
338         }, tod.getParent().getTransformTo(tod, date), 5.0e-11);
339         checkRotation(new double[][] {
340             { 0.99999954755358466674,   -0.00087243169070689370777, -0.00037915111913272635073 },
341             { 0.0008724195377896877112,  0.99999961892302935418,    -3.2217171614061089913e-05 },
342             { 0.00037917908192846747854, 3.1886378193416632805e-05,  0.99999992760323874741    }
343 
344         }, tod.getParent().getParent().getTransformTo(tod, date), 5.0e-11);
345 
346     }
347 
348     @Test
349     public void testTOD1976vs2006() {
350 
351         final Frame tod1976 = FramesFactory.getTOD(IERSConventions.IERS_1996, true);
352         final Frame tod2006 = FramesFactory.getTOD(IERSConventions.IERS_2010, true);
353         for (double dt = 0; dt < 2 * Constants.JULIAN_YEAR; dt += 100 * Constants.JULIAN_DAY) {
354             AbsoluteDate date = new AbsoluteDate(AbsoluteDate.J2000_EPOCH, dt);
355             double delta = tod1976.getTransformTo(tod2006, date).getRotation().getAngle();
356             // TOD2006 and TOD2000 are similar to about 65 milli-arcseconds
357             // between 2000 and 2002, with EOP corrections taken into account in both cases
358             Assert.assertEquals(0.0, delta, 3.2e-7);
359         }
360 
361     }
362 
363     @Test
364     public void testTOD2000vs2006() {
365 
366         final Frame tod2000 = FramesFactory.getTOD(IERSConventions.IERS_2003, true);
367         final Frame tod2006 = FramesFactory.getTOD(IERSConventions.IERS_2010, true);
368         for (double dt = 0; dt < 2 * Constants.JULIAN_YEAR; dt += 100 * Constants.JULIAN_DAY) {
369             AbsoluteDate date = new AbsoluteDate(AbsoluteDate.J2000_EPOCH, dt);
370             double delta = tod2000.getTransformTo(tod2006, date).getRotation().getAngle();
371             // TOD2006 and TOD2000 are similar to about 30 micro-arcseconds
372             // between 2000 and 2002, with EOP corrections taken into account in both cases
373             Assert.assertEquals(0.0, delta, 1.5e-10);
374         }
375 
376     }
377 
378     @Test
379     public void testSerialization() throws IOException, ClassNotFoundException {
380         TODProvider provider = new TODProvider(IERSConventions.IERS_2010,
381                                                FramesFactory.getEOPHistory(IERSConventions.IERS_2010, true),
382                                                DataContext.getDefault().getTimeScales());
383 
384         ByteArrayOutputStream bos = new ByteArrayOutputStream();
385         ObjectOutputStream    oos = new ObjectOutputStream(bos);
386         oos.writeObject(provider);
387 
388         Assert.assertTrue(bos.size() > 295000);
389         Assert.assertTrue(bos.size() < 300000);
390 
391         ByteArrayInputStream  bis = new ByteArrayInputStream(bos.toByteArray());
392         ObjectInputStream     ois = new ObjectInputStream(bis);
393         TODProvider deserialized  = (TODProvider) ois.readObject();
394         for (int i = 0; i < FastMath.min(100, provider.getEOPHistory().getEntries().size()); ++i) {
395             AbsoluteDate date = provider.getEOPHistory().getEntries().get(i).getDate();
396             Transform expectedIdentity = new Transform(date,
397                                                        provider.getTransform(date).getInverse(),
398                                                        deserialized.getTransform(date));
399             Assert.assertEquals(0.0, expectedIdentity.getTranslation().getNorm(), 1.0e-15);
400             Assert.assertEquals(0.0, expectedIdentity.getRotation().getAngle(),   1.0e-15);
401         }
402 
403     }
404 
405     @Before
406     public void setUp() {
407         Utils.setDataRoot("compressed-data");
408     }
409 
410     private void checkPV(PVCoordinates reference, PVCoordinates result,
411                          double expectedPositionError, double expectedVelocityError) {
412 
413         Vector3D dP = result.getPosition().subtract(reference.getPosition());
414         Vector3D dV = result.getVelocity().subtract(reference.getVelocity());
415         Assert.assertEquals(expectedPositionError, dP.getNorm(), 0.01 * expectedPositionError);
416         Assert.assertEquals(expectedVelocityError, dV.getNorm(), 0.01 * expectedVelocityError);
417     }
418 
419     private void checkRotation(double[][] reference, Transform t, double epsilon) {
420         double[][] mat = t.getRotation().getMatrix();
421         for (int i = 0; i < 3; ++i) {
422             for (int j = 0; j < 3; ++j) {
423                 Assert.assertEquals(reference[i][j], mat[i][j], epsilon);
424             }
425         }
426     }
427 
428 }