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