1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.attitudes;
18
19 import java.util.ArrayList;
20 import java.util.Arrays;
21 import java.util.List;
22 import java.util.stream.Collectors;
23 import java.util.stream.Stream;
24
25 import org.hipparchus.CalculusFieldElement;
26 import org.hipparchus.Field;
27 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
28 import org.hipparchus.geometry.euclidean.threed.Rotation;
29 import org.hipparchus.geometry.euclidean.threed.Vector3D;
30 import org.hipparchus.util.Binary64;
31 import org.hipparchus.util.Binary64Field;
32 import org.hipparchus.util.CombinatoricsUtils;
33 import org.hipparchus.util.FastMath;
34 import org.junit.jupiter.api.Assertions;
35 import org.junit.jupiter.api.BeforeEach;
36 import org.junit.jupiter.api.Test;
37 import org.orekit.Utils;
38 import org.orekit.frames.Frame;
39 import org.orekit.frames.FramesFactory;
40 import org.orekit.orbits.FieldKeplerianOrbit;
41 import org.orekit.orbits.FieldOrbit;
42 import org.orekit.orbits.KeplerianOrbit;
43 import org.orekit.orbits.Orbit;
44 import org.orekit.time.AbsoluteDate;
45 import org.orekit.time.DateComponents;
46 import org.orekit.time.FieldAbsoluteDate;
47 import org.orekit.time.TimeComponents;
48 import org.orekit.time.TimeScalesFactory;
49 import org.orekit.utils.FieldPVCoordinates;
50 import org.orekit.utils.PVCoordinates;
51
52 public class TorqueFreeTest {
53
54 @Test
55 void testLocalBehavior() {
56 AbsoluteDate initialDate = new AbsoluteDate(new DateComponents(2004, 3, 2),
57 new TimeComponents(13, 17, 7.865),
58 TimeScalesFactory.getUTC());
59 final Frame frame = FramesFactory.getEME2000();
60 final Attitude initialAttitude = new Attitude(initialDate, frame,
61 new Rotation(0.9, 0.437, 0.0, 0.0, true),
62 new Vector3D(0.05, 0.0, 0.04),
63 Vector3D.ZERO);
64 final Inertia inertia = new Inertia(new InertiaAxis(3.0 / 8.0, Vector3D.PLUS_I),
65 new InertiaAxis(1.0 / 2.0, Vector3D.PLUS_J),
66 new InertiaAxis(5.0 / 8.0, Vector3D.PLUS_K));
67 final TorqueFree torqueFree = new TorqueFree(initialAttitude, inertia);
68 PVCoordinates pv =
69 new PVCoordinates(new Vector3D(28812595.32012577, 5948437.4640250085, 0),
70 new Vector3D(0, 0, 3680.853673522056));
71 Orbit orbit = new KeplerianOrbit(pv, frame, initialDate, 3.986004415e14);
72
73 Assertions.assertSame(initialAttitude, torqueFree.getInitialAttitude());
74 Assertions.assertSame(inertia, torqueFree.getInertia());
75
76
77 Attitude attitude0 = torqueFree.getAttitude(orbit, initialDate, frame);
78 Assertions.assertEquals(0,
79 Rotation.distance(initialAttitude.getRotation(), attitude0.getRotation()),
80 1.0e-10);
81 Assertions.assertEquals(0,
82 Vector3D.distance(initialAttitude.getSpin(), attitude0.getSpin()),
83 1.0e-10);
84
85
86 double maxError = 0;
87 for (double dt = -0.1; dt < 0.1; dt += 0.001) {
88 double error = Rotation.distance(initialAttitude.shiftedBy(dt).getRotation(),
89 torqueFree.getAttitude(orbit, initialDate.shiftedBy(dt), frame).getRotation());
90 maxError = FastMath.max(error, maxError);
91 }
92 Assertions.assertEquals(5.0e-6, maxError, 1.0e-7);
93 maxError = 0;
94 for (double dt = -1.0; dt < 1.0; dt += 0.001) {
95 double error = Rotation.distance(initialAttitude.shiftedBy(dt).getRotation(),
96 torqueFree.getAttitude(orbit, initialDate.shiftedBy(dt), frame).getRotation());
97 maxError = FastMath.max(error, maxError);
98 }
99 Assertions.assertEquals(5.0e-4, maxError, 1.0e-7);
100
101 }
102
103
104 @Test
105 void testMomentum() {
106 AbsoluteDate initialDate = new AbsoluteDate(new DateComponents(2004, 3, 2),
107 new TimeComponents(13, 17, 7.865),
108 TimeScalesFactory.getUTC());
109 final Frame frame = FramesFactory.getEME2000();
110 final Attitude initialAttitude = new Attitude(initialDate, frame,
111 new Rotation(0.9, 0.437, 0.0, 0.0, true),
112 new Vector3D(0.05, 0.0, 0.04),
113 Vector3D.ZERO);
114 final Inertia inertia = new Inertia(new InertiaAxis(3.0 / 8.0, Vector3D.PLUS_I),
115 new InertiaAxis(1.0 / 2.0, Vector3D.PLUS_J),
116 new InertiaAxis(5.0 / 8.0, Vector3D.PLUS_K));
117 PVCoordinates pv =
118 new PVCoordinates(new Vector3D(28812595.32012577, 5948437.4640250085, 0),
119 new Vector3D(0, 0, 3680.853673522056));
120 Orbit orbit = new KeplerianOrbit(pv, frame, initialDate, 3.986004415e14);
121 Stream<TorqueFree> useCases = Stream.empty();
122
123
124 useCases = Stream.concat(useCases,
125 permute(initialAttitude.getSpin()).
126 map(s -> new TorqueFree(new Attitude(initialDate, frame,
127 initialAttitude.getRotation(),
128 s,
129 initialAttitude.getRotationAcceleration()),
130 inertia)));
131
132
133 useCases = Stream.concat(useCases,
134 permute(initialAttitude.getRotation()).
135 map(r -> new TorqueFree(new Attitude(initialDate, frame,
136 r,
137 initialAttitude.getSpin(),
138 initialAttitude.getRotationAcceleration()),
139 inertia)));
140
141
142 useCases = Stream.concat(useCases,
143 permute(inertia).map(i -> new TorqueFree(initialAttitude, i)));
144
145 useCases.forEach(tf -> {
146 doTestMomentum(tf, orbit, 1.6e-15);
147 });
148
149 }
150
151 private void doTestMomentum(final TorqueFree torqueFree, final Orbit orbit, final double tol) {
152 final Attitude initialAttitude = torqueFree.getInitialAttitude();
153 final Inertia inertia = torqueFree.getInertia();
154 final Vector3D initialMomentum = initialAttitude.getRotation().
155 applyInverseTo(torqueFree.getInertia().momentum(initialAttitude.getSpin()));
156
157 double maxError = 0;
158 for (double dt = -40; dt < 40; dt += 0.01) {
159 final Attitude attitude = torqueFree.getAttitude(orbit, orbit.getDate().shiftedBy(dt),
160 initialAttitude.getReferenceFrame());
161 final Vector3D momentum = attitude.getRotation().applyInverseTo(inertia.momentum(attitude.getSpin()));
162 maxError = FastMath.max(maxError, Vector3D.angle(momentum, initialMomentum));
163 }
164 Assertions.assertEquals(0.0, maxError, tol);
165
166 }
167
168
169 @Test
170 void testFieldMomentum() {
171 AbsoluteDate initialDate = new AbsoluteDate(new DateComponents(2004, 3, 2),
172 new TimeComponents(13, 17, 7.865),
173 TimeScalesFactory.getUTC());
174 final Frame frame = FramesFactory.getEME2000();
175 final Attitude initialAttitude = new Attitude(initialDate, frame,
176 new Rotation(0.9, 0.437, 0.0, 0.0, true),
177 new Vector3D(0.05, 0.0, 0.04),
178 Vector3D.ZERO);
179 final Inertia inertia = new Inertia(new InertiaAxis(3.0 / 8.0, Vector3D.PLUS_I),
180 new InertiaAxis(1.0 / 2.0, Vector3D.PLUS_J),
181 new InertiaAxis(5.0 / 8.0, Vector3D.PLUS_K));
182 PVCoordinates pv =
183 new PVCoordinates(new Vector3D(28812595.32012577, 5948437.4640250085, 0),
184 new Vector3D(0, 0, 3680.853673522056));
185 Orbit orbit = new KeplerianOrbit(pv, frame, initialDate, 3.986004415e14);
186 Stream<TorqueFree> useCases = Stream.empty();
187
188
189 useCases = Stream.concat(useCases,
190 permute(initialAttitude.getSpin()).
191 map(s -> new TorqueFree(new Attitude(initialDate, frame,
192 initialAttitude.getRotation(),
193 s,
194 initialAttitude.getRotationAcceleration()),
195 inertia)));
196
197
198 useCases = Stream.concat(useCases,
199 permute(initialAttitude.getRotation()).
200 map(r -> new TorqueFree(new Attitude(initialDate, frame,
201 r,
202 initialAttitude.getSpin(),
203 initialAttitude.getRotationAcceleration()),
204 inertia)));
205
206
207 useCases = Stream.concat(useCases,
208 permute(inertia).map(i -> new TorqueFree(initialAttitude, i)));
209
210 useCases.forEach(tf -> {
211 doTestMomentum(Binary64Field.getInstance(), tf, orbit, 1.6e-15);
212 });
213
214 }
215
216 private <T extends CalculusFieldElement<T>> void doTestMomentum(final Field<T> field,
217 final TorqueFree torqueFree,
218 final Orbit orbit,
219 final double tol) {
220 final T zero = field.getZero();
221 final Attitude initialAttitude = torqueFree.getInitialAttitude();
222 final Inertia inertia = torqueFree.getInertia();
223 final Vector3D initialMomentum = initialAttitude.getRotation().applyInverseTo(inertia.momentum(initialAttitude.getSpin()));
224 final FieldVector3D<T> fInitialMomentum = new FieldVector3D<>(field, initialMomentum);
225 final FieldInertia<T> fInertia =
226 new FieldInertia<>(new FieldInertiaAxis<>(zero.newInstance(inertia.getInertiaAxis1().getI()),
227 new FieldVector3D<>(field, inertia.getInertiaAxis1().getA())),
228 new FieldInertiaAxis<>(zero.newInstance(inertia.getInertiaAxis2().getI()),
229 new FieldVector3D<>(field, inertia.getInertiaAxis2().getA())),
230 new FieldInertiaAxis<>(zero.newInstance(inertia.getInertiaAxis3().getI()),
231 new FieldVector3D<>(field, inertia.getInertiaAxis3().getA())));
232
233 FieldOrbit<T> fOrbit = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(field, orbit.getPVCoordinates()),
234 orbit.getFrame(), new FieldAbsoluteDate<>(field, orbit.getDate()),
235 field.getZero().newInstance(orbit.getMu()));
236 T maxError = field.getZero();
237 for (double dt = -40; dt < 40; dt += 0.01) {
238 final FieldAttitude<T> attitude = torqueFree.getAttitude(fOrbit, fOrbit.getDate().shiftedBy(dt),
239 initialAttitude.getReferenceFrame());
240 final FieldVector3D<T> momentum = attitude.getRotation().applyInverseTo(fInertia.momentum(attitude.getSpin()));
241 maxError = FastMath.max(maxError, FieldVector3D.angle(momentum, fInitialMomentum));
242 }
243 Assertions.assertEquals(0.0, maxError.getReal(), tol);
244
245 }
246
247 @Test
248 void testField() {
249 AbsoluteDate initialDate = new AbsoluteDate(new DateComponents(2004, 3, 2),
250 new TimeComponents(13, 17, 7.865),
251 TimeScalesFactory.getUTC());
252 final Frame frame = FramesFactory.getEME2000();
253 final Attitude initialAttitude = new Attitude(initialDate, frame,
254 new Rotation(0.9, 0.437, 0.0, 0.0, true),
255 new Vector3D(0.05, 0.0, 0.04),
256 Vector3D.ZERO);
257 final Inertia inertia = new Inertia(new InertiaAxis(3.0 / 8.0, Vector3D.PLUS_I),
258 new InertiaAxis(1.0 / 2.0, Vector3D.PLUS_J),
259 new InertiaAxis(5.0 / 8.0, Vector3D.PLUS_K));
260 final TorqueFree torqueFree = new TorqueFree(initialAttitude, inertia);
261 PVCoordinates pv =
262 new PVCoordinates(new Vector3D(28812595.32012577, 5948437.4640250085, 0),
263 new Vector3D(0, 0, 3680.853673522056));
264 Orbit orbit = new KeplerianOrbit(pv, frame, initialDate, 3.986004415e14);
265 Field<Binary64> field = Binary64Field.getInstance();
266 FieldOrbit<Binary64> orbit64= new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(field, pv),
267 frame, new FieldAbsoluteDate<>(field, orbit.getDate()),
268 field.getZero().newInstance(orbit.getMu()));
269
270 for (double dt = -20.0; dt < 20.0; dt += 0.1) {
271 final Attitude a = torqueFree.getAttitude(orbit,
272 orbit.getDate().shiftedBy(dt),
273 frame);
274 final FieldAttitude<Binary64> a64 = torqueFree.getAttitude(orbit64,
275 orbit64.getDate().shiftedBy(dt),
276 frame);
277 Assertions.assertEquals(0.0,
278 Rotation.distance(a.getRotation(), a64.getRotation().toRotation()),
279 1.0e-10);
280 Assertions.assertEquals(0.0,
281 Vector3D.distance(a.getSpin(), a64.getSpin().toVector3D()),
282 1.0e-10);
283 Assertions.assertEquals(0.0,
284 Vector3D.distance(a.getRotationAcceleration(), a64.getRotationAcceleration().toVector3D()),
285 1.0e-10);
286 }
287 }
288
289 @BeforeEach
290 public void setUp() {
291 Utils.setDataRoot("regular-data");
292 }
293
294
295
296
297
298 private Stream<Vector3D> permute(final Vector3D v) {
299 return CombinatoricsUtils.
300 permutations(Arrays.asList(v.getX(), v.getY(), v.getZ())).
301 map(a -> new Vector3D(a.get(0), a.get(1), a.get(2)));
302 }
303
304
305
306
307
308 private Stream<Rotation> permute(final Rotation r) {
309 return CombinatoricsUtils.
310 permutations(Arrays.asList(r.getQ0(), r.getQ1(), r.getQ2(), r.getQ3())).
311 map(a -> new Rotation(a.get(0), a.get(1), a.get(2), a.get(3), false));
312 }
313
314
315
316
317
318 private Stream<Inertia> permute(final Inertia inertia) {
319 List<Inertia> permuted = new ArrayList<>();
320
321
322 CombinatoricsUtils.
323 permutations(Arrays.asList(inertia.getInertiaAxis1(), inertia.getInertiaAxis2(), inertia.getInertiaAxis3())).
324 forEach(ia ->
325 permuted.addAll(CombinatoricsUtils.permutations(Arrays.asList(0, 1, 2)).
326 map(i -> new Inertia(new InertiaAxis(ia.get(i.get(0)).getI(), ia.get(0).getA()),
327 new InertiaAxis(ia.get(i.get(1)).getI(), ia.get(1).getA()),
328 new InertiaAxis(ia.get(i.get(2)).getI(), ia.get(2).getA()))).
329 collect(Collectors.toList())));
330 return permuted.stream();
331 }
332
333 }
334