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