1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.rugged.linesensor;
18
19 import java.net.URISyntaxException;
20 import java.util.ArrayList;
21 import java.util.List;
22 import java.util.stream.Collectors;
23
24 import org.hipparchus.Field;
25 import org.hipparchus.analysis.UnivariateMatrixFunction;
26 import org.hipparchus.analysis.differentiation.DSFactory;
27 import org.hipparchus.analysis.differentiation.DerivativeStructure;
28 import org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator;
29 import org.hipparchus.analysis.differentiation.Gradient;
30 import org.hipparchus.analysis.differentiation.GradientField;
31 import org.hipparchus.analysis.differentiation.UnivariateDifferentiableMatrixFunction;
32 import org.hipparchus.analysis.polynomials.PolynomialFunction;
33 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
34 import org.hipparchus.geometry.euclidean.threed.Rotation;
35 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
36 import org.hipparchus.geometry.euclidean.threed.Vector3D;
37 import org.hipparchus.random.UncorrelatedRandomVectorGenerator;
38 import org.hipparchus.random.UniformRandomGenerator;
39 import org.hipparchus.random.Well19937a;
40 import org.hipparchus.util.FastMath;
41 import org.junit.jupiter.api.Assertions;
42 import org.junit.jupiter.api.BeforeEach;
43 import org.junit.jupiter.api.Test;
44 import org.orekit.rugged.los.LOSBuilder;
45 import org.orekit.rugged.los.PolynomialRotation;
46 import org.orekit.rugged.los.TimeDependentLOS;
47 import org.orekit.rugged.utils.DerivativeGenerator;
48 import org.orekit.time.AbsoluteDate;
49 import org.orekit.utils.ParameterDriver;
50
51 public class PolynomialRotationTest {
52
53 private List<Vector3D> raw;
54
55 @Test
56 public void testIdentity() {
57 UniformRandomGenerator rng = new UniformRandomGenerator(new Well19937a(0xbe0d9b530fe7f53cl));
58 UncorrelatedRandomVectorGenerator rvg = new UncorrelatedRandomVectorGenerator(3, rng);
59 for (int k = 0; k < 20; ++k) {
60 LOSBuilder builder = new LOSBuilder(raw);
61 builder.addTransform(new PolynomialRotation("identity",
62 new Vector3D(rvg.nextVector()),
63 AbsoluteDate.J2000_EPOCH, 0.0));
64 TimeDependentLOS tdl = builder.build();
65 for (int i = 0; i < raw.size(); ++i) {
66 Assertions.assertEquals(0.0,
67 Vector3D.distance(raw.get(i), tdl.getLOS(i, AbsoluteDate.J2000_EPOCH)),
68 2.0e-15);
69 }
70
71 Assertions.assertEquals(1, tdl.getParametersDrivers().count());
72 Assertions.assertEquals("identity[0]", tdl.getParametersDrivers().findFirst().get().getName());
73
74 }
75 }
76
77 @Test
78 public void testFixedCombination() {
79 UniformRandomGenerator rng = new UniformRandomGenerator(new Well19937a(0xdc4cfdea38edd2bbl));
80 UncorrelatedRandomVectorGenerator rvg = new UncorrelatedRandomVectorGenerator(3, rng);
81 for (int k = 0; k < 20; ++k) {
82
83 LOSBuilder builder = new LOSBuilder(raw);
84
85 Vector3D axis1 = new Vector3D(rvg.nextVector());
86 double angle1 = 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3);
87 builder.addTransform(new PolynomialRotation("r1", axis1, AbsoluteDate.J2000_EPOCH, angle1));
88 Rotation r1 = new Rotation(axis1, angle1, RotationConvention.VECTOR_OPERATOR);
89
90 Vector3D axis2 = new Vector3D(rvg.nextVector());
91 double angle2 = 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3);
92 builder.addTransform(new PolynomialRotation("r2", axis2, AbsoluteDate.J2000_EPOCH, angle2));
93 Rotation r2 = new Rotation(axis2, angle2, RotationConvention.VECTOR_OPERATOR);
94
95 Vector3D axis3 = new Vector3D(rvg.nextVector());
96 double angle3 = 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3);
97 builder.addTransform(new PolynomialRotation("r3", axis3, AbsoluteDate.J2000_EPOCH, angle3));
98 Rotation r3 = new Rotation(axis3, angle3, RotationConvention.VECTOR_OPERATOR);
99
100 TimeDependentLOS tdl = builder.build();
101 Rotation combined = r3.applyTo(r2.applyTo(r1));
102
103 for (int i = 0; i < raw.size(); ++i) {
104 Assertions.assertEquals(0.0,
105 Vector3D.distance(combined.applyTo(raw.get(i)),
106 tdl.getLOS(i, AbsoluteDate.J2000_EPOCH)),
107 2.0e-15);
108 }
109
110 List<ParameterDriver> drivers = tdl.getParametersDrivers().collect(Collectors.toList());
111 Assertions.assertEquals(3, drivers.size());
112 ParameterDriver driver1 = drivers.get(0);
113 ParameterDriver driver2 = drivers.get(1);
114 ParameterDriver driver3 = drivers.get(2);
115 Assertions.assertEquals("r1[0]", driver1.getName());
116 Assertions.assertTrue(Double.isInfinite(driver1.getMinValue()));
117 Assertions.assertTrue(driver1.getMinValue() < 0);
118 Assertions.assertTrue(Double.isInfinite(driver1.getMaxValue()));
119 Assertions.assertTrue(driver1.getMaxValue() > 0);
120 Assertions.assertEquals(angle1, driver1.getValue(), 2.0e-15);
121 Assertions.assertEquals("r2[0]", driver2.getName());
122 Assertions.assertTrue(Double.isInfinite(driver2.getMinValue()));
123 Assertions.assertTrue(driver2.getMinValue() < 0);
124 Assertions.assertTrue(Double.isInfinite(driver2.getMaxValue()));
125 Assertions.assertTrue(driver2.getMaxValue() > 0);
126 Assertions.assertEquals(angle2, driver2.getValue(), 2.0e-15);
127 Assertions.assertEquals("r3[0]", driver3.getName());
128 Assertions.assertTrue(Double.isInfinite(driver3.getMinValue()));
129 Assertions.assertTrue(driver3.getMinValue() < 0);
130 Assertions.assertTrue(Double.isInfinite(driver3.getMaxValue()));
131 Assertions.assertTrue(driver3.getMaxValue() > 0);
132 Assertions.assertEquals(angle3, driver3.getValue(), 2.0e-15);
133
134 driver1.setValue(0.0);
135 driver2.setValue(0.0);
136 driver3.setValue(0.0);
137
138 for (int i = 0; i < raw.size(); ++i) {
139 Assertions.assertEquals(0.0,
140 Vector3D.distance(raw.get(i),
141 tdl.getLOS(i, AbsoluteDate.J2000_EPOCH)),
142 2.0e-15);
143 }
144
145 }
146 }
147
148 @Test
149 public void testDerivatives() {
150 UniformRandomGenerator rng = new UniformRandomGenerator(new Well19937a(0xc60acfc04eb27935l));
151 UncorrelatedRandomVectorGenerator rvg = new UncorrelatedRandomVectorGenerator(3, rng);
152 for (int k = 0; k < 20; ++k) {
153
154 LOSBuilder builder = new LOSBuilder(raw);
155
156 builder.addTransform(new PolynomialRotation("r1",
157 new Vector3D(rvg.nextVector()),
158 AbsoluteDate.J2000_EPOCH,
159 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3),
160 1.0e-4 * 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3)));
161 builder.addTransform(new PolynomialRotation("r2",
162 new Vector3D(rvg.nextVector()),
163 AbsoluteDate.J2000_EPOCH,
164 new PolynomialFunction(new double[] {
165 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3),
166 1.0e-4 * 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3),
167 1.0e-8 * 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3)
168 })));
169 builder.addTransform(new PolynomialRotation("r3",
170 new Vector3D(rvg.nextVector()),
171 AbsoluteDate.J2000_EPOCH,
172 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3),
173 1.0e-4 * 2 * FastMath.PI * rng.nextNormalizedDouble() / FastMath.sqrt(3)));
174 TimeDependentLOS tdl = builder.build();
175 final List<ParameterDriver> selected = tdl.getParametersDrivers().collect(Collectors.toList());
176 for (final ParameterDriver driver : selected) {
177 driver.setSelected(true);
178 }
179 final GradientField field = GradientField.getField(selected.size());
180 DerivativeGenerator<Gradient> generator = new DerivativeGenerator<Gradient>() {
181
182
183 @Override
184 public List<ParameterDriver> getSelected() {
185 return selected;
186 }
187
188
189 @Override
190 public Gradient constant(final double value) {
191 return Gradient.constant(selected.size(), value);
192 }
193
194
195 @Override
196 public Gradient variable(final ParameterDriver driver) {
197 int index = 0;
198 for (ParameterDriver d : getSelected()) {
199 if (d == driver) {
200 return Gradient.variable(selected.size(), index, driver.getValue());
201 }
202 ++index;
203 }
204 return constant(driver.getValue());
205 }
206
207
208 @Override
209 public Field<Gradient> getField() {
210 return field;
211 }
212
213 };
214 Assertions.assertEquals(7, generator.getSelected().size());
215
216 FiniteDifferencesDifferentiator differentiator =
217 new FiniteDifferencesDifferentiator(4, 0.0001);
218 int index = 0;
219 DSFactory factory11 = new DSFactory(1, 1);
220 final AbsoluteDate date = AbsoluteDate.J2000_EPOCH.shiftedBy(7.0);
221 for (final ParameterDriver driver : selected) {
222 int[] orders = new int[selected.size()];
223 orders[index] = 1;
224 UnivariateDifferentiableMatrixFunction f =
225 differentiator.differentiate((UnivariateMatrixFunction) x -> {
226 double oldX = driver.getValue();
227 double[][] matrix = new double[raw.size()][];
228 driver.setValue(x);
229 for (int i = 0 ; i < raw.size(); ++i) {
230 matrix[i] = tdl.getLOS(i, date).toArray();
231 }
232 driver.setValue(oldX);
233 return matrix;
234 });
235 DerivativeStructure[][] mDS = f.value(factory11.variable(0, driver.getValue()));
236 for (int i = 0; i < raw.size(); ++i) {
237 Vector3D los = tdl.getLOS(i, date);
238 FieldVector3D<Gradient> losDS = tdl.getLOSDerivatives(i, date, generator);
239 Assertions.assertEquals(los.getX(), losDS.getX().getValue(), 2.0e-15);
240 Assertions.assertEquals(los.getY(), losDS.getY().getValue(), 2.0e-15);
241 Assertions.assertEquals(los.getZ(), losDS.getZ().getValue(), 2.0e-15);
242 Assertions.assertEquals(mDS[i][0].getPartialDerivative(1), losDS.getX().getPartialDerivative(orders), 2.0e-10);
243 Assertions.assertEquals(mDS[i][1].getPartialDerivative(1), losDS.getY().getPartialDerivative(orders), 2.0e-10);
244 Assertions.assertEquals(mDS[i][2].getPartialDerivative(1), losDS.getZ().getPartialDerivative(orders), 2.0e-10);
245 }
246 ++index;
247 }
248 }
249
250 }
251
252 @BeforeEach
253 public void setUp() throws URISyntaxException {
254
255 final Vector3D normal = Vector3D.PLUS_I;
256 final Vector3D fovCenter = Vector3D.PLUS_K;
257 final Vector3D cross = Vector3D.crossProduct(normal, fovCenter);
258
259
260 raw = new ArrayList<Vector3D>();
261 for (int i = -100; i <= 100; ++i) {
262 final double alpha = i * 0.17 / 1000;
263 raw.add(new Vector3D(FastMath.cos(alpha), fovCenter, FastMath.sin(alpha), cross));
264 }
265
266 }
267
268 }