1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18 package org.orekit.utils;
19
20 import org.hamcrest.MatcherAssert;
21 import org.hipparchus.geometry.euclidean.threed.Rotation;
22 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
23 import org.hipparchus.geometry.euclidean.threed.Vector3D;
24 import org.hipparchus.ode.ODEIntegrator;
25 import org.hipparchus.ode.ODEState;
26 import org.hipparchus.ode.ODEStateAndDerivative;
27 import org.hipparchus.ode.OrdinaryDifferentialEquation;
28 import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
29 import org.hipparchus.ode.sampling.ODEFixedStepHandler;
30 import org.hipparchus.ode.sampling.StepNormalizer;
31 import org.hipparchus.util.FastMath;
32 import org.hipparchus.util.MathArrays;
33 import org.junit.jupiter.api.Assertions;
34 import org.junit.jupiter.api.BeforeEach;
35 import org.junit.jupiter.api.DisplayName;
36 import org.junit.jupiter.api.Test;
37 import org.orekit.OrekitMatchers;
38 import org.orekit.Utils;
39 import org.orekit.time.AbsoluteDate;
40 import org.orekit.time.AbstractTimeInterpolator;
41 import org.orekit.time.TimeInterpolator;
42 import org.orekit.time.TimeScalesFactory;
43
44 import java.util.ArrayList;
45 import java.util.List;
46
47 class TimeStampedAngularCoordinatesHermiteInterpolatorTest {
48
49 @Test
50 public void testInterpolationAroundPI() {
51
52 List<TimeStampedAngularCoordinates> sample = new ArrayList<TimeStampedAngularCoordinates>();
53
54
55 AbsoluteDate t0 = new AbsoluteDate("2012-01-01T00:00:00.000", TimeScalesFactory.getTAI());
56 TimeStampedAngularCoordinates ac0 = new TimeStampedAngularCoordinates(t0,
57 new Rotation(Vector3D.PLUS_I,
58 FastMath.toRadians(179.999),
59 RotationConvention.VECTOR_OPERATOR),
60 new Vector3D(FastMath.toRadians(0), 0, 0),
61 Vector3D.ZERO);
62 sample.add(ac0);
63
64
65 AbsoluteDate t1 = new AbsoluteDate("2012-01-01T00:00:02.000", TimeScalesFactory.getTAI());
66 TimeStampedAngularCoordinates ac1 = new TimeStampedAngularCoordinates(t1,
67 new Rotation(Vector3D.PLUS_I,
68 FastMath.toRadians(-179.999),
69 RotationConvention.VECTOR_OPERATOR),
70 new Vector3D(FastMath.toRadians(0), 0, 0),
71 Vector3D.ZERO);
72 sample.add(ac1);
73
74
75 final TimeInterpolator<TimeStampedAngularCoordinates> interpolator =
76 new TimeStampedAngularCoordinatesHermiteInterpolator(sample.size(), AngularDerivativesFilter.USE_R);
77
78
79 AbsoluteDate t = new AbsoluteDate("2012-01-01T00:00:01.000", TimeScalesFactory.getTAI());
80 TimeStampedAngularCoordinates interpolated = interpolator.interpolate(t, sample);
81
82 Assertions.assertEquals(FastMath.toRadians(180), interpolated.getRotation().getAngle(), 1.0e-12);
83
84 }
85
86 @Test
87 public void testInterpolationWithoutAcceleration() {
88 AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
89 double alpha0 = 0.5 * FastMath.PI;
90 double omega = 0.05 * FastMath.PI;
91 final TimeStampedAngularCoordinates reference =
92 new TimeStampedAngularCoordinates(date,
93 new Rotation(Vector3D.PLUS_K, alpha0, RotationConvention.VECTOR_OPERATOR),
94 new Vector3D(omega, Vector3D.MINUS_K),
95 Vector3D.ZERO);
96 double[] errors = interpolationErrors(reference, 1.0);
97 Assertions.assertEquals(0.0, errors[0], 1.4e-15);
98 Assertions.assertEquals(0.0, errors[1], 3.0e-15);
99 Assertions.assertEquals(0.0, errors[2], 3.0e-14);
100 }
101
102 @Test
103 public void testInterpolationWithAcceleration() {
104 AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
105 double alpha0 = 0.5 * FastMath.PI;
106 double omega = 0.05 * FastMath.PI;
107 double eta = 0.005 * FastMath.PI;
108 final TimeStampedAngularCoordinates reference =
109 new TimeStampedAngularCoordinates(date,
110 new Rotation(Vector3D.PLUS_K, alpha0,
111 RotationConvention.VECTOR_OPERATOR),
112 new Vector3D(omega, Vector3D.MINUS_K),
113 new Vector3D(eta, Vector3D.PLUS_J));
114 double[] errors = interpolationErrors(reference, 1.0);
115 Assertions.assertEquals(0.0, errors[0], 3.0e-5);
116 Assertions.assertEquals(0.0, errors[1], 2.0e-4);
117 Assertions.assertEquals(0.0, errors[2], 4.6e-3);
118 }
119
120 @Test
121 public void testInterpolationNeedOffsetWrongRate() {
122 AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
123 double omega = 2.0 * FastMath.PI;
124 TimeStampedAngularCoordinates reference =
125 new TimeStampedAngularCoordinates(date,
126 Rotation.IDENTITY,
127 new Vector3D(omega, Vector3D.MINUS_K),
128 Vector3D.ZERO);
129
130 List<TimeStampedAngularCoordinates> sample = new ArrayList<TimeStampedAngularCoordinates>();
131 for (double dt : new double[] { 0.0, 0.25, 0.5, 0.75, 1.0 }) {
132 TimeStampedAngularCoordinates shifted = reference.shiftedBy(dt);
133 sample.add(new TimeStampedAngularCoordinates(shifted.getDate(),
134 shifted.getRotation(),
135 Vector3D.ZERO, Vector3D.ZERO));
136 }
137
138
139 final TimeInterpolator<TimeStampedAngularCoordinates> interpolator =
140 new TimeStampedAngularCoordinatesHermiteInterpolator(sample.size(), AngularDerivativesFilter.USE_RR);
141
142 for (TimeStampedAngularCoordinates s : sample) {
143 TimeStampedAngularCoordinates interpolated = interpolator.interpolate(s.getDate(), sample);
144 Rotation r = interpolated.getRotation();
145 Vector3D rate = interpolated.getRotationRate();
146 Assertions.assertEquals(0.0, Rotation.distance(s.getRotation(), r), 2.0e-14);
147 Assertions.assertEquals(0.0, Vector3D.distance(s.getRotationRate(), rate), 2.0e-13);
148 }
149
150 }
151
152 @Test
153 public void testInterpolationRotationOnly() {
154 AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
155 double alpha0 = 0.5 * FastMath.PI;
156 double omega = 0.5 * FastMath.PI;
157 TimeStampedAngularCoordinates reference =
158 new TimeStampedAngularCoordinates(date,
159 new Rotation(Vector3D.PLUS_K, alpha0,
160 RotationConvention.VECTOR_OPERATOR),
161 new Vector3D(omega, Vector3D.MINUS_K),
162 Vector3D.ZERO);
163
164 List<TimeStampedAngularCoordinates> sample = new ArrayList<TimeStampedAngularCoordinates>();
165 for (double dt : new double[] { 0.0, 0.2, 0.4, 0.6, 0.8, 1.0 }) {
166 Rotation r = reference.shiftedBy(dt).getRotation();
167 sample.add(new TimeStampedAngularCoordinates(date.shiftedBy(dt), r, Vector3D.ZERO, Vector3D.ZERO));
168 }
169
170
171 final TimeInterpolator<TimeStampedAngularCoordinates> interpolator =
172 new TimeStampedAngularCoordinatesHermiteInterpolator(sample.size(), AngularDerivativesFilter.USE_R);
173
174 for (double dt = 0; dt < 1.0; dt += 0.001) {
175 TimeStampedAngularCoordinates interpolated = interpolator.interpolate(date.shiftedBy(dt), sample);
176 Rotation r = interpolated.getRotation();
177 Vector3D rate = interpolated.getRotationRate();
178 Assertions.assertEquals(0.0, Rotation.distance(reference.shiftedBy(dt).getRotation(), r), 3.0e-4);
179 Assertions.assertEquals(0.0, Vector3D.distance(reference.shiftedBy(dt).getRotationRate(), rate), 1.0e-2);
180 }
181
182 }
183
184
185
186
187
188
189 @Test
190 public void testInterpolationSmallSample() {
191 AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
192 double alpha0 = 0.5 * FastMath.PI;
193 double omega = 0.5 * FastMath.PI;
194 TimeStampedAngularCoordinates reference =
195 new TimeStampedAngularCoordinates(date,
196 new Rotation(Vector3D.PLUS_K, alpha0,
197 RotationConvention.VECTOR_OPERATOR),
198 new Vector3D(omega, Vector3D.MINUS_K),
199 Vector3D.ZERO);
200
201 List<TimeStampedAngularCoordinates> sample = new ArrayList<TimeStampedAngularCoordinates>();
202 Rotation r = reference.shiftedBy(0.2).getRotation();
203 sample.add(new TimeStampedAngularCoordinates(date.shiftedBy(0.2), r, Vector3D.ZERO, Vector3D.ZERO));
204
205
206 final TimeInterpolator<TimeStampedAngularCoordinates> interpolator =
207 new TimeStampedAngularCoordinatesHermiteInterpolator(sample.size(), 0.3, AngularDerivativesFilter.USE_RR);
208
209
210 final AbsoluteDate t = date.shiftedBy(0.3);
211 final TimeStampedAngularCoordinates actual = interpolator.interpolate(t, sample);
212
213
214 MatcherAssert.assertThat(actual.getDate(),
215 OrekitMatchers.durationFrom(t, OrekitMatchers.closeTo(0, 0)));
216 MatcherAssert.assertThat(actual.getRotation(),
217 OrekitMatchers.distanceIs(r, OrekitMatchers.closeTo(0, FastMath.ulp(1.0))));
218 MatcherAssert.assertThat(actual.getRotationRate(),
219 OrekitMatchers.vectorCloseTo(Vector3D.ZERO, 0));
220 MatcherAssert.assertThat(actual.getRotationAcceleration(),
221 OrekitMatchers.vectorCloseTo(Vector3D.ZERO, 0));
222 }
223
224 @Test
225 public void testInterpolationGTODIssue() {
226 AbsoluteDate t0 = new AbsoluteDate("2004-04-06T19:59:28.000", TimeScalesFactory.getTAI());
227 double[][] params = new double[][] {
228 { 0.0, -0.3802356750911964, -0.9248896320037013, 7.292115030462892e-5 },
229 { 4.0, 0.1345716955788532, -0.990903859488413, 7.292115033301528e-5 },
230 { 8.0, -0.613127541102373, 0.7899839354960061, 7.292115037371062e-5 }
231 };
232 List<TimeStampedAngularCoordinates> sample = new ArrayList<TimeStampedAngularCoordinates>();
233 for (double[] row : params) {
234 AbsoluteDate t = t0.shiftedBy(row[0] * 3600.0);
235 Rotation r = new Rotation(row[1], 0.0, 0.0, row[2], false);
236 Vector3D o = new Vector3D(row[3], Vector3D.PLUS_K);
237 sample.add(new TimeStampedAngularCoordinates(t, r, o, Vector3D.ZERO));
238 }
239
240
241 final TimeInterpolator<TimeStampedAngularCoordinates> interpolator =
242 new TimeStampedAngularCoordinatesHermiteInterpolator(sample.size(), 18200, AngularDerivativesFilter.USE_RR);
243
244 for (double dt = 0; dt < 29000; dt += 120) {
245 TimeStampedAngularCoordinates shifted = sample.get(0).shiftedBy(dt);
246 TimeStampedAngularCoordinates interpolated = interpolator.interpolate(t0.shiftedBy(dt), sample);
247 Assertions.assertEquals(0.0,
248 Rotation.distance(shifted.getRotation(), interpolated.getRotation()),
249 1.3e-7);
250 Assertions.assertEquals(0.0,
251 Vector3D.distance(shifted.getRotationRate(), interpolated.getRotationRate()),
252 1.0e-11);
253 }
254
255 }
256
257 private double[] interpolationErrors(final TimeStampedAngularCoordinates reference, double dt) {
258
259 final OrdinaryDifferentialEquation ode = new OrdinaryDifferentialEquation() {
260 public int getDimension() {
261 return 4;
262 }
263
264 public double[] computeDerivatives(final double t, final double[] q) {
265 final double omegaX = reference.getRotationRate().getX() + t * reference.getRotationAcceleration().getX();
266 final double omegaY = reference.getRotationRate().getY() + t * reference.getRotationAcceleration().getY();
267 final double omegaZ = reference.getRotationRate().getZ() + t * reference.getRotationAcceleration().getZ();
268 return new double[] {
269 0.5 * MathArrays.linearCombination(-q[1], omegaX, -q[2], omegaY, -q[3], omegaZ),
270 0.5 * MathArrays.linearCombination(q[0], omegaX, -q[3], omegaY, q[2], omegaZ),
271 0.5 * MathArrays.linearCombination(q[3], omegaX, q[0], omegaY, -q[1], omegaZ),
272 0.5 * MathArrays.linearCombination(-q[2], omegaX, q[1], omegaY, q[0], omegaZ)
273 };
274 }
275 };
276 final List<TimeStampedAngularCoordinates> complete = new ArrayList<TimeStampedAngularCoordinates>();
277 ODEIntegrator integrator = new DormandPrince853Integrator(1.0e-6, 1.0, 1.0e-12, 1.0e-12);
278 integrator.addStepHandler(new StepNormalizer(dt / 2000, new ODEFixedStepHandler() {
279 public void handleStep(ODEStateAndDerivative state, boolean isLast) {
280 final double t = state.getTime();
281 final double[] q = state.getPrimaryState();
282 complete.add(new TimeStampedAngularCoordinates(reference.getDate().shiftedBy(t),
283 new Rotation(q[0], q[1], q[2], q[3], true),
284 new Vector3D(1, reference.getRotationRate(),
285 t, reference.getRotationAcceleration()),
286 reference.getRotationAcceleration()));
287 }
288 }));
289
290 double[] y = new double[] {
291 reference.getRotation().getQ0(),
292 reference.getRotation().getQ1(),
293 reference.getRotation().getQ2(),
294 reference.getRotation().getQ3()
295 };
296 integrator.integrate(ode, new ODEState(0, y), dt);
297
298 List<TimeStampedAngularCoordinates> sample = new ArrayList<TimeStampedAngularCoordinates>();
299 sample.add(complete.get(0));
300 sample.add(complete.get(complete.size() / 2));
301 sample.add(complete.get(complete.size() - 1));
302
303
304 final TimeInterpolator<TimeStampedAngularCoordinates> interpolator =
305 new TimeStampedAngularCoordinatesHermiteInterpolator(sample.size(), AngularDerivativesFilter.USE_RRA);
306
307 double maxRotationError = 0;
308 double maxRateError = 0;
309 double maxAccelerationError = 0;
310 for (TimeStampedAngularCoordinates acRef : complete) {
311 TimeStampedAngularCoordinates interpolated =
312 interpolator.interpolate(acRef.getDate(), sample);
313 double rotationError = Rotation.distance(acRef.getRotation(), interpolated.getRotation());
314 double rateError = Vector3D.distance(acRef.getRotationRate(), interpolated.getRotationRate());
315 double accelerationError =
316 Vector3D.distance(acRef.getRotationAcceleration(), interpolated.getRotationAcceleration());
317 maxRotationError = FastMath.max(maxRotationError, rotationError);
318 maxRateError = FastMath.max(maxRateError, rateError);
319 maxAccelerationError = FastMath.max(maxAccelerationError, accelerationError);
320 }
321
322 return new double[] {
323 maxRotationError, maxRateError, maxAccelerationError
324 };
325
326 }
327
328 @Test
329 @DisplayName("Test default constructor")
330 void testDefaultConstructor() {
331
332
333 final TimeStampedAngularCoordinatesHermiteInterpolator interpolator =
334 new TimeStampedAngularCoordinatesHermiteInterpolator();
335
336
337 final AngularDerivativesFilter expectedFilter = AngularDerivativesFilter.USE_RR;
338
339 Assertions.assertEquals(AbstractTimeInterpolator.DEFAULT_EXTRAPOLATION_THRESHOLD_SEC,
340 interpolator.getExtrapolationThreshold());
341 Assertions.assertEquals(expectedFilter, interpolator.getFilter());
342 }
343
344 @BeforeEach
345 public void setUp() {
346 Utils.setDataRoot("regular-data");
347 }
348
349 }