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.analysis.differentiation.DSFactory;
22 import org.hipparchus.analysis.differentiation.DerivativeStructure;
23 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
24 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
25 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
26 import org.hipparchus.geometry.euclidean.threed.Vector3D;
27 import org.hipparchus.util.Binary64;
28 import org.hipparchus.util.FastMath;
29 import org.junit.jupiter.api.Assertions;
30 import org.junit.jupiter.api.BeforeEach;
31 import org.junit.jupiter.api.DisplayName;
32 import org.junit.jupiter.api.Test;
33 import org.orekit.OrekitMatchers;
34 import org.orekit.Utils;
35 import org.orekit.time.AbsoluteDate;
36 import org.orekit.time.AbstractTimeInterpolator;
37 import org.orekit.time.FieldTimeInterpolator;
38 import org.orekit.time.TimeScalesFactory;
39
40 import java.util.ArrayList;
41 import java.util.List;
42
43 class TimeStampedFieldAngularCoordinatesHermiteInterpolatorTest {
44 @Test
45 public void testInterpolationNeedOffsetWrongRate() {
46 AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
47 double omega = 2.0 * FastMath.PI;
48 TimeStampedFieldAngularCoordinates<DerivativeStructure> reference =
49 new TimeStampedFieldAngularCoordinates<>(date,
50 TimeStampedFieldAngularCoordinatesTest.createRotation(1, 0, 0, 0,
51 false),
52 TimeStampedFieldAngularCoordinatesTest.createVector(0, 0, -omega,
53 4),
54 TimeStampedFieldAngularCoordinatesTest.createVector(0, 0, 0, 4));
55
56 List<TimeStampedFieldAngularCoordinates<DerivativeStructure>> sample = new ArrayList<>();
57 for (double dt : new double[] { 0.0, 0.25, 0.5, 0.75, 1.0 }) {
58 TimeStampedFieldAngularCoordinates<DerivativeStructure> shifted = reference.shiftedBy(dt);
59 sample.add(new TimeStampedFieldAngularCoordinates<>(shifted.getDate(),
60 shifted.getRotation(),
61 TimeStampedFieldAngularCoordinatesTest.createVector(0, 0, 0,
62 4),
63 TimeStampedFieldAngularCoordinatesTest.createVector(0, 0, 0,
64 4)));
65 }
66
67
68 final FieldTimeInterpolator<TimeStampedFieldAngularCoordinates<DerivativeStructure>, DerivativeStructure>
69 interpolator = new TimeStampedFieldAngularCoordinatesHermiteInterpolator<>(sample.size());
70
71 for (TimeStampedFieldAngularCoordinates<DerivativeStructure> s : sample) {
72 TimeStampedFieldAngularCoordinates<DerivativeStructure> interpolated =
73 interpolator.interpolate(s.getDate(), sample);
74 FieldRotation<DerivativeStructure> r = interpolated.getRotation();
75 FieldVector3D<DerivativeStructure> rate = interpolated.getRotationRate();
76 Assertions.assertEquals(0.0, FieldRotation.distance(s.getRotation(), r).getReal(), 2.0e-14);
77 Assertions.assertEquals(0.0, FieldVector3D.distance(s.getRotationRate(), rate).getReal(), 2.0e-13);
78 }
79
80 }
81
82 @Test
83 public void testInterpolationRotationOnly() {
84 AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
85 double alpha0 = 0.5 * FastMath.PI;
86 double omega = 0.5 * FastMath.PI;
87 TimeStampedFieldAngularCoordinates<DerivativeStructure> reference =
88 new TimeStampedFieldAngularCoordinates<>(date,
89 TimeStampedFieldAngularCoordinatesTest.createRotation(
90 TimeStampedFieldAngularCoordinatesTest.createVector(0, 0, 1,
91 4),
92 alpha0),
93 new FieldVector3D<>(omega,
94 TimeStampedFieldAngularCoordinatesTest.createVector(
95 0, 0, -1, 4)),
96 TimeStampedFieldAngularCoordinatesTest.createVector(0, 0, 0, 4));
97
98 List<TimeStampedFieldAngularCoordinates<DerivativeStructure>> sample = new ArrayList<>();
99 for (double dt : new double[] { 0.0, 0.2, 0.4, 0.6, 0.8, 1.0 }) {
100 FieldRotation<DerivativeStructure> r = reference.shiftedBy(dt).getRotation();
101 sample.add(new TimeStampedFieldAngularCoordinates<>(date.shiftedBy(dt), r,
102 TimeStampedFieldAngularCoordinatesTest.createVector(0, 0, 0,
103 4),
104 TimeStampedFieldAngularCoordinatesTest.createVector(0, 0, 0,
105 4)));
106 }
107
108
109 final FieldTimeInterpolator<TimeStampedFieldAngularCoordinates<DerivativeStructure>, DerivativeStructure>
110 interpolator =
111 new TimeStampedFieldAngularCoordinatesHermiteInterpolator<>(sample.size(), AngularDerivativesFilter.USE_R);
112
113 for (double dt = 0; dt < 1.0; dt += 0.001) {
114 TimeStampedFieldAngularCoordinates<DerivativeStructure> interpolated =
115 interpolator.interpolate(date.shiftedBy(dt), sample);
116 FieldRotation<DerivativeStructure> r = interpolated.getRotation();
117 FieldVector3D<DerivativeStructure> rate = interpolated.getRotationRate();
118 FieldVector3D<DerivativeStructure> acceleration = interpolated.getRotationAcceleration();
119 Assertions.assertEquals(0.0, FieldRotation.distance(reference.shiftedBy(dt).getRotation(), r).getReal(), 3.0e-4);
120 Assertions.assertEquals(0.0, FieldVector3D.distance(reference.shiftedBy(dt).getRotationRate(), rate).getReal(),
121 1.0e-2);
122 Assertions.assertEquals(0.0,
123 FieldVector3D.distance(reference.shiftedBy(dt).getRotationAcceleration(), acceleration)
124 .getReal(), 1.0e-2);
125 }
126
127 }
128
129 @Test
130 public void testInterpolationAroundPI() {
131
132 DSFactory factory = new DSFactory(4, 1);
133 List<TimeStampedFieldAngularCoordinates<DerivativeStructure>> sample = new ArrayList<>();
134
135
136 AbsoluteDate t0 = new AbsoluteDate("2012-01-01T00:00:00.000", TimeScalesFactory.getTAI());
137 TimeStampedFieldAngularCoordinates<DerivativeStructure> ac0 =
138 new TimeStampedFieldAngularCoordinates<>(t0,
139 new FieldRotation<>(
140 TimeStampedFieldAngularCoordinatesTest.createVector(1, 0, 0,
141 4),
142 factory.variable(3, FastMath.toRadians(179.999)),
143 RotationConvention.VECTOR_OPERATOR),
144 TimeStampedFieldAngularCoordinatesTest.createVector(
145 FastMath.toRadians(0), 0, 0, 4),
146 TimeStampedFieldAngularCoordinatesTest.createVector(0, 0, 0, 4));
147 sample.add(ac0);
148
149
150 AbsoluteDate t1 = new AbsoluteDate("2012-01-01T00:00:02.000", TimeScalesFactory.getTAI());
151 TimeStampedFieldAngularCoordinates<DerivativeStructure> ac1 =
152 new TimeStampedFieldAngularCoordinates<>(t1,
153 new FieldRotation<>(
154 TimeStampedFieldAngularCoordinatesTest.createVector(1, 0, 0,
155 4),
156 factory.variable(3, FastMath.toRadians(-179.999)),
157 RotationConvention.VECTOR_OPERATOR),
158 TimeStampedFieldAngularCoordinatesTest.createVector(
159 FastMath.toRadians(0), 0, 0, 4),
160 TimeStampedFieldAngularCoordinatesTest.createVector(0, 0, 0, 4));
161 sample.add(ac1);
162
163
164 final FieldTimeInterpolator<TimeStampedFieldAngularCoordinates<DerivativeStructure>, DerivativeStructure>
165 interpolator =
166 new TimeStampedFieldAngularCoordinatesHermiteInterpolator<>(sample.size(), AngularDerivativesFilter.USE_R);
167
168
169 AbsoluteDate t = new AbsoluteDate("2012-01-01T00:00:01.000", TimeScalesFactory.getTAI());
170 TimeStampedFieldAngularCoordinates<DerivativeStructure> interpolated =
171 interpolator.interpolate(t, sample);
172
173 Assertions.assertEquals(FastMath.toRadians(180), interpolated.getRotation().getAngle().getReal(), 1.0e-12);
174
175 }
176
177
178
179
180
181 @Test
182 public void testInterpolationSmallSample() {
183 DSFactory factory = new DSFactory(4, 1);
184 AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
185 double alpha0 = 0.5 * FastMath.PI;
186 double omega = 0.5 * FastMath.PI;
187 TimeStampedFieldAngularCoordinates<DerivativeStructure> reference =
188 new TimeStampedFieldAngularCoordinates<>(date,
189 new FieldRotation<>(
190 TimeStampedFieldAngularCoordinatesTest.createVector(0, 0, 1,
191 4),
192 factory.variable(3, alpha0),
193 RotationConvention.VECTOR_OPERATOR),
194 TimeStampedFieldAngularCoordinatesTest.createVector(0, 0, -omega,
195 4),
196 TimeStampedFieldAngularCoordinatesTest.createVector(0, 0, 0, 4));
197
198 List<TimeStampedFieldAngularCoordinates<DerivativeStructure>> sample = new ArrayList<>();
199 FieldRotation<DerivativeStructure> r = reference.shiftedBy(0.2).getRotation();
200 sample.add(new TimeStampedFieldAngularCoordinates<>(date.shiftedBy(0.2), r,
201 TimeStampedFieldAngularCoordinatesTest.createVector(0, 0, 0, 4),
202 TimeStampedFieldAngularCoordinatesTest.createVector(0, 0, 0,
203 4)));
204
205
206 final FieldTimeInterpolator<TimeStampedFieldAngularCoordinates<DerivativeStructure>, DerivativeStructure>
207 interpolator =
208 new TimeStampedFieldAngularCoordinatesHermiteInterpolator<>(sample.size(), AngularDerivativesFilter.USE_RR);
209
210
211 final TimeStampedFieldAngularCoordinates<DerivativeStructure> actual =
212 interpolator.interpolate(date.shiftedBy(0.3), sample);
213
214
215 MatcherAssert.assertThat(actual.getRotation().toRotation(),
216 OrekitMatchers.distanceIs(
217 r.toRotation(),
218 OrekitMatchers.closeTo(0, FastMath.ulp(1.0))));
219 MatcherAssert.assertThat(actual.getRotationRate().toVector3D(),
220 OrekitMatchers.vectorCloseTo(Vector3D.ZERO, 0));
221 MatcherAssert.assertThat(actual.getRotationAcceleration().toVector3D(),
222 OrekitMatchers.vectorCloseTo(Vector3D.ZERO, 0));
223 }
224
225 @Test
226 public void testInterpolationGTODIssue() {
227 AbsoluteDate t0 = new AbsoluteDate("2004-04-06T19:59:28.000", TimeScalesFactory.getTAI());
228 double[][] params = new double[][] {
229 { 0.0, -0.3802356750911964, -0.9248896320037013, 7.292115030462892e-5 },
230 { 4.0, 0.1345716955788532, -0.990903859488413, 7.292115033301528e-5 },
231 { 8.0, -0.613127541102373, 0.7899839354960061, 7.292115037371062e-5 }
232 };
233 List<TimeStampedFieldAngularCoordinates<DerivativeStructure>> sample =
234 new ArrayList<TimeStampedFieldAngularCoordinates<DerivativeStructure>>();
235 for (double[] row : params) {
236 AbsoluteDate t = t0.shiftedBy(row[0] * 3600.0);
237 FieldRotation<DerivativeStructure> r =
238 TimeStampedFieldAngularCoordinatesTest.createRotation(row[1], 0.0, 0.0, row[2], false);
239 FieldVector3D<DerivativeStructure> o =
240 new FieldVector3D<>(row[3], TimeStampedFieldAngularCoordinatesTest.createVector(0, 0, 1, 4));
241 sample.add(new TimeStampedFieldAngularCoordinates<>(t, r, o,
242 TimeStampedFieldAngularCoordinatesTest.createVector(0, 0, 0,
243 4)));
244 }
245
246
247 final FieldTimeInterpolator<TimeStampedFieldAngularCoordinates<DerivativeStructure>, DerivativeStructure>
248 interpolator = new TimeStampedFieldAngularCoordinatesHermiteInterpolator<>(sample.size(), 200,
249 AngularDerivativesFilter.USE_RR);
250
251 for (double dt = 0; dt < 29000; dt += 120) {
252 TimeStampedFieldAngularCoordinates<DerivativeStructure> shifted = sample.get(0).shiftedBy(dt);
253 TimeStampedFieldAngularCoordinates<DerivativeStructure> interpolated =
254 interpolator.interpolate(t0.shiftedBy(dt), sample);
255 Assertions.assertEquals(0.0, FieldRotation.distance(shifted.getRotation(), interpolated.getRotation()).getReal(),
256 1.3e-7);
257 Assertions.assertEquals(0.0, FieldVector3D.distance(shifted.getRotationRate(), interpolated.getRotationRate())
258 .getReal(), 1.0e-11);
259 }
260
261 }
262
263 @Test
264 @DisplayName("Test default constructor")
265 void testDefaultConstructor() {
266
267 final AngularDerivativesFilter filter = AngularDerivativesFilter.USE_R;
268
269
270 final TimeStampedFieldAngularCoordinatesHermiteInterpolator<Binary64> interpolator =
271 new TimeStampedFieldAngularCoordinatesHermiteInterpolator<>(2, filter);
272
273
274 Assertions.assertEquals(AbstractTimeInterpolator.DEFAULT_EXTRAPOLATION_THRESHOLD_SEC,
275 interpolator.getExtrapolationThreshold());
276 Assertions.assertEquals(filter, interpolator.getFilter());
277 }
278
279 @BeforeEach
280 public void setUp() {
281 Utils.setDataRoot("regular-data");
282 }
283
284 }