1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.files.ccsds.ndm.adm;
18
19 import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
20 import org.hipparchus.analysis.differentiation.UnivariateDerivative2Field;
21 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
22 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
23 import org.hipparchus.geometry.euclidean.threed.Rotation;
24 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
25 import org.hipparchus.geometry.euclidean.threed.RotationOrder;
26 import org.hipparchus.geometry.euclidean.threed.Vector3D;
27 import org.hipparchus.random.RandomGenerator;
28 import org.hipparchus.random.UnitSphereRandomVectorGenerator;
29 import org.hipparchus.random.Well19937a;
30 import org.hipparchus.util.FastMath;
31 import org.hipparchus.util.MathUtils;
32 import org.junit.jupiter.api.Assertions;
33 import org.junit.jupiter.api.Test;
34 import org.orekit.errors.OrekitException;
35 import org.orekit.errors.OrekitMessages;
36
37
38
39
40
41 public class PrecessionFinderTest {
42
43 @Test
44 public void testNoSpin() {
45 final FieldVector3D<UnivariateDerivative2> spin =
46 FieldVector3D.getZero(UnivariateDerivative2Field.getInstance());
47 PrecessionFinder pf = new PrecessionFinder(spin);
48 Assertions.assertEquals(0.0, Vector3D.PLUS_K.distance(pf.getAxis()), 1.0e-15);
49 Assertions.assertEquals(0.0, pf.getPrecessionAngle(), 1.0e-15);
50 Assertions.assertEquals(0.0, pf.getAngularVelocity(), 1.0e-15);
51 }
52
53 @Test
54 public void testFixedSpin() {
55 final FieldVector3D<UnivariateDerivative2> spin =
56 new FieldVector3D<>(new UnivariateDerivative2( 1.25, 0.0, 0.0),
57 new UnivariateDerivative2(-0.50, 0.0, 0.0),
58 new UnivariateDerivative2( 2.00, 0.0, 0.0));
59 PrecessionFinder pf = new PrecessionFinder(spin);
60 Assertions.assertEquals(0.0, new Vector3D(1.25, -0.50, 2.00).normalize().distance(pf.getAxis()), 1.0e-15);
61 Assertions.assertEquals(0.0, pf.getPrecessionAngle(), 1.0e-15);
62 Assertions.assertEquals(0.0, pf.getAngularVelocity(), 1.0e-15);
63 }
64
65 @Test
66 public void testMissingSecondDerivative() {
67 final UnivariateDerivative2Field field = UnivariateDerivative2Field.getInstance();
68 final FieldRotation<UnivariateDerivative2> r =
69 new FieldRotation<>(FieldVector3D.getPlusK(field),
70 new UnivariateDerivative2(0.0, -0.5, 0.0),
71 RotationConvention.FRAME_TRANSFORM);
72 final FieldVector3D<UnivariateDerivative2> spin = r.applyTo(new Vector3D(3, 0, 4));
73 final FieldVector3D<UnivariateDerivative2> spinWithoutSecondDerivative =
74 new FieldVector3D<>(new UnivariateDerivative2(spin.getX().getValue(), spin.getX().getFirstDerivative(), 0.0),
75 new UnivariateDerivative2(spin.getY().getValue(), spin.getY().getFirstDerivative(), 0.0),
76 new UnivariateDerivative2(spin.getZ().getValue(), spin.getZ().getFirstDerivative(), 0.0));
77 try {
78 new PrecessionFinder(spinWithoutSecondDerivative);
79 Assertions.fail("an exception should have been thrown");
80 } catch (OrekitException oe) {
81 Assertions.assertEquals(OrekitMessages.CANNOT_ESTIMATE_PRECESSION_WITHOUT_PROPER_DERIVATIVES,
82 oe.getSpecifier());
83 }
84 }
85
86 @Test
87 public void testCanonicalZ() {
88 final UnivariateDerivative2Field field = UnivariateDerivative2Field.getInstance();
89 final FieldRotation<UnivariateDerivative2> r =
90 new FieldRotation<>(FieldVector3D.getPlusK(field),
91 new UnivariateDerivative2(0.0, -0.5, 0.0),
92 RotationConvention.FRAME_TRANSFORM);
93 final FieldVector3D<UnivariateDerivative2> spin = r.applyTo(new Vector3D(3, 0, 4));
94 PrecessionFinder pf = new PrecessionFinder(spin);
95 Assertions.assertEquals(0.0, Vector3D.PLUS_K.distance(pf.getAxis()), 1.0e-15);
96 Assertions.assertEquals(Vector3D.angle(new Vector3D(3, 0, 4), Vector3D.PLUS_K), pf.getPrecessionAngle(), 1.0e-15);
97 Assertions.assertEquals(0.5, pf.getAngularVelocity(), 1.0e-15);
98 }
99
100 @Test
101 public void testRandom() {
102 RandomGenerator random = new Well19937a(0x7a1ac34897e52f1fl);
103 UnitSphereRandomVectorGenerator us = new UnitSphereRandomVectorGenerator(3, random);
104 double maxAngleError = 0;
105 double maxRateError = 0;
106 double maxAlignmentError = 0;
107 for (int i = 0; i < 1000; ++i) {
108 final Rotation base = new Rotation(new Vector3D(us.nextVector()),
109 FastMath.PI * random.nextDouble(),
110 RotationConvention.FRAME_TRANSFORM);
111 final double phi0 = MathUtils.TWO_PI * random.nextDouble();
112 final double phi0Dot = MathUtils.TWO_PI * random.nextDouble() * 0.001;
113 final double theta = FastMath.PI * random.nextDouble();
114 final double psi0 = MathUtils.TWO_PI * random.nextDouble();
115 final double psi0Dot = MathUtils.TWO_PI * random.nextDouble() * 0.01;
116 final FieldRotation<UnivariateDerivative2> r =
117 new FieldRotation<>(RotationOrder.ZXZ, RotationConvention.FRAME_TRANSFORM,
118 new UnivariateDerivative2(phi0, phi0Dot, 0.0),
119 new UnivariateDerivative2(theta, 0.0, 0.0),
120 new UnivariateDerivative2(psi0, psi0Dot, 0.0)).
121 applyTo(base);
122
123
124 final FieldVector3D<UnivariateDerivative2> spin = r.applyInverseTo(Vector3D.PLUS_K);
125 final PrecessionFinder pf = new PrecessionFinder(spin);
126 maxAngleError = FastMath.max(maxAngleError, FastMath.abs(pf.getPrecessionAngle() - theta));
127 maxRateError = FastMath.max(maxRateError, FastMath.abs(pf.getAngularVelocity() - phi0Dot));
128 maxAlignmentError = FastMath.max(maxAlignmentError, Vector3D.angle(pf.getAxis(), base.applyInverseTo(Vector3D.PLUS_K)));
129 }
130 Assertions.assertEquals(0.0, maxAngleError, 1.7e-9);
131 Assertions.assertEquals(0.0, maxRateError, 2.3e-13);
132 Assertions.assertEquals(0.0, maxAlignmentError, 1.7e-9);
133 }
134
135 }