1   /* Copyright 2002-2022 CS GROUP
2    * Licensed to CS GROUP (CS) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * CS licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *   http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  package org.orekit.attitudes;
18  
19  
20  import org.hamcrest.MatcherAssert;
21  import org.hipparchus.Field;
22  import org.hipparchus.CalculusFieldElement;
23  import org.hipparchus.geometry.euclidean.threed.Rotation;
24  import org.hipparchus.geometry.euclidean.threed.RotationConvention;
25  import org.hipparchus.geometry.euclidean.threed.Vector3D;
26  import org.hipparchus.util.Decimal64;
27  import org.hipparchus.util.Decimal64Field;
28  import org.hipparchus.util.FastMath;
29  import org.junit.After;
30  import org.junit.Assert;
31  import org.junit.Before;
32  import org.junit.Test;
33  import org.orekit.Utils;
34  import org.orekit.errors.OrekitException;
35  import org.orekit.frames.Frame;
36  import org.orekit.frames.FramesFactory;
37  import org.orekit.frames.Transform;
38  import org.orekit.orbits.FieldCartesianOrbit;
39  import org.orekit.orbits.FieldOrbit;
40  import org.orekit.orbits.KeplerianOrbit;
41  import org.orekit.orbits.Orbit;
42  import org.orekit.orbits.PositionAngle;
43  import org.orekit.propagation.FieldSpacecraftState;
44  import org.orekit.propagation.Propagator;
45  import org.orekit.propagation.SpacecraftState;
46  import org.orekit.propagation.analytical.KeplerianPropagator;
47  import org.orekit.time.AbsoluteDate;
48  import org.orekit.time.DateComponents;
49  import org.orekit.time.FieldAbsoluteDate;
50  import org.orekit.time.TimeComponents;
51  import org.orekit.time.TimeScalesFactory;
52  import org.orekit.utils.AngularCoordinates;
53  import org.orekit.utils.FieldPVCoordinates;
54  import org.orekit.utils.FieldPVCoordinatesProvider;
55  
56  import static org.hamcrest.CoreMatchers.is;
57  import static org.hamcrest.CoreMatchers.not;
58  import static org.orekit.OrekitMatchers.attitudeIs;
59  import static org.orekit.OrekitMatchers.closeTo;
60  import static org.orekit.OrekitMatchers.distanceIs;
61  
62  
63  public class InertialAttitudeTest {
64  
65      private AbsoluteDate t0;
66      private Orbit        orbit0;
67  
68      @Test
69      public void testIsInertial() {
70          InertialProvider law = new InertialProvider(new Rotation(new Vector3D(0.6, 0.48, 0.64), 0.9, RotationConvention.VECTOR_OPERATOR));
71          KeplerianPropagator propagator = new KeplerianPropagator(orbit0, law);
72          Attitude initial = propagator.propagate(t0).getAttitude();
73          for (double t = 0; t < 10000.0; t += 100) {
74              SpacecraftState state = propagator.propagate(t0.shiftedBy(t));
75              checkField(Decimal64Field.getInstance(), law, state.getOrbit(), state.getDate(), state.getFrame());
76              Attitude attitude = state.getAttitude();
77              Rotation evolution = attitude.getRotation().compose(initial.getRotation().revert(),
78                                                                  RotationConvention.VECTOR_OPERATOR);
79              Assert.assertEquals(0, evolution.getAngle(), 1.0e-10);
80              Assert.assertEquals(FramesFactory.getEME2000(), attitude.getReferenceFrame());
81          }
82      }
83  
84      @Test
85      public void testCompensateMomentum() {
86          InertialProvider law = new InertialProvider(new Rotation(new Vector3D(-0.64, 0.6, 0.48), 0.2, RotationConvention.VECTOR_OPERATOR));
87          KeplerianPropagator propagator = new KeplerianPropagator(orbit0, law);
88          Attitude initial = propagator.propagate(t0).getAttitude();
89          for (double t = 0; t < 10000.0; t += 100) {
90              Attitude attitude = propagator.propagate(t0.shiftedBy(t)).getAttitude();
91              Rotation evolution = attitude.getRotation().compose(initial.getRotation().revert(),
92                                                                  RotationConvention.VECTOR_OPERATOR);
93              Assert.assertEquals(0, evolution.getAngle(), 1.0e-10);
94              Assert.assertEquals(FramesFactory.getEME2000(), attitude.getReferenceFrame());
95          }
96      }
97  
98      @Test
99      public void testSpin() {
100 
101         AbsoluteDate date = new AbsoluteDate(new DateComponents(1970, 01, 01),
102                                              new TimeComponents(3, 25, 45.6789),
103                                              TimeScalesFactory.getUTC());
104 
105         AttitudeProvider law = new InertialProvider(new Rotation(new Vector3D(-0.64, 0.6, 0.48), 0.2, RotationConvention.VECTOR_OPERATOR));
106 
107         KeplerianOrbit orbit =
108             new KeplerianOrbit(7178000.0, 1.e-4, FastMath.toRadians(50.),
109                               FastMath.toRadians(10.), FastMath.toRadians(20.),
110                               FastMath.toRadians(30.), PositionAngle.MEAN,
111                               FramesFactory.getEME2000(), date, 3.986004415e14);
112 
113         Propagator propagator = new KeplerianPropagator(orbit, law);
114 
115         double h = 100.0;
116         SpacecraftState sMinus = propagator.propagate(date.shiftedBy(-h));
117         SpacecraftState s0     = propagator.propagate(date);
118         SpacecraftState sPlus  = propagator.propagate(date.shiftedBy(h));
119 
120         // check spin is consistent with attitude evolution
121         double errorAngleMinus     = Rotation.distance(sMinus.shiftedBy(h).getAttitude().getRotation(),
122                                                        s0.getAttitude().getRotation());
123         double evolutionAngleMinus = Rotation.distance(sMinus.getAttitude().getRotation(),
124                                                        s0.getAttitude().getRotation());
125         Assert.assertEquals(0.0, errorAngleMinus, 1.0e-6 * evolutionAngleMinus);
126         double errorAnglePlus      = Rotation.distance(s0.getAttitude().getRotation(),
127                                                        sPlus.shiftedBy(-h).getAttitude().getRotation());
128         double evolutionAnglePlus  = Rotation.distance(s0.getAttitude().getRotation(),
129                                                        sPlus.getAttitude().getRotation());
130         Assert.assertEquals(0.0, errorAnglePlus, 1.0e-6 * evolutionAnglePlus);
131 
132         // compute spin axis using finite differences
133         Rotation rMinus = sMinus.getAttitude().getRotation();
134         Rotation rPlus  = sPlus.getAttitude().getRotation();
135         Rotation dr     = rPlus.compose(rMinus.revert(), RotationConvention.VECTOR_OPERATOR);
136         Assert.assertEquals(0, dr.getAngle(), 1.0e-10);
137 
138         Vector3D spin0 = s0.getAttitude().getSpin();
139         Assert.assertEquals(0, spin0.getNorm(), 1.0e-10);
140 
141     }
142 
143     private <T extends CalculusFieldElement<T>> void checkField(final Field<T> field, final AttitudeProvider provider,
144                                                             final Orbit orbit, final AbsoluteDate date,
145                                                             final Frame frame)
146         {
147         Attitude attitudeD = provider.getAttitude(orbit, date, frame);
148         final FieldOrbit<T> orbitF = new FieldSpacecraftState<>(field, new SpacecraftState(orbit)).getOrbit();
149         final FieldAbsoluteDate<T> dateF = new FieldAbsoluteDate<>(field, date);
150         FieldAttitude<T> attitudeF = provider.getAttitude(orbitF, dateF, frame);
151         Assert.assertEquals(0.0, Rotation.distance(attitudeD.getRotation(), attitudeF.getRotation().toRotation()), 1.0e-15);
152         Assert.assertEquals(0.0, Vector3D.distance(attitudeD.getSpin(), attitudeF.getSpin().toVector3D()), 1.0e-15);
153         Assert.assertEquals(0.0, Vector3D.distance(attitudeD.getRotationAcceleration(), attitudeF.getRotationAcceleration().toVector3D()), 1.0e-15);
154     }
155 
156     @Test
157     public void testGetAttitude() {
158         // expected
159         Frame eci = orbit0.getFrame();
160         Attitude expected = new Attitude(t0, eci, AngularCoordinates.IDENTITY);
161         AttitudeProvider law = InertialProvider.of(eci);
162 
163         // action + verify
164         Attitude actual = law.getAttitude(orbit0, t0, eci);
165         MatcherAssert.assertThat(actual.getReferenceFrame(), is(eci));
166         MatcherAssert.assertThat(actual, attitudeIs(expected));
167         actual = law.getAttitude(orbit0.shiftedBy(1e3), t0.shiftedBy(1e3), eci);
168         MatcherAssert.assertThat(actual.getReferenceFrame(), is(eci));
169         MatcherAssert.assertThat(actual, attitudeIs(expected));
170         // create new frame for testing frame transforms
171         Rotation rotation = new Rotation(
172                 Vector3D.PLUS_K,
173                 FastMath.PI / 2.0,
174                 RotationConvention.FRAME_TRANSFORM);
175         Transform angular = new Transform(
176                 t0,
177                 rotation,
178                 new Vector3D(1, 2, 3),
179                 new Vector3D(-4, 5, 6));
180         Transform translation = new Transform(
181                 t0,
182                 new Vector3D(-1, 2, -3),
183                 new Vector3D(4, -5, 6),
184                 new Vector3D(7, 8, -9));
185         Frame other = new Frame(eci, new Transform(t0, angular, translation), "other");
186         actual = law.getAttitude(orbit0.shiftedBy(1e3), t0.shiftedBy(1e3), other);
187         MatcherAssert.assertThat(actual.getReferenceFrame(), is(other));
188         MatcherAssert.assertThat(actual, attitudeIs(expected));
189         // check not identity rotation
190         MatcherAssert.assertThat(actual.getRotation(),
191                 not(distanceIs(Rotation.IDENTITY, closeTo(0.0, 1e-1))));
192     }
193 
194 
195     /**
196      * Unit tests for {@link FrameAligned#getAttitude(FieldPVCoordinatesProvider,
197      * FieldAbsoluteDate, Frame)}.
198      */
199     @Test
200     public void testGetAttitudeField() {
201         // expected
202         Frame eci = orbit0.getFrame();
203         Attitude expected = new Attitude(t0, eci, AngularCoordinates.IDENTITY);
204         AttitudeProvider law = InertialProvider.of(eci);
205         Decimal64 one = Decimal64.ONE;
206         FieldAbsoluteDate<Decimal64> date = new FieldAbsoluteDate<>(one.getField(), t0);
207         FieldOrbit<Decimal64> orbit = new FieldCartesianOrbit<>(
208                 new FieldPVCoordinates<>(one, this.orbit0.getPVCoordinates()),
209                 eci,
210                 date,
211                 one.multiply(orbit0.getMu()));
212 
213         // action + verify
214         FieldAttitude<Decimal64> actual = law.getAttitude(orbit, date, eci);
215         MatcherAssert.assertThat(actual.getReferenceFrame(), is(eci));
216         MatcherAssert.assertThat(actual.toAttitude(), attitudeIs(expected));
217         actual = law.getAttitude(orbit.shiftedBy(1e3), date.shiftedBy(1e3), eci);
218         MatcherAssert.assertThat(actual.getReferenceFrame(), is(eci));
219         MatcherAssert.assertThat(actual.toAttitude(), attitudeIs(expected));
220         // create new frame for testing frame transforms
221         Rotation rotation = new Rotation(
222                 Vector3D.PLUS_K,
223                 FastMath.PI / 2.0,
224                 RotationConvention.FRAME_TRANSFORM);
225         Transform angular = new Transform(
226                 t0,
227                 rotation,
228                 new Vector3D(1, 2, 3),
229                 new Vector3D(-4, 5, 6));
230         Transform translation = new Transform(
231                 t0,
232                 new Vector3D(-1, 2, -3),
233                 new Vector3D(4, -5, 6),
234                 new Vector3D(7, 8, -9));
235         Frame other = new Frame(eci, new Transform(t0, angular, translation), "other");
236         actual = law.getAttitude(orbit.shiftedBy(1e3), date.shiftedBy(1e3), other);
237         MatcherAssert.assertThat(actual.getReferenceFrame(), is(other));
238         MatcherAssert.assertThat(actual.toAttitude(), attitudeIs(expected));
239         // check not identity rotation
240         MatcherAssert.assertThat(actual.getRotation().toRotation(),
241                 not(distanceIs(Rotation.IDENTITY, closeTo(0.0, 1e-1))));
242     }
243 
244 
245 
246     @Before
247     public void setUp() {
248         try {
249             Utils.setDataRoot("regular-data");
250 
251             t0 = new AbsoluteDate(new DateComponents(2008, 06, 03), TimeComponents.H12,
252                                   TimeScalesFactory.getUTC());
253             orbit0 =
254                 new KeplerianOrbit(12345678.9, 0.001, 2.3, 0.1, 3.04, 2.4,
255                                    PositionAngle.TRUE, FramesFactory.getEME2000(),
256                                    t0, 3.986004415e14);
257         } catch (OrekitException oe) {
258             Assert.fail(oe.getMessage());
259         }
260     }
261 
262     @After
263     public void tearDown() {
264         t0     = null;
265         orbit0 = null;
266     }
267 
268 }
269