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  import org.hipparchus.CalculusFieldElement;
20  import org.hipparchus.Field;
21  import org.hipparchus.geometry.euclidean.threed.Rotation;
22  import org.hipparchus.geometry.euclidean.threed.RotationConvention;
23  import org.hipparchus.geometry.euclidean.threed.RotationOrder;
24  import org.hipparchus.geometry.euclidean.threed.Vector3D;
25  import org.hipparchus.util.Decimal64Field;
26  import org.hipparchus.util.FastMath;
27  import org.junit.jupiter.api.AfterEach;
28  import org.junit.jupiter.api.Assertions;
29  import org.junit.jupiter.api.BeforeEach;
30  import org.junit.jupiter.api.Test;
31  import org.orekit.Utils;
32  import org.orekit.bodies.GeodeticPoint;
33  import org.orekit.bodies.OneAxisEllipsoid;
34  import org.orekit.errors.OrekitException;
35  import org.orekit.frames.Frame;
36  import org.orekit.frames.FramesFactory;
37  import org.orekit.frames.LOFType;
38  import org.orekit.orbits.CircularOrbit;
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.IERSConventions;
54  import org.orekit.utils.PVCoordinates;
55  
56  
57  public class LofOffsetTest {
58  
59      // Computation date
60      private AbsoluteDate date;
61  
62      // Body mu
63      private double mu;
64  
65      // Reference frame = ITRF
66      private Frame itrf;
67  
68      // Earth shape
69      OneAxisEllipsoid earthSpheric;
70  
71      //  Satellite position
72      CircularOrbit orbit;
73      PVCoordinates pvSatEME2000;
74  
75      /** Test is the lof offset is the one expected
76       */
77      @Test
78      public void testZero() {
79  
80          //  Satellite position
81  
82          // Lof aligned attitude provider
83          final LofOffset lofAlignedLaw = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS);
84          final Rotation lofOffsetRot = lofAlignedLaw.getAttitude(orbit, date, orbit.getFrame()).getRotation();
85  
86          // Check that
87          final Vector3D momentumEME2000 = pvSatEME2000.getMomentum();
88          final Vector3D momentumLof = lofOffsetRot.applyTo(momentumEME2000);
89          final double cosinus = FastMath.cos(Vector3D.dotProduct(momentumLof, Vector3D.PLUS_K));
90          Assertions.assertEquals(1., cosinus, Utils.epsilonAngle);
91  
92      }
93      /** Test if the lof offset is the one expected
94       */
95      @Test
96      public void testOffset() {
97  
98          //  Satellite position
99          final CircularOrbit circ =
100            new CircularOrbit(7178000.0, 0.5e-4, -0.5e-4, FastMath.toRadians(0.), FastMath.toRadians(270.),
101                                    FastMath.toRadians(5.300), PositionAngle.MEAN,
102                                    FramesFactory.getEME2000(), date, mu);
103 
104         // Create target pointing attitude provider
105         // ************************************
106         // Elliptic earth shape
107         final OneAxisEllipsoid earthShape = new OneAxisEllipsoid(6378136.460, 1 / 298.257222101, itrf);
108         final GeodeticPoint geoTargetITRF = new GeodeticPoint(FastMath.toRadians(43.36), FastMath.toRadians(1.26), 600.);
109 
110         // Attitude law definition from geodetic point target
111         final TargetPointing targetLaw = new TargetPointing(circ.getFrame(), geoTargetITRF, earthShape);
112         final Rotation targetRot = targetLaw.getAttitude(circ, date, circ.getFrame()).getRotation();
113 
114         // Create lof aligned attitude provider
115         // *******************************
116         final LofOffset lofAlignedLaw = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS);
117         final Rotation lofAlignedRot = lofAlignedLaw.getAttitude(circ, date, circ.getFrame()).getRotation();
118 
119         // Get rotation from LOF to target pointing attitude
120         Rotation rollPitchYaw = targetRot.compose(lofAlignedRot.revert(), RotationConvention.VECTOR_OPERATOR).revert();
121         final double[] angles = rollPitchYaw.getAngles(RotationOrder.ZYX, RotationConvention.VECTOR_OPERATOR);
122         final double yaw = angles[0];
123         final double pitch = angles[1];
124         final double roll = angles[2];
125 
126         // Create lof offset attitude provider with computed roll, pitch, yaw
127         // **************************************************************
128         final LofOffset lofOffsetLaw = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS, RotationOrder.ZYX, yaw, pitch, roll);
129         final Rotation lofOffsetRot = lofOffsetLaw.getAttitude(circ, date, circ.getFrame()).getRotation();
130 
131         // Compose rotations : target pointing attitudes
132         final double angleCompo = targetRot.composeInverse(lofOffsetRot, RotationConvention.VECTOR_OPERATOR).getAngle();
133         Assertions.assertEquals(0., angleCompo, Utils.epsilonAngle);
134 
135     }
136 
137     /** Test is the target pointed is the one expected
138      */
139     @Test
140     public void testTarget()
141         {
142 
143         // Create target point and target pointing law towards that point
144         final GeodeticPoint targetDef  = new GeodeticPoint(FastMath.toRadians(5.), FastMath.toRadians(-40.), 0.);
145         final TargetPointing targetLaw = new TargetPointing(orbit.getFrame(), targetDef, earthSpheric);
146 
147         // Get roll, pitch, yaw angles corresponding to this pointing law
148         final LofOffset lofAlignedLaw = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS);
149         final Rotation lofAlignedRot = lofAlignedLaw.getAttitude(orbit, date, orbit.getFrame()).getRotation();
150         final Attitude targetAttitude = targetLaw.getAttitude(orbit, date, orbit.getFrame());
151         final Rotation rollPitchYaw = targetAttitude.getRotation().compose(lofAlignedRot.revert(), RotationConvention.VECTOR_OPERATOR).revert();
152         final double[] angles = rollPitchYaw.getAngles(RotationOrder.ZYX, RotationConvention.VECTOR_OPERATOR);
153         final double yaw   = angles[0];
154         final double pitch = angles[1];
155         final double roll  = angles[2];
156 
157         // Create a lof offset law from those values
158         final LofOffset lofOffsetLaw = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS, RotationOrder.ZYX, yaw, pitch, roll);
159         final LofOffsetPointing lofOffsetPtLaw = new LofOffsetPointing(orbit.getFrame(), earthSpheric, lofOffsetLaw, Vector3D.PLUS_K);
160 
161         // Check target pointed by this law : shall be the same as defined
162         final Vector3D pTargetRes =
163                 lofOffsetPtLaw.getTargetPV(orbit, date, earthSpheric.getBodyFrame()).getPosition();
164         final GeodeticPoint targetRes = earthSpheric.transform(pTargetRes, earthSpheric.getBodyFrame(), date);
165 
166         Assertions.assertEquals(targetDef.getLongitude(), targetRes.getLongitude(), Utils.epsilonAngle);
167         Assertions.assertEquals(targetDef.getLongitude(), targetRes.getLongitude(), Utils.epsilonAngle);
168 
169     }
170 
171     @Test
172     public void testSpin() {
173 
174         final AttitudeProvider law = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS, RotationOrder.XYX, 0.1, 0.2, 0.3);
175 
176         AbsoluteDate date = new AbsoluteDate(new DateComponents(1970, 01, 01),
177                                              new TimeComponents(3, 25, 45.6789),
178                                              TimeScalesFactory.getUTC());
179         KeplerianOrbit orbit =
180             new KeplerianOrbit(7178000.0, 1.e-4, FastMath.toRadians(50.),
181                               FastMath.toRadians(10.), FastMath.toRadians(20.),
182                               FastMath.toRadians(30.), PositionAngle.MEAN,
183                               FramesFactory.getEME2000(), date, 3.986004415e14);
184 
185         Propagator propagator = new KeplerianPropagator(orbit, law);
186 
187         double h = 0.01;
188         SpacecraftState sMinus = propagator.propagate(date.shiftedBy(-h));
189         SpacecraftState s0     = propagator.propagate(date);
190         SpacecraftState sPlus  = propagator.propagate(date.shiftedBy(h));
191 
192         // check spin is consistent with attitude evolution
193         double errorAngleMinus     = Rotation.distance(sMinus.shiftedBy(h).getAttitude().getRotation(),
194                                                        s0.getAttitude().getRotation());
195         double evolutionAngleMinus = Rotation.distance(sMinus.getAttitude().getRotation(),
196                                                        s0.getAttitude().getRotation());
197         Assertions.assertEquals(0.0, errorAngleMinus, 1.0e-6 * evolutionAngleMinus);
198         double errorAnglePlus      = Rotation.distance(s0.getAttitude().getRotation(),
199                                                        sPlus.shiftedBy(-h).getAttitude().getRotation());
200         double evolutionAnglePlus  = Rotation.distance(s0.getAttitude().getRotation(),
201                                                        sPlus.getAttitude().getRotation());
202         Assertions.assertEquals(0.0, errorAnglePlus, 1.0e-6 * evolutionAnglePlus);
203 
204         Vector3D spin0 = s0.getAttitude().getSpin();
205         Vector3D reference = AngularCoordinates.estimateRate(sMinus.getAttitude().getRotation(),
206                                                              sPlus.getAttitude().getRotation(),
207                                                              2 * h);
208         Assertions.assertEquals(0.0, spin0.subtract(reference).getNorm(), 1.0e-10);
209 
210     }
211 
212     @Test
213     public void testAnglesSign() {
214 
215         AbsoluteDate date = new AbsoluteDate(new DateComponents(1970, 01, 01),
216                                              new TimeComponents(3, 25, 45.6789),
217                                              TimeScalesFactory.getUTC());
218         KeplerianOrbit orbit =
219             new KeplerianOrbit(7178000.0, 1.e-8, FastMath.toRadians(50.),
220                               FastMath.toRadians(10.), FastMath.toRadians(20.),
221                               FastMath.toRadians(0.), PositionAngle.MEAN,
222                               FramesFactory.getEME2000(), date, 3.986004415e14);
223 
224         double alpha = 0.1;
225         double cos = FastMath.cos(alpha);
226         double sin = FastMath.sin(alpha);
227 
228         // Roll
229         Attitude attitude = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS, RotationOrder.XYZ, alpha, 0.0, 0.0).getAttitude(orbit, date, orbit.getFrame());
230         checkSatVector(orbit, attitude, Vector3D.PLUS_I,  1.0,  0.0,  0.0, 1.0e-8);
231         checkSatVector(orbit, attitude, Vector3D.PLUS_J,  0.0,  cos,  sin, 1.0e-8);
232         checkSatVector(orbit, attitude, Vector3D.PLUS_K,  0.0, -sin,  cos, 1.0e-8);
233 
234         // Pitch
235         attitude = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS, RotationOrder.XYZ, 0.0, alpha, 0.0).getAttitude(orbit, date, orbit.getFrame());
236         checkSatVector(orbit, attitude, Vector3D.PLUS_I,  cos,  0.0, -sin, 1.0e-8);
237         checkSatVector(orbit, attitude, Vector3D.PLUS_J,  0.0,  1.0,  0.0, 1.0e-8);
238         checkSatVector(orbit, attitude, Vector3D.PLUS_K,  sin,  0.0,  cos, 1.0e-8);
239 
240         // Yaw
241         attitude = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS, RotationOrder.XYZ, 0.0, 0.0, alpha).getAttitude(orbit, date, orbit.getFrame());
242         checkSatVector(orbit, attitude, Vector3D.PLUS_I,  cos,  sin,  0.0, 1.0e-8);
243         checkSatVector(orbit, attitude, Vector3D.PLUS_J, -sin,  cos,  0.0, 1.0e-8);
244         checkSatVector(orbit, attitude, Vector3D.PLUS_K,  0.0,  0.0,  1.0, 1.0e-8);
245 
246     }
247 
248     @Test
249     public void testRetrieveAngles() {
250         AbsoluteDate date = new AbsoluteDate(new DateComponents(1970, 01, 01),
251                                              new TimeComponents(3, 25, 45.6789),
252                                              TimeScalesFactory.getUTC());
253         KeplerianOrbit orbit =
254             new KeplerianOrbit(7178000.0, 1.e-4, FastMath.toRadians(50.),
255                               FastMath.toRadians(10.), FastMath.toRadians(20.),
256                               FastMath.toRadians(30.), PositionAngle.MEAN,
257                               FramesFactory.getEME2000(), date, 3.986004415e14);
258 
259         RotationOrder order = RotationOrder.ZXY;
260         double alpha1 = 0.123;
261         double alpha2 = 0.456;
262         double alpha3 = 0.789;
263         LofOffset law = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS, order, alpha1, alpha2, alpha3);
264         Rotation offsetAtt  = law.getAttitude(orbit, date, orbit.getFrame()).getRotation();
265         Rotation alignedAtt = new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS).getAttitude(orbit, date, orbit.getFrame()).getRotation();
266         Rotation offsetProper = offsetAtt.compose(alignedAtt.revert(), RotationConvention.VECTOR_OPERATOR);
267         double[] anglesV = offsetProper.revert().getAngles(order, RotationConvention.VECTOR_OPERATOR);
268         Assertions.assertEquals(alpha1, anglesV[0], 1.0e-11);
269         Assertions.assertEquals(alpha2, anglesV[1], 1.0e-11);
270         Assertions.assertEquals(alpha3, anglesV[2], 1.0e-11);
271         double[] anglesF = offsetProper.getAngles(order, RotationConvention.FRAME_TRANSFORM);
272         Assertions.assertEquals(alpha1, anglesF[0], 1.0e-11);
273         Assertions.assertEquals(alpha2, anglesF[1], 1.0e-11);
274         Assertions.assertEquals(alpha3, anglesF[2], 1.0e-11);
275     }
276 
277     @Test
278     public void testTypesField() {
279         AbsoluteDate date = new AbsoluteDate(new DateComponents(1970, 01, 01),
280                                              new TimeComponents(3, 25, 45.6789),
281                                              TimeScalesFactory.getUTC());
282         KeplerianOrbit orbit =
283             new KeplerianOrbit(7178000.0, 1.e-4, FastMath.toRadians(50.),
284                               FastMath.toRadians(10.), FastMath.toRadians(20.),
285                               FastMath.toRadians(30.), PositionAngle.MEAN,
286                               FramesFactory.getEME2000(), date, 3.986004415e14);
287 
288         for (final LOFType type : LOFType.values()) {
289             RotationOrder order = RotationOrder.ZXY;
290             double alpha1 = 0.123;
291             double alpha2 = 0.456;
292             double alpha3 = 0.789;
293             LofOffset law = new LofOffset(orbit.getFrame(), type, order, alpha1, alpha2, alpha3);
294             checkField(Decimal64Field.getInstance(), law, orbit, date, orbit.getFrame());
295         }
296     }
297 
298     private void checkSatVector(Orbit o, Attitude a, Vector3D satVector,
299                                 double expectedX, double expectedY, double expectedZ,
300                                 double threshold) {
301         Vector3D zLof = o.getPVCoordinates().getPosition().normalize().negate();
302         Vector3D yLof = o.getPVCoordinates().getMomentum().normalize().negate();
303         Vector3D xLof = Vector3D.crossProduct(yLof, zLof);
304         Assertions.assertTrue(Vector3D.dotProduct(xLof, o.getPVCoordinates().getVelocity()) > 0);
305         Vector3D v = a.getRotation().applyInverseTo(satVector);
306         Assertions.assertEquals(expectedX, Vector3D.dotProduct(v, xLof), 1.0e-8);
307         Assertions.assertEquals(expectedY, Vector3D.dotProduct(v, yLof), 1.0e-8);
308         Assertions.assertEquals(expectedZ, Vector3D.dotProduct(v, zLof), 1.0e-8);
309     }
310 
311     private <T extends CalculusFieldElement<T>> void checkField(final Field<T> field, final AttitudeProvider provider,
312                                                             final Orbit orbit, final AbsoluteDate date,
313                                                             final Frame frame)
314         {
315         Attitude attitudeD = provider.getAttitude(orbit, date, frame);
316         final FieldOrbit<T> orbitF = new FieldSpacecraftState<>(field, new SpacecraftState(orbit)).getOrbit();
317         final FieldAbsoluteDate<T> dateF = new FieldAbsoluteDate<>(field, date);
318         FieldAttitude<T> attitudeF = provider.getAttitude(orbitF, dateF, frame);
319         Assertions.assertEquals(0.0, Rotation.distance(attitudeD.getRotation(), attitudeF.getRotation().toRotation()), 1.0e-15);
320         Assertions.assertEquals(0.0, Vector3D.distance(attitudeD.getSpin(), attitudeF.getSpin().toVector3D()), 1.0e-15);
321         Assertions.assertEquals(0.0, Vector3D.distance(attitudeD.getRotationAcceleration(), attitudeF.getRotationAcceleration().toVector3D()), 1.0e-15);
322     }
323 
324     @BeforeEach
325     public void setUp() {
326         try {
327 
328             Utils.setDataRoot("regular-data");
329 
330             // Computation date
331             date = new AbsoluteDate(new DateComponents(2008, 04, 07),
332                                     TimeComponents.H00,
333                                     TimeScalesFactory.getUTC());
334 
335             // Body mu
336             mu = 3.9860047e14;
337 
338             // Reference frame = ITRF
339             itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
340 
341             // Elliptic earth shape
342             earthSpheric =
343                 new OneAxisEllipsoid(6378136.460, 0., itrf);
344 
345             //  Satellite position
346             orbit =
347                 new CircularOrbit(7178000.0, 0.5e-8, -0.5e-8, FastMath.toRadians(50.), FastMath.toRadians(150.),
348                                        FastMath.toRadians(5.300), PositionAngle.MEAN,
349                                        FramesFactory.getEME2000(), date, mu);
350             pvSatEME2000 = orbit.getPVCoordinates();
351 
352 
353         } catch (OrekitException oe) {
354             Assertions.fail(oe.getMessage());
355         }
356 
357     }
358 
359     @AfterEach
360     public void tearDown() {
361         date = null;
362         itrf = null;
363         earthSpheric = null;
364     }
365 
366 }
367