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