1   /* Copyright 2002-2023 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.utils;
18  
19  import org.hipparchus.analysis.differentiation.DerivativeStructure;
20  import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
21  import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
22  import org.hipparchus.geometry.euclidean.threed.FieldRotation;
23  import org.hipparchus.geometry.euclidean.threed.Rotation;
24  import org.hipparchus.geometry.euclidean.threed.Vector3D;
25  import org.hipparchus.random.RandomGenerator;
26  import org.hipparchus.random.Well1024a;
27  import org.hipparchus.util.FastMath;
28  import org.junit.jupiter.api.Assertions;
29  import org.junit.jupiter.api.Test;
30  import org.orekit.time.AbsoluteDate;
31  
32  public class TimeStampedAngularCoordinatesTest {
33  
34      @Test
35      public void testZeroRate() {
36          TimeStampedAngularCoordinates ac =
37                  new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH,
38                                                    new Rotation(0.48, 0.64, 0.36, 0.48, false),
39                                                    Vector3D.ZERO, Vector3D.ZERO);
40          Assertions.assertEquals(Vector3D.ZERO, ac.getRotationRate());
41          double dt = 10.0;
42          TimeStampedAngularCoordinates shifted = ac.shiftedBy(dt);
43          Assertions.assertEquals(Vector3D.ZERO, shifted.getRotationAcceleration());
44          Assertions.assertEquals(Vector3D.ZERO, shifted.getRotationRate());
45          Assertions.assertEquals(0.0, Rotation.distance(ac.getRotation(), shifted.getRotation()), 1.0e-15);
46      }
47  
48      @Test
49      public void testOnePair() throws java.io.IOException {
50          RandomGenerator random = new Well1024a(0xed7dd911a44c5197l);
51  
52          for (int i = 0; i < 20; ++i) {
53  
54              Rotation r = randomRotation(random);
55              Vector3D o = randomVector(random, 1.0e-2);
56              Vector3D a = randomVector(random, 1.0e-2);
57              TimeStampedAngularCoordinates reference = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, a);
58  
59              PVCoordinates u = randomPVCoordinates(random, 1000, 1.0, 0.001);
60              PVCoordinates v = reference.applyTo(u);
61              TimeStampedAngularCoordinates ac =
62                      new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, u, v);
63  
64              Assertions.assertEquals(0, Vector3D.distance(v.getPosition().normalize(), ac.applyTo(u).getPosition().normalize()), 1.0e-14);
65              Assertions.assertEquals(0, Vector3D.distance(v.getVelocity().normalize(), ac.applyTo(u).getVelocity().normalize()), 4.0e-14);
66              Assertions.assertEquals(0, Vector3D.distance(v.getAcceleration().normalize(), ac.applyTo(u).getAcceleration().normalize()), 1.0e-14);
67  
68          }
69  
70      }
71  
72      @Test
73      public void testTwoPairs() throws java.io.IOException {
74          RandomGenerator random = new Well1024a(0x976ad943966c9f00l);
75  
76          for (int i = 0; i < 20; ++i) {
77  
78              Rotation r = randomRotation(random);
79              Vector3D o = randomVector(random, 1.0e-2);
80              Vector3D a = randomVector(random, 1.0e-2);
81              TimeStampedAngularCoordinates reference = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, a);
82  
83              PVCoordinates u1 = randomPVCoordinates(random, 1000, 1.0, 0.001);
84              PVCoordinates u2 = randomPVCoordinates(random, 1000, 1.0, 0.001);
85              PVCoordinates v1 = reference.applyTo(u1);
86              PVCoordinates v2 = reference.applyTo(u2);
87              TimeStampedAngularCoordinates ac =
88                      new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, u1, u2, v1, v2, 1.0e-9);
89  
90              Assertions.assertEquals(0, Vector3D.distance(v1.getPosition().normalize(), ac.applyTo(u1).getPosition().normalize()), 1.0e-14);
91              Assertions.assertEquals(0, Vector3D.distance(v1.getVelocity().normalize(), ac.applyTo(u1).getVelocity().normalize()), 1.0e-14);
92              Assertions.assertEquals(0, Vector3D.distance(v1.getAcceleration().normalize(), ac.applyTo(u1).getAcceleration().normalize()), 1.0e-14);
93              Assertions.assertEquals(0, Vector3D.distance(v2.getPosition().normalize(), ac.applyTo(u2).getPosition().normalize()), 1.0e-14);
94              Assertions.assertEquals(0, Vector3D.distance(v2.getVelocity().normalize(), ac.applyTo(u2).getVelocity().normalize()), 1.0e-14);
95              Assertions.assertEquals(0, Vector3D.distance(v2.getAcceleration().normalize(), ac.applyTo(u2).getAcceleration().normalize()), 1.0e-14);
96  
97          }
98  
99      }
100 
101     @Test
102     public void testDerivativesStructures0() {
103         RandomGenerator random = new Well1024a(0x18a0a08fd63f047al);
104 
105         Rotation r    = randomRotation(random);
106         Vector3D o    = randomVector(random, 1.0e-2);
107         Vector3D oDot = randomVector(random, 1.0e-2);
108         TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, oDot);
109         TimeStampedAngularCoordinates rebuilt = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH,
110                                                                                   ac.toDerivativeStructureRotation(0));
111         Assertions.assertEquals(0.0, Rotation.distance(ac.getRotation(), rebuilt.getRotation()), 1.0e-15);
112         Assertions.assertEquals(0.0, rebuilt.getRotationRate().getNorm(), 1.0e-15);
113         Assertions.assertEquals(0.0, rebuilt.getRotationAcceleration().getNorm(), 1.0e-15);
114     }
115 
116     @Test
117     public void testDerivativesStructures1() {
118         RandomGenerator random = new Well1024a(0x8f8fc6d27bbdc46dl);
119 
120         Rotation r    = randomRotation(random);
121         Vector3D o    = randomVector(random, 1.0e-2);
122         Vector3D oDot = randomVector(random, 1.0e-2);
123         TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, oDot);
124         TimeStampedAngularCoordinates rebuilt = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH,
125                                                                                   ac.toDerivativeStructureRotation(1));
126         Assertions.assertEquals(0.0, Rotation.distance(ac.getRotation(), rebuilt.getRotation()), 1.0e-15);
127         Assertions.assertEquals(0.0, Vector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()), 1.0e-15);
128         Assertions.assertEquals(0.0, rebuilt.getRotationAcceleration().getNorm(), 1.0e-15);
129     }
130 
131     @Test
132     public void testDerivativesStructures2() {
133         RandomGenerator random = new Well1024a(0x1633878dddac047dl);
134 
135         Rotation r    = randomRotation(random);
136         Vector3D o    = randomVector(random, 1.0e-2);
137         Vector3D oDot = randomVector(random, 1.0e-2);
138         TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, oDot);
139         TimeStampedAngularCoordinates rebuilt = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH,
140                                                                                   ac.toDerivativeStructureRotation(2));
141         Assertions.assertEquals(0.0, Rotation.distance(ac.getRotation(), rebuilt.getRotation()), 1.0e-15);
142         Assertions.assertEquals(0.0, Vector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()), 1.0e-15);
143         Assertions.assertEquals(0.0, Vector3D.distance(ac.getRotationAcceleration(), rebuilt.getRotationAcceleration()), 1.0e-15);
144     }
145 
146     @Test
147     public void testUnivariateDerivative1() {
148         RandomGenerator random = new Well1024a(0x6de8cce747539904l);
149 
150         Rotation r    = randomRotation(random);
151         Vector3D o    = randomVector(random, 1.0e-2);
152         Vector3D oDot = randomVector(random, 1.0e-2);
153         TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, oDot);
154         FieldRotation<UnivariateDerivative1> rotationUD = ac.toUnivariateDerivative1Rotation();
155         FieldRotation<DerivativeStructure>   rotationDS = ac.toDerivativeStructureRotation(1);
156         Assertions.assertEquals(rotationDS.getQ0().getReal(), rotationUD.getQ0().getReal(), 1.0e-15);
157         Assertions.assertEquals(rotationDS.getQ1().getReal(), rotationUD.getQ1().getReal(), 1.0e-15);
158         Assertions.assertEquals(rotationDS.getQ2().getReal(), rotationUD.getQ2().getReal(), 1.0e-15);
159         Assertions.assertEquals(rotationDS.getQ3().getReal(), rotationUD.getQ3().getReal(), 1.0e-15);
160         Assertions.assertEquals(rotationDS.getQ0().getPartialDerivative(1), rotationUD.getQ0().getFirstDerivative(), 1.0e-15);
161         Assertions.assertEquals(rotationDS.getQ1().getPartialDerivative(1), rotationUD.getQ1().getFirstDerivative(), 1.0e-15);
162         Assertions.assertEquals(rotationDS.getQ2().getPartialDerivative(1), rotationUD.getQ2().getFirstDerivative(), 1.0e-15);
163         Assertions.assertEquals(rotationDS.getQ3().getPartialDerivative(1), rotationUD.getQ3().getFirstDerivative(), 1.0e-15);
164 
165         TimeStampedAngularCoordinates rebuilt = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, rotationUD);
166         Assertions.assertEquals(0.0, Rotation.distance(ac.getRotation(), rebuilt.getRotation()), 1.0e-15);
167         Assertions.assertEquals(0.0, Vector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()), 1.0e-15);
168 
169     }
170 
171     @Test
172     public void testUnivariateDerivative2() {
173         RandomGenerator random = new Well1024a(0x255710c8fa2247ecl);
174 
175         Rotation r    = randomRotation(random);
176         Vector3D o    = randomVector(random, 1.0e-2);
177         Vector3D oDot = randomVector(random, 1.0e-2);
178         TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, oDot);
179         FieldRotation<UnivariateDerivative2> rotationUD = ac.toUnivariateDerivative2Rotation();
180         FieldRotation<DerivativeStructure>   rotationDS = ac.toDerivativeStructureRotation(2);
181         Assertions.assertEquals(rotationDS.getQ0().getReal(), rotationUD.getQ0().getReal(), 1.0e-15);
182         Assertions.assertEquals(rotationDS.getQ1().getReal(), rotationUD.getQ1().getReal(), 1.0e-15);
183         Assertions.assertEquals(rotationDS.getQ2().getReal(), rotationUD.getQ2().getReal(), 1.0e-15);
184         Assertions.assertEquals(rotationDS.getQ3().getReal(), rotationUD.getQ3().getReal(), 1.0e-15);
185         Assertions.assertEquals(rotationDS.getQ0().getPartialDerivative(1), rotationUD.getQ0().getFirstDerivative(), 1.0e-15);
186         Assertions.assertEquals(rotationDS.getQ1().getPartialDerivative(1), rotationUD.getQ1().getFirstDerivative(), 1.0e-15);
187         Assertions.assertEquals(rotationDS.getQ2().getPartialDerivative(1), rotationUD.getQ2().getFirstDerivative(), 1.0e-15);
188         Assertions.assertEquals(rotationDS.getQ3().getPartialDerivative(1), rotationUD.getQ3().getFirstDerivative(), 1.0e-15);
189         Assertions.assertEquals(rotationDS.getQ0().getPartialDerivative(2), rotationUD.getQ0().getSecondDerivative(), 1.0e-15);
190         Assertions.assertEquals(rotationDS.getQ1().getPartialDerivative(2), rotationUD.getQ1().getSecondDerivative(), 1.0e-15);
191         Assertions.assertEquals(rotationDS.getQ2().getPartialDerivative(2), rotationUD.getQ2().getSecondDerivative(), 1.0e-15);
192         Assertions.assertEquals(rotationDS.getQ3().getPartialDerivative(2), rotationUD.getQ3().getSecondDerivative(), 1.0e-15);
193 
194         TimeStampedAngularCoordinates rebuilt = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, rotationUD);
195         Assertions.assertEquals(0.0, Rotation.distance(ac.getRotation(), rebuilt.getRotation()), 1.0e-15);
196         Assertions.assertEquals(0.0, Vector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()), 1.0e-15);
197         Assertions.assertEquals(0.0, Vector3D.distance(ac.getRotationAcceleration(), rebuilt.getRotationAcceleration()), 1.0e-15);
198 
199     }
200 
201     @Test
202     public void testShift() {
203         double rate = 2 * FastMath.PI / (12 * 60);
204         TimeStampedAngularCoordinates ac =
205                 new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH,
206                                                   Rotation.IDENTITY,
207                                                   new Vector3D(rate, Vector3D.PLUS_K), Vector3D.ZERO);
208         Assertions.assertEquals(rate, ac.getRotationRate().getNorm(), 1.0e-10);
209         double dt = 10.0;
210         double alpha = rate * dt;
211         TimeStampedAngularCoordinates shifted = ac.shiftedBy(dt);
212         Assertions.assertEquals(rate, shifted.getRotationRate().getNorm(), 1.0e-10);
213         Assertions.assertEquals(alpha, Rotation.distance(ac.getRotation(), shifted.getRotation()), 1.0e-10);
214 
215         Vector3D xSat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_I);
216         Assertions.assertEquals(0.0, xSat.subtract(new Vector3D(FastMath.cos(alpha), FastMath.sin(alpha), 0)).getNorm(), 1.0e-10);
217         Vector3D ySat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_J);
218         Assertions.assertEquals(0.0, ySat.subtract(new Vector3D(-FastMath.sin(alpha), FastMath.cos(alpha), 0)).getNorm(), 1.0e-10);
219         Vector3D zSat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_K);
220         Assertions.assertEquals(0.0, zSat.subtract(Vector3D.PLUS_K).getNorm(), 1.0e-10);
221 
222     }
223 
224     @Test
225     public void testSpin() {
226         double rate = 2 * FastMath.PI / (12 * 60);
227         TimeStampedAngularCoordinates ac =
228                 new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH,
229                                                   new Rotation(0.48, 0.64, 0.36, 0.48, false),
230                                                   new Vector3D(rate, Vector3D.PLUS_K), Vector3D.ZERO);
231         Assertions.assertEquals(rate, ac.getRotationRate().getNorm(), 1.0e-10);
232         double dt = 10.0;
233         TimeStampedAngularCoordinates shifted = ac.shiftedBy(dt);
234         Assertions.assertEquals(rate, shifted.getRotationRate().getNorm(), 1.0e-10);
235         Assertions.assertEquals(rate * dt, Rotation.distance(ac.getRotation(), shifted.getRotation()), 1.0e-10);
236 
237         Vector3D shiftedX  = shifted.getRotation().applyInverseTo(Vector3D.PLUS_I);
238         Vector3D shiftedY  = shifted.getRotation().applyInverseTo(Vector3D.PLUS_J);
239         Vector3D shiftedZ  = shifted.getRotation().applyInverseTo(Vector3D.PLUS_K);
240         Vector3D originalX = ac.getRotation().applyInverseTo(Vector3D.PLUS_I);
241         Vector3D originalY = ac.getRotation().applyInverseTo(Vector3D.PLUS_J);
242         Vector3D originalZ = ac.getRotation().applyInverseTo(Vector3D.PLUS_K);
243         Assertions.assertEquals( FastMath.cos(rate * dt), Vector3D.dotProduct(shiftedX, originalX), 1.0e-10);
244         Assertions.assertEquals( FastMath.sin(rate * dt), Vector3D.dotProduct(shiftedX, originalY), 1.0e-10);
245         Assertions.assertEquals( 0.0,                 Vector3D.dotProduct(shiftedX, originalZ), 1.0e-10);
246         Assertions.assertEquals(-FastMath.sin(rate * dt), Vector3D.dotProduct(shiftedY, originalX), 1.0e-10);
247         Assertions.assertEquals( FastMath.cos(rate * dt), Vector3D.dotProduct(shiftedY, originalY), 1.0e-10);
248         Assertions.assertEquals( 0.0,                 Vector3D.dotProduct(shiftedY, originalZ), 1.0e-10);
249         Assertions.assertEquals( 0.0,                 Vector3D.dotProduct(shiftedZ, originalX), 1.0e-10);
250         Assertions.assertEquals( 0.0,                 Vector3D.dotProduct(shiftedZ, originalY), 1.0e-10);
251         Assertions.assertEquals( 1.0,                 Vector3D.dotProduct(shiftedZ, originalZ), 1.0e-10);
252 
253         Vector3D forward = TimeStampedAngularCoordinates.estimateRate(ac.getRotation(), shifted.getRotation(), dt);
254         Assertions.assertEquals(0.0, forward.subtract(ac.getRotationRate()).getNorm(), 1.0e-10);
255 
256         Vector3D reversed = TimeStampedAngularCoordinates.estimateRate(shifted.getRotation(), ac.getRotation(), dt);
257         Assertions.assertEquals(0.0, reversed.add(ac.getRotationRate()).getNorm(), 1.0e-10);
258 
259     }
260 
261     @Test
262     public void testReverseOffset() {
263         RandomGenerator random = new Well1024a(0x4ecca9d57a8f1611l);
264         for (int i = 0; i < 100; ++i) {
265             Rotation r = randomRotation(random);
266             Vector3D o = randomVector(random, 1.0e-3);
267             Vector3D a = randomVector(random, 1.0e-3);
268             TimeStampedAngularCoordinates ac = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r, o, a);
269             TimeStampedAngularCoordinates sum = ac.addOffset(ac.revert());
270             Assertions.assertEquals(0.0, sum.getRotation().getAngle(), 1.0e-15);
271             Assertions.assertEquals(0.0, sum.getRotationRate().getNorm(), 1.0e-15);
272             Assertions.assertEquals(0.0, sum.getRotationAcceleration().getNorm(), 1.0e-15);
273         }
274     }
275 
276     @Test
277     public void testNoCommute() {
278         TimeStampedAngularCoordinates ac1 =
279         new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, new Rotation(0.48,  0.64, 0.36, 0.48, false), Vector3D.ZERO, Vector3D.ZERO);
280         TimeStampedAngularCoordinates ac2 =
281         new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, new Rotation(0.36, -0.48, 0.48, 0.64, false), Vector3D.ZERO, Vector3D.ZERO);
282 
283         TimeStampedAngularCoordinates add12 = ac1.addOffset(ac2);
284         TimeStampedAngularCoordinates add21 = ac2.addOffset(ac1);
285 
286         // the rotations are really different from each other
287         Assertions.assertEquals(2.574, Rotation.distance(add12.getRotation(), add21.getRotation()), 1.0e-3);
288 
289     }
290 
291     @Test
292     public void testRoundTripNoOp() {
293         RandomGenerator random = new Well1024a(0x1e610cfe89306669l);
294         for (int i = 0; i < 100; ++i) {
295 
296             Rotation r1 = randomRotation(random);
297             Vector3D o1 = randomVector(random, 1.0e-2);
298             Vector3D a1 = randomVector(random, 1.0e-2);
299             TimeStampedAngularCoordinates ac1 = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r1, o1, a1);
300             Rotation r2 = randomRotation(random);
301             Vector3D o2 = randomVector(random, 1.0e-2);
302             Vector3D a2 = randomVector(random, 1.0e-2);
303 
304             TimeStampedAngularCoordinates ac2 = new TimeStampedAngularCoordinates(AbsoluteDate.J2000_EPOCH, r2, o2, a2);
305             TimeStampedAngularCoordinates roundTripSA = ac1.subtractOffset(ac2).addOffset(ac2);
306             Assertions.assertEquals(0.0, Rotation.distance(ac1.getRotation(), roundTripSA.getRotation()), 4.0e-16);
307             Assertions.assertEquals(0.0, Vector3D.distance(ac1.getRotationRate(), roundTripSA.getRotationRate()), 2.0e-17);
308             Assertions.assertEquals(0.0, Vector3D.distance(ac1.getRotationAcceleration(), roundTripSA.getRotationAcceleration()), 1.0e-17);
309 
310             TimeStampedAngularCoordinates roundTripAS = ac1.addOffset(ac2).subtractOffset(ac2);
311             Assertions.assertEquals(0.0, Rotation.distance(ac1.getRotation(), roundTripAS.getRotation()), 6.0e-16);
312             Assertions.assertEquals(0.0, Vector3D.distance(ac1.getRotationRate(), roundTripAS.getRotationRate()), 2.0e-17);
313             Assertions.assertEquals(0.0, Vector3D.distance(ac1.getRotationAcceleration(), roundTripAS.getRotationAcceleration()), 2.0e-17);
314 
315         }
316     }
317 
318     private Vector3D randomVector(RandomGenerator random, double norm) {
319         double n = random.nextDouble() * norm;
320         double x = 2 * random.nextDouble() - 1;
321         double y = 2 * random.nextDouble() - 1;
322         double z = 2 * random.nextDouble() - 1;
323         return new Vector3D(n, new Vector3D(x, y, z).normalize());
324     }
325 
326     private PVCoordinates randomPVCoordinates(RandomGenerator random,
327                                               double norm0, double norm1, double norm2) {
328         Vector3D p0 = randomVector(random, norm0);
329         Vector3D p1 = randomVector(random, norm1);
330         Vector3D p2 = randomVector(random, norm2);
331         return new PVCoordinates(p0, p1, p2);
332     }
333 
334     private Rotation randomRotation(RandomGenerator random) {
335         double q0 = random.nextDouble() * 2 - 1;
336         double q1 = random.nextDouble() * 2 - 1;
337         double q2 = random.nextDouble() * 2 - 1;
338         double q3 = random.nextDouble() * 2 - 1;
339         double q  = FastMath.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
340         return new Rotation(q0 / q, q1 / q, q2 / q, q3 / q, false);
341     }
342 
343 }
344