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.hipparchus.Field;
21  import org.hipparchus.CalculusFieldElement;
22  import org.hipparchus.geometry.euclidean.threed.FieldRotation;
23  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
24  import org.hipparchus.geometry.euclidean.threed.Rotation;
25  import org.hipparchus.geometry.euclidean.threed.Vector3D;
26  import org.hipparchus.util.Decimal64Field;
27  import org.hipparchus.util.FastMath;
28  import org.junit.Assert;
29  import org.junit.Before;
30  import org.junit.Test;
31  import org.orekit.Utils;
32  import org.orekit.frames.Frame;
33  import org.orekit.frames.FramesFactory;
34  import org.orekit.orbits.FieldKeplerianOrbit;
35  import org.orekit.orbits.FieldOrbit;
36  import org.orekit.orbits.KeplerianOrbit;
37  import org.orekit.orbits.Orbit;
38  import org.orekit.orbits.PositionAngle;
39  import org.orekit.propagation.FieldPropagator;
40  import org.orekit.propagation.FieldSpacecraftState;
41  import org.orekit.propagation.Propagator;
42  import org.orekit.propagation.SpacecraftState;
43  import org.orekit.propagation.analytical.FieldKeplerianPropagator;
44  import org.orekit.propagation.analytical.KeplerianPropagator;
45  import org.orekit.time.AbsoluteDate;
46  import org.orekit.time.DateComponents;
47  import org.orekit.time.FieldAbsoluteDate;
48  import org.orekit.time.TimeComponents;
49  import org.orekit.time.TimeScalesFactory;
50  import org.orekit.utils.AngularCoordinates;
51  import org.orekit.utils.FieldAngularCoordinates;
52  import org.orekit.utils.FieldPVCoordinates;
53  import org.orekit.utils.PVCoordinates;
54  
55  public class FixedRateTest {
56  
57      @Test
58      public void testZeroRate() {
59          AbsoluteDate date = new AbsoluteDate(new DateComponents(2004, 3, 2),
60                                               new TimeComponents(13, 17, 7.865),
61                                               TimeScalesFactory.getUTC());
62          final Frame frame = FramesFactory.getEME2000();
63          FixedRate law = new FixedRate(new Attitude(date, frame,
64                                                     new Rotation(0.48, 0.64, 0.36, 0.48, false),
65                                                     Vector3D.ZERO, Vector3D.ZERO));
66          PVCoordinates pv =
67              new PVCoordinates(new Vector3D(28812595.32012577, 5948437.4640250085, 0),
68                                new Vector3D(0, 0, 3680.853673522056));
69          Orbit orbit = new KeplerianOrbit(pv, frame, date, 3.986004415e14);
70          Rotation attitude0 = law.getAttitude(orbit, date, frame).getRotation();
71          Assert.assertEquals(0, Rotation.distance(attitude0, law.getReferenceAttitude().getRotation()), 1.0e-10);
72          Rotation attitude1 = law.getAttitude(orbit.shiftedBy(10.0), date.shiftedBy(10.0), frame).getRotation();
73          Assert.assertEquals(0, Rotation.distance(attitude1, law.getReferenceAttitude().getRotation()), 1.0e-10);
74          Rotation attitude2 = law.getAttitude(orbit.shiftedBy(20.0), date.shiftedBy(20.0), frame).getRotation();
75          Assert.assertEquals(0, Rotation.distance(attitude2, law.getReferenceAttitude().getRotation()), 1.0e-10);
76  
77      }
78  
79      @Test
80      public void testNonZeroRate() {
81          final AbsoluteDate date = new AbsoluteDate(new DateComponents(2004, 3, 2),
82                                                     new TimeComponents(13, 17, 7.865),
83                                                     TimeScalesFactory.getUTC());
84          final double rate = 2 * FastMath.PI / (12 * 60);
85          final Frame frame = FramesFactory.getEME2000();
86          final Frame gcrf  = FramesFactory.getGCRF();
87          FixedRate law = new FixedRate(new Attitude(date, frame,
88                                                     new Rotation(0.48, 0.64, 0.36, 0.48, false),
89                                                     new Vector3D(rate, Vector3D.PLUS_K), Vector3D.ZERO));
90          final Rotation ref = law.getReferenceAttitude().getRotation().applyTo(gcrf.getTransformTo(frame, date).getRotation());
91          PVCoordinates pv =
92              new PVCoordinates(new Vector3D(28812595.32012577, 5948437.4640250085, 0),
93                                new Vector3D(0, 0, 3680.853673522056));
94          Orbit orbit = new KeplerianOrbit(pv, FramesFactory.getEME2000(), date, 3.986004415e14);
95          Rotation attitude0 = law.getAttitude(orbit, date, gcrf).getRotation();
96          Assert.assertEquals(0, Rotation.distance(attitude0, ref), 1.0e-10);
97          Rotation attitude1 = law.getAttitude(orbit.shiftedBy(10.0), date.shiftedBy(10.0), gcrf).getRotation();
98          Assert.assertEquals(10 * rate, Rotation.distance(attitude1, ref), 1.0e-10);
99          Rotation attitude2 = law.getAttitude(orbit.shiftedBy(-20.0), date.shiftedBy(-20.0), gcrf).getRotation();
100         Assert.assertEquals(20 * rate, Rotation.distance(attitude2, ref), 1.0e-10);
101         Assert.assertEquals(30 * rate, Rotation.distance(attitude2, attitude1), 1.0e-10);
102         Rotation attitude3 = law.getAttitude(orbit.shiftedBy(0.0), date, frame).getRotation();
103         Assert.assertEquals(0, Rotation.distance(attitude3, law.getReferenceAttitude().getRotation()), 1.0e-10);
104 
105     }
106 
107     @Test
108     public void testSpin() {
109 
110         AbsoluteDate date = new AbsoluteDate(new DateComponents(1970, 01, 01),
111                                              new TimeComponents(3, 25, 45.6789),
112                                              TimeScalesFactory.getUTC());
113 
114         final double rate = 2 * FastMath.PI / (12 * 60);
115         AttitudeProvider law =
116             new FixedRate(new Attitude(date, FramesFactory.getEME2000(),
117                                        new Rotation(0.48, 0.64, 0.36, 0.48, false),
118                                        new Vector3D(rate, Vector3D.PLUS_K),
119                                        Vector3D.ZERO));
120 
121         KeplerianOrbit orbit =
122             new KeplerianOrbit(7178000.0, 1.e-4, FastMath.toRadians(50.),
123                               FastMath.toRadians(10.), FastMath.toRadians(20.),
124                               FastMath.toRadians(30.), PositionAngle.MEAN,
125                               FramesFactory.getEME2000(), date, 3.986004415e14);
126 
127         Propagator propagator = new KeplerianPropagator(orbit, law);
128 
129         double h = 0.01;
130         SpacecraftState sMinus = propagator.propagate(date.shiftedBy(-h));
131         SpacecraftState s0     = propagator.propagate(date);
132         SpacecraftState sPlus  = propagator.propagate(date.shiftedBy(h));
133 
134         // check spin is consistent with attitude evolution
135         double errorAngleMinus     = Rotation.distance(sMinus.shiftedBy(h).getAttitude().getRotation(),
136                                                        s0.getAttitude().getRotation());
137         double evolutionAngleMinus = Rotation.distance(sMinus.getAttitude().getRotation(),
138                                                        s0.getAttitude().getRotation());
139         Assert.assertEquals(0.0, errorAngleMinus, 1.0e-6 * evolutionAngleMinus);
140         double errorAnglePlus      = Rotation.distance(s0.getAttitude().getRotation(),
141                                                        sPlus.shiftedBy(-h).getAttitude().getRotation());
142         double evolutionAnglePlus  = Rotation.distance(s0.getAttitude().getRotation(),
143                                                        sPlus.getAttitude().getRotation());
144         Assert.assertEquals(0.0, errorAnglePlus, 1.0e-6 * evolutionAnglePlus);
145 
146         Vector3D spin0 = s0.getAttitude().getSpin();
147         Vector3D reference = AngularCoordinates.estimateRate(sMinus.getAttitude().getRotation(),
148                                                              sPlus.getAttitude().getRotation(),
149                                                              2 * h);
150         Assert.assertEquals(0.0, spin0.subtract(reference).getNorm(), 1.0e-14);
151 
152     }
153 
154     @Test
155     public void testZeroRateField() {
156         doTestZeroRate(Decimal64Field.getInstance());
157     }
158 
159     private <T extends CalculusFieldElement<T>> void doTestZeroRate(final Field<T> field)
160         {
161         final T zero = field.getZero();
162         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field,
163                                                             new DateComponents(2004, 3, 2),
164                                                             new TimeComponents(13, 17, 7.865),
165                                                             TimeScalesFactory.getUTC());
166         final Frame frame = FramesFactory.getEME2000();
167         final Frame gcrf  = FramesFactory.getGCRF();
168         FixedRate law = new FixedRate(new Attitude(date.toAbsoluteDate(), frame,
169                                                    new Rotation(0.48, 0.64, 0.36, 0.48, false),
170                                                    Vector3D.ZERO, Vector3D.ZERO));
171         final Rotation ref = law.getReferenceAttitude().getRotation().applyTo(gcrf.getTransformTo(frame, date.toAbsoluteDate()).getRotation());
172         FieldPVCoordinates<T> pv =
173             new FieldPVCoordinates<>(field.getOne(),
174                                      new PVCoordinates(new Vector3D(28812595.32012577, 5948437.4640250085, 0),
175                                                        new Vector3D(0, 0, 3680.853673522056)));
176         FieldOrbit<T> orbit = new FieldKeplerianOrbit<>(pv, frame, date, zero.add(3.986004415e14));
177         FieldRotation<T> attitude0 = law.getAttitude(orbit, date, gcrf).getRotation();
178         Assert.assertEquals(0, Rotation.distance(attitude0.toRotation(), ref), 1.0e-10);
179         FieldRotation<T> attitude1 = law.getAttitude(orbit.shiftedBy(zero.add(10.0)), date.shiftedBy(10.0), gcrf).getRotation();
180         Assert.assertEquals(0, Rotation.distance(attitude1.toRotation(), ref), 1.0e-10);
181         FieldRotation<T> attitude2 = law.getAttitude(orbit.shiftedBy(zero.add(20.0)), date.shiftedBy(20.0), gcrf).getRotation();
182         Assert.assertEquals(0, Rotation.distance(attitude2.toRotation(), ref), 1.0e-10);
183 
184     }
185 
186     @Test
187     public void testNonZeroRateField() {
188         doTestNonZeroRate(Decimal64Field.getInstance());
189     }
190 
191     private <T extends CalculusFieldElement<T>> void doTestNonZeroRate(final Field<T> field) {
192         final T zero = field.getZero();
193         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field,
194                                                             new DateComponents(2004, 3, 2),
195                                                             new TimeComponents(13, 17, 7.865),
196                                                             TimeScalesFactory.getUTC());
197         final T rate = zero.add(2 * FastMath.PI / (12 * 60));
198         final Frame frame = FramesFactory.getEME2000();
199         FixedRate law = new FixedRate(new Attitude(date.toAbsoluteDate(), frame,
200                                                    new Rotation(0.48, 0.64, 0.36, 0.48, false),
201                                                    new Vector3D(rate.getReal(), Vector3D.PLUS_K), Vector3D.ZERO));
202         FieldPVCoordinates<T> pv =
203                         new FieldPVCoordinates<>(field.getOne(),
204                                                  new PVCoordinates(new Vector3D(28812595.32012577, 5948437.4640250085, 0),
205                                                                    new Vector3D(0, 0, 3680.853673522056)));
206         FieldOrbit<T> orbit = new FieldKeplerianOrbit<>(pv, FramesFactory.getEME2000(), date, zero.add(3.986004415e14));
207         FieldRotation<T> attitude0 = law.getAttitude(orbit, date, frame).getRotation();
208         Assert.assertEquals(0, Rotation.distance(attitude0.toRotation(), law.getReferenceAttitude().getRotation()), 1.0e-10);
209         FieldRotation<T> attitude1 = law.getAttitude(orbit.shiftedBy(zero.add(10.0)), date.shiftedBy(10.0), frame).getRotation();
210         Assert.assertEquals(10 * rate.getReal(), Rotation.distance(attitude1.toRotation(), law.getReferenceAttitude().getRotation()), 1.0e-10);
211         FieldRotation<T> attitude2 = law.getAttitude(orbit.shiftedBy(zero.add(-20.0)), date.shiftedBy(-20.0), frame).getRotation();
212         Assert.assertEquals(20 * rate.getReal(), Rotation.distance(attitude2.toRotation(), law.getReferenceAttitude().getRotation()), 1.0e-10);
213         Assert.assertEquals(30 * rate.getReal(), Rotation.distance(attitude2.toRotation(), attitude1.toRotation()), 1.0e-10);
214         FieldRotation<T> attitude3 = law.getAttitude(orbit.shiftedBy(zero.add(0.0)), date, frame).getRotation();
215         Assert.assertEquals(0, Rotation.distance(attitude3.toRotation(), law.getReferenceAttitude().getRotation()), 1.0e-10);
216 
217     }
218 
219     @Test
220     public void testSpinField() {
221         doTestSpin(Decimal64Field.getInstance());
222     }
223 
224     private <T extends CalculusFieldElement<T>> void doTestSpin(final Field<T> field) {
225 
226         final T zero = field.getZero();
227         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field,
228                                                             new DateComponents(1970, 01, 01),
229                                                             new TimeComponents(3, 25, 45.6789),
230                                                             TimeScalesFactory.getUTC());
231 
232         final T rate = zero.add(2 * FastMath.PI / (12 * 60));
233         AttitudeProvider law =
234                         new FixedRate(new Attitude(date.toAbsoluteDate(), FramesFactory.getEME2000(),
235                                                    new Rotation(0.48, 0.64, 0.36, 0.48, false),
236                                                    new Vector3D(rate.getReal(), Vector3D.PLUS_K),
237                                                    Vector3D.ZERO));
238 
239         FieldKeplerianOrbit<T> orbit =
240             new FieldKeplerianOrbit<>(zero.add(7178000.0),
241                                       zero.add(1.e-4),
242                                       zero.add(FastMath.toRadians(50.)),
243                                       zero.add(FastMath.toRadians(10.)),
244                                       zero.add(FastMath.toRadians(20.)),
245                                       zero.add(FastMath.toRadians(30.)), PositionAngle.MEAN,
246                                       FramesFactory.getEME2000(), date, zero.add(3.986004415e14));
247 
248         FieldPropagator<T> propagator = new FieldKeplerianPropagator<>(orbit, law);
249 
250         T h = zero.add(0.01);
251         FieldSpacecraftState<T> sMinus = propagator.propagate(date.shiftedBy(h.negate()));
252         FieldSpacecraftState<T> s0     = propagator.propagate(date);
253         FieldSpacecraftState<T> sPlus  = propagator.propagate(date.shiftedBy(h));
254 
255         // check spin is consistent with attitude evolution
256         double errorAngleMinus     = FieldRotation.distance(sMinus.shiftedBy(h).getAttitude().getRotation(),
257                                                             s0.getAttitude().getRotation()).getReal();
258         double evolutionAngleMinus = FieldRotation.distance(sMinus.getAttitude().getRotation(),
259                                                             s0.getAttitude().getRotation()).getReal();
260         Assert.assertEquals(0.0, errorAngleMinus, 1.0e-6 * evolutionAngleMinus);
261         double errorAnglePlus      = FieldRotation.distance(s0.getAttitude().getRotation(),
262                                                             sPlus.shiftedBy(h.negate()).getAttitude().getRotation()).getReal();
263         double evolutionAnglePlus  = FieldRotation.distance(s0.getAttitude().getRotation(),
264                                                             sPlus.getAttitude().getRotation()).getReal();
265         Assert.assertEquals(0.0, errorAnglePlus, 1.0e-6 * evolutionAnglePlus);
266 
267         FieldVector3D<T> spin0 = s0.getAttitude().getSpin();
268         FieldVector3D<T> reference = FieldAngularCoordinates.estimateRate(sMinus.getAttitude().getRotation(),
269                                                                           sPlus.getAttitude().getRotation(),
270                                                                           h.multiply(2));
271         Assert.assertEquals(0.0, spin0.subtract(reference).getNorm().getReal(), 1.0e-14);
272 
273     }
274 
275     @Before
276     public void setUp() {
277         Utils.setDataRoot("regular-data");
278     }
279 
280 }
281