1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.attitudes;
18
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
22 import org.hipparchus.analysis.differentiation.UnivariateDerivative1Field;
23 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
24 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
25 import org.hipparchus.geometry.euclidean.threed.Vector3D;
26 import org.hipparchus.util.Binary64Field;
27 import org.hipparchus.util.FastMath;
28 import org.junit.jupiter.api.Assertions;
29 import org.junit.jupiter.api.Test;
30 import org.mockito.Mockito;
31 import org.orekit.frames.FieldTransform;
32 import org.orekit.frames.Frame;
33 import org.orekit.frames.FramesFactory;
34 import org.orekit.time.FieldAbsoluteDate;
35 import org.orekit.utils.FieldAngularCoordinates;
36
37
38 class FieldAttitudeTest {
39
40 @Test
41 void testShift() {
42 doTestShift(Binary64Field.getInstance());
43 }
44
45 @Test
46 void testSpin() {
47 doTestSpin(Binary64Field.getInstance());
48 }
49
50 private <T extends CalculusFieldElement<T>> void doTestShift(final Field<T> field){
51 T zero = field.getZero();
52 T one = field.getOne();
53 T rate = one.multiply(2 * FastMath.PI / (12 * 60));
54 FieldAttitude<T> attitude = new FieldAttitude<>(new FieldAbsoluteDate<>(field), FramesFactory.getEME2000(),
55 new FieldRotation<>(one, zero, zero, zero, false),
56 new FieldVector3D<>(rate, new FieldVector3D<>(zero, zero, one)), new FieldVector3D<>(zero, zero, zero));
57 Assertions.assertEquals(rate.getReal(), attitude.getSpin().getNorm().getReal(), 1.0e-10);
58 double dt_R = 10.0;
59 T dt = zero.add(dt_R);
60
61 T alpha = rate.multiply(dt);
62 FieldAttitude<T> shifted = attitude.shiftedBy(dt);
63 Assertions.assertEquals(rate.getReal(), shifted.getSpin().getNorm().getReal(), 1.0e-10);
64 Assertions.assertEquals(alpha.getReal(), FieldRotation.distance(attitude.getRotation(), shifted.getRotation()).getReal(), 1.0e-10);
65
66 FieldVector3D<T> xSat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_I);
67 Assertions.assertEquals(0.0, xSat.subtract(new FieldVector3D<>(alpha.cos(), alpha.sin(), zero)).getNorm().getReal(), 1.0e-10);
68 FieldVector3D<T> ySat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_J);
69 Assertions.assertEquals(0.0, ySat.subtract(new FieldVector3D<>(alpha.sin().multiply(-1), alpha.cos(), zero)).getNorm().getReal(), 1.0e-10);
70 FieldVector3D<T> zSat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_K);
71 Assertions.assertEquals(0.0, zSat.subtract(Vector3D.PLUS_K).getNorm().getReal(), 1.0e-10);
72
73 }
74
75
76 private <T extends CalculusFieldElement<T>> void doTestSpin(final Field<T> field) {
77 T zero = field.getZero();
78 T rate = zero.add(2 * FastMath.PI / (12 * 60));
79 FieldAttitude<T> attitude = new FieldAttitude<>(new FieldAbsoluteDate<>(field), FramesFactory.getEME2000(),
80 new FieldRotation<>(zero.add(0.48), zero.add(0.64), zero.add(0.36), zero.add(0.48), false),
81 new FieldVector3D<>(rate, FieldVector3D.getPlusK(field)), FieldVector3D.getZero(field));
82 Assertions.assertEquals(rate.getReal(), attitude.getSpin().getNorm().getReal(), 1.0e-10);
83 T dt = zero.add(10.0);
84 FieldAttitude<T> shifted = attitude.shiftedBy(dt);
85 Assertions.assertEquals(rate.getReal(), shifted.getSpin().getNorm().getReal(), 1.0e-10);
86
87 FieldVector3D<T> shiftedX = shifted.getRotation().applyInverseTo(Vector3D.PLUS_I);
88 FieldVector3D<T> shiftedY = shifted.getRotation().applyInverseTo(Vector3D.PLUS_J);
89 FieldVector3D<T> shiftedZ = shifted.getRotation().applyInverseTo(Vector3D.PLUS_K);
90 FieldVector3D<T> originalX = attitude.getRotation().applyInverseTo(Vector3D.PLUS_I);
91 FieldVector3D<T> originalY = attitude.getRotation().applyInverseTo(Vector3D.PLUS_J);
92 FieldVector3D<T> originalZ = attitude.getRotation().applyInverseTo(Vector3D.PLUS_K);
93 Assertions.assertEquals( FastMath.cos(rate.getReal() * dt.getReal()), FieldVector3D.dotProduct(shiftedX, originalX).getReal(), 1.0e-10);
94 Assertions.assertEquals( FastMath.sin(rate.getReal() * dt.getReal()), FieldVector3D.dotProduct(shiftedX, originalY).getReal(), 1.0e-10);
95 Assertions.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedX, originalZ).getReal(), 1.0e-10);
96 Assertions.assertEquals(-FastMath.sin(rate.getReal() * dt.getReal()), FieldVector3D.dotProduct(shiftedY, originalX).getReal(), 1.0e-10);
97 Assertions.assertEquals( FastMath.cos(rate.getReal() * dt.getReal()), FieldVector3D.dotProduct(shiftedY, originalY).getReal(), 1.0e-10);
98 Assertions.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedY, originalZ).getReal(), 1.0e-10);
99 Assertions.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedZ, originalX).getReal(), 1.0e-10);
100 Assertions.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedZ, originalY).getReal(), 1.0e-10);
101 Assertions.assertEquals( 1.0, FieldVector3D.dotProduct(shiftedZ, originalZ).getReal(), 1.0e-10);
102
103 FieldVector3D<T> forward = FieldAngularCoordinates.estimateRate(attitude.getRotation(), shifted.getRotation(), dt);
104 Assertions.assertEquals(0.0, forward.subtract(attitude.getSpin()).getNorm().getReal(), 1.0e-10);
105
106 FieldVector3D<T> reversed = FieldAngularCoordinates.estimateRate(shifted.getRotation(), attitude.getRotation(), dt);
107 Assertions.assertEquals(0.0, reversed.add(attitude.getSpin()).getNorm().getReal(), 1.0e-10);
108
109 }
110
111 @Test
112 void testWithReferenceFrame() {
113
114 final UnivariateDerivative1Field field = UnivariateDerivative1Field.getInstance();
115 final FieldAbsoluteDate<UnivariateDerivative1> date = new FieldAbsoluteDate<>(field).shiftedBy(new UnivariateDerivative1(0, 1));
116 final FieldAngularCoordinates<UnivariateDerivative1> angularCoordinates = new FieldAngularCoordinates<>(FieldRotation.getIdentity(field),
117 FieldVector3D.getZero(field));
118 final Frame frame = FramesFactory.getEME2000();
119 final FieldAttitude<UnivariateDerivative1> attitude = new FieldAttitude<>(date, frame, angularCoordinates);
120 final Frame mockedFrame = Mockito.mock(Frame.class);
121 Mockito.when(mockedFrame.getTransformTo(frame, date)).thenReturn(new FieldTransform<>(date,
122 FieldRotation.getIdentity(field), FieldVector3D.getPlusI(field)));
123
124 final FieldAttitude<UnivariateDerivative1> attitudeInNonInertialFrame = attitude.withReferenceFrame(mockedFrame);
125
126 Assertions.assertEquals(1., attitudeInNonInertialFrame.getSpin().getX().getReal());
127 }
128
129 }
130