1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.utils;
18
19 import java.lang.reflect.InvocationTargetException;
20 import java.lang.reflect.Method;
21
22 import org.hipparchus.CalculusFieldElement;
23 import org.hipparchus.Field;
24 import org.hipparchus.analysis.differentiation.DSFactory;
25 import org.hipparchus.analysis.differentiation.DerivativeStructure;
26 import org.hipparchus.analysis.differentiation.FieldDerivativeStructure;
27 import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative1;
28 import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
29 import org.hipparchus.exception.MathIllegalArgumentException;
30 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
31 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
32 import org.hipparchus.geometry.euclidean.threed.Rotation;
33 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
34 import org.hipparchus.geometry.euclidean.threed.Vector3D;
35 import org.hipparchus.ode.FieldExpandableODE;
36 import org.hipparchus.ode.FieldODEIntegrator;
37 import org.hipparchus.ode.FieldODEState;
38 import org.hipparchus.ode.FieldODEStateAndDerivative;
39 import org.hipparchus.ode.FieldOrdinaryDifferentialEquation;
40 import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator;
41 import org.hipparchus.ode.sampling.FieldODEFixedStepHandler;
42 import org.hipparchus.ode.sampling.FieldStepNormalizer;
43 import org.hipparchus.random.RandomGenerator;
44 import org.hipparchus.random.Well1024a;
45 import org.hipparchus.util.Binary64;
46 import org.hipparchus.util.Binary64Field;
47 import org.hipparchus.util.FastMath;
48 import org.hipparchus.util.Precision;
49 import org.junit.jupiter.api.Assertions;
50 import org.junit.jupiter.api.Test;
51 import org.orekit.errors.OrekitException;
52 import org.orekit.errors.OrekitMessages;
53 import org.orekit.time.AbsoluteDate;
54
55 public class FieldAngularCoordinatesTest {
56
57 @Test
58 public void testAccelerationModeling() {
59 Binary64 rate = new Binary64(2 * FastMath.PI / (12 * 60));
60 Binary64 acc = new Binary64(0.01);
61 Binary64 dt = new Binary64(1.0);
62 int n = 2000;
63 final FieldAngularCoordinates<Binary64> quadratic =
64 new FieldAngularCoordinates<>(FieldRotation.getIdentity(Binary64Field.getInstance()),
65 new FieldVector3D<>(rate, Vector3D.PLUS_K),
66 new FieldVector3D<>(acc, Vector3D.PLUS_J));
67
68 final FieldOrdinaryDifferentialEquation<Binary64> ode = new FieldOrdinaryDifferentialEquation<Binary64>() {
69 public int getDimension() {
70 return 4;
71 }
72 public Binary64[] computeDerivatives(final Binary64 t, final Binary64[] q) {
73 final Binary64 omegaX = quadratic.getRotationRate().getX().add(t.multiply(quadratic.getRotationAcceleration().getX()));
74 final Binary64 omegaY = quadratic.getRotationRate().getY().add(t.multiply(quadratic.getRotationAcceleration().getY()));
75 final Binary64 omegaZ = quadratic.getRotationRate().getZ().add(t.multiply(quadratic.getRotationAcceleration().getZ()));
76 return new Binary64[] {
77 t.linearCombination(q[1].negate(), omegaX, q[2].negate(), omegaY, q[3].negate(), omegaZ).multiply(0.5),
78 t.linearCombination(q[0], omegaX, q[3].negate(), omegaY, q[2], omegaZ).multiply(0.5),
79 t.linearCombination(q[3], omegaX, q[0], omegaY, q[1].negate(), omegaZ).multiply(0.5),
80 t.linearCombination(q[2].negate(), omegaX, q[1], omegaY, q[0], omegaZ).multiply(0.5)
81 };
82 }
83 };
84 FieldODEIntegrator<Binary64> integrator =
85 new DormandPrince853FieldIntegrator<>(Binary64Field.getInstance(), 1.0e-6, 1.0, 1.0e-12, 1.0e-12);
86 integrator.addStepHandler(new FieldStepNormalizer<>(dt.getReal() / n, new FieldODEFixedStepHandler<Binary64>() {
87 private Binary64 tM4, tM3, tM2, tM1, t0, tP1, tP2, tP3, tP4;
88 private Binary64[] yM4, yM3, yM2, yM1, y0, yP1, yP2, yP3, yP4;
89 private Binary64[] ydM4, ydM3, ydM2, ydM1, yd0, ydP1, ydP2, ydP3, ydP4;
90 public void handleStep(FieldODEStateAndDerivative<Binary64> s, boolean isLast) {
91 tM4 = tM3; yM4 = yM3; ydM4 = ydM3;
92 tM3 = tM2; yM3 = yM2; ydM3 = ydM2;
93 tM2 = tM1; yM2 = yM1; ydM2 = ydM1;
94 tM1 = t0 ; yM1 = y0 ; ydM1 = yd0 ;
95 t0 = tP1; y0 = yP1; yd0 = ydP1;
96 tP1 = tP2; yP1 = yP2; ydP1 = ydP2;
97 tP2 = tP3; yP2 = yP3; ydP2 = ydP3;
98 tP3 = tP4; yP3 = yP4; ydP3 = ydP4;
99 tP4 = s.getTime();
100 yP4 = s.getPrimaryState();
101 ydP4 = s.getPrimaryDerivative();
102
103 if (yM4 != null) {
104 Binary64 dt = tP4.subtract(tM4).divide(8);
105 final Binary64 c = dt.multiply(840).reciprocal();
106 final Binary64[] ydd0 = {
107 c.multiply(-3).multiply(ydP4[0].subtract(ydM4[0])).add(c.multiply(32).multiply(ydP3[0].subtract(ydM3[0]))).add(c.multiply(-168).multiply(ydP2[0].subtract(ydM2[0]))).add(c.multiply(672).multiply(ydP1[0].subtract(ydM1[0]))),
108 c.multiply(-3).multiply(ydP4[1].subtract(ydM4[1])).add(c.multiply(32).multiply(ydP3[1].subtract(ydM3[1]))).add(c.multiply(-168).multiply(ydP2[1].subtract(ydM2[1]))).add(c.multiply(672).multiply(ydP1[1].subtract(ydM1[1]))),
109 c.multiply(-3).multiply(ydP4[2].subtract(ydM4[2])).add(c.multiply(32).multiply(ydP3[2].subtract(ydM3[2]))).add(c.multiply(-168).multiply(ydP2[2].subtract(ydM2[2]))).add(c.multiply(672).multiply(ydP1[2].subtract(ydM1[2]))),
110 c.multiply(-3).multiply(ydP4[3].subtract(ydM4[3])).add(c.multiply(32).multiply(ydP3[3].subtract(ydM3[3]))).add(c.multiply(-168).multiply(ydP2[3].subtract(ydM2[3]))).add(c.multiply(672).multiply(ydP1[3].subtract(ydM1[3]))),
111 };
112 FieldAngularCoordinates<Binary64> ac =
113 new FieldAngularCoordinates<>(new FieldRotation<>(new FieldUnivariateDerivative2<>(y0[0], yd0[0], ydd0[0]),
114 new FieldUnivariateDerivative2<>(y0[1], yd0[1], ydd0[1]),
115 new FieldUnivariateDerivative2<>(y0[2], yd0[2], ydd0[2]),
116 new FieldUnivariateDerivative2<>(y0[3], yd0[3], ydd0[3]),
117 false));
118 Assertions.assertEquals(0.0,
119 FieldVector3D.distance(quadratic.getRotationAcceleration(),
120 ac.getRotationAcceleration()).getReal(),
121 4.0e-13);
122 }
123
124 }
125 }));
126
127 Binary64[] y ={
128 quadratic.getRotation().getQ0(),
129 quadratic.getRotation().getQ1(),
130 quadratic.getRotation().getQ2(),
131 quadratic.getRotation().getQ3()
132 };
133 integrator.integrate(new FieldExpandableODE<>(ode), new FieldODEState<>(new Binary64(0), y), dt);
134
135 }
136
137 @Test
138 public void testDerivativesStructuresNeg() {
139 try {
140 AngularCoordinates.IDENTITY.toDerivativeStructureRotation(-1);
141 Assertions.fail("an exception should have been thrown");
142 } catch (OrekitException oe) {
143 Assertions.assertEquals(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, oe.getSpecifier());
144 Assertions.assertEquals(-1, ((Integer) (oe.getParts()[0])).intValue());
145 }
146
147 }
148
149 @Test
150 public void testDerivativesStructures3() {
151 try {
152 AngularCoordinates.IDENTITY.toDerivativeStructureRotation(3);
153 Assertions.fail("an exception should have been thrown");
154 } catch (OrekitException oe) {
155 Assertions.assertEquals(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, oe.getSpecifier());
156 Assertions.assertEquals(3, ((Integer) (oe.getParts()[0])).intValue());
157 }
158
159 }
160
161 @Test
162 public void testDerivativesStructures0() {
163 RandomGenerator random = new Well1024a(0x18a0a08fd63f047al);
164
165 FieldRotation<Binary64> r = randomRotation64(random);
166 FieldVector3D<Binary64> o = randomVector64(random, 1.0e-2);
167 FieldVector3D<Binary64> oDot = randomVector64(random, 1.0e-2);
168 FieldAngularCoordinates<Binary64> ac = new FieldAngularCoordinates<>(r, o, oDot);
169 FieldAngularCoordinates<Binary64> rebuilt = new FieldAngularCoordinates<>(ac.toDerivativeStructureRotation(0));
170 Assertions.assertEquals(0.0, FieldRotation.distance(ac.getRotation(), rebuilt.getRotation()).getReal(), 1.0e-15);
171 Assertions.assertEquals(0.0, rebuilt.getRotationRate().getNorm().getReal(), 1.0e-15);
172 Assertions.assertEquals(0.0, rebuilt.getRotationAcceleration().getNorm().getReal(), 1.0e-15);
173 }
174
175 @Test
176 public void testDerivativesStructures1() {
177 RandomGenerator random = new Well1024a(0x8f8fc6d27bbdc46dl);
178
179 FieldRotation<Binary64> r = randomRotation64(random);
180 FieldVector3D<Binary64> o = randomVector64(random, 1.0e-2);
181 FieldVector3D<Binary64> oDot = randomVector64(random, 1.0e-2);
182 FieldAngularCoordinates<Binary64> ac = new FieldAngularCoordinates<>(r, o, oDot);
183 FieldAngularCoordinates<Binary64> rebuilt = new FieldAngularCoordinates<>(ac.toDerivativeStructureRotation(1));
184 Assertions.assertEquals(0.0, FieldRotation.distance(ac.getRotation(), rebuilt.getRotation()).getReal(), 1.0e-15);
185 Assertions.assertEquals(0.0, FieldVector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()).getReal(), 1.0e-15);
186 Assertions.assertEquals(0.0, rebuilt.getRotationAcceleration().getNorm().getReal(), 1.0e-15);
187 }
188
189 @Test
190 public void testDerivativesStructures2() {
191 RandomGenerator random = new Well1024a(0x1633878dddac047dl);
192
193 FieldRotation<Binary64> r = randomRotation64(random);
194 FieldVector3D<Binary64> o = randomVector64(random, 1.0e-2);
195 FieldVector3D<Binary64> oDot = randomVector64(random, 1.0e-2);
196 FieldAngularCoordinates<Binary64> ac = new FieldAngularCoordinates<>(r, o, oDot);
197 FieldAngularCoordinates<Binary64> rebuilt = new FieldAngularCoordinates<>(ac.toDerivativeStructureRotation(2));
198 Assertions.assertEquals(0.0, FieldRotation.distance(ac.getRotation(), rebuilt.getRotation()).getReal(), 1.0e-15);
199 Assertions.assertEquals(0.0, FieldVector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()).getReal(), 1.0e-15);
200 Assertions.assertEquals(0.0, FieldVector3D.distance(ac.getRotationAcceleration(), rebuilt.getRotationAcceleration()).getReal(), 1.0e-15);
201
202 }
203
204 @Test
205 public void testUnivariateDerivative1() {
206 RandomGenerator random = new Well1024a(0x6de8cce747539904l);
207
208 FieldRotation<Binary64> r = randomRotation64(random);
209 FieldVector3D<Binary64> o = randomVector64(random, 1.0e-2);
210 FieldVector3D<Binary64> oDot = randomVector64(random, 1.0e-2);
211 FieldAngularCoordinates<Binary64> ac = new FieldAngularCoordinates<>(r, o, oDot);
212 FieldRotation<FieldUnivariateDerivative1<Binary64>> rotationUD = ac.toUnivariateDerivative1Rotation();
213 FieldRotation<FieldDerivativeStructure<Binary64>> rotationDS = ac.toDerivativeStructureRotation(1);
214 Assertions.assertEquals(rotationDS.getQ0().getReal(), rotationUD.getQ0().getReal(), 1.0e-15);
215 Assertions.assertEquals(rotationDS.getQ1().getReal(), rotationUD.getQ1().getReal(), 1.0e-15);
216 Assertions.assertEquals(rotationDS.getQ2().getReal(), rotationUD.getQ2().getReal(), 1.0e-15);
217 Assertions.assertEquals(rotationDS.getQ3().getReal(), rotationUD.getQ3().getReal(), 1.0e-15);
218 Assertions.assertEquals(rotationDS.getQ0().getPartialDerivative(1).getReal(), rotationUD.getQ0().getFirstDerivative().getReal(), 1.0e-15);
219 Assertions.assertEquals(rotationDS.getQ1().getPartialDerivative(1).getReal(), rotationUD.getQ1().getFirstDerivative().getReal(), 1.0e-15);
220 Assertions.assertEquals(rotationDS.getQ2().getPartialDerivative(1).getReal(), rotationUD.getQ2().getFirstDerivative().getReal(), 1.0e-15);
221 Assertions.assertEquals(rotationDS.getQ3().getPartialDerivative(1).getReal(), rotationUD.getQ3().getFirstDerivative().getReal(), 1.0e-15);
222
223 FieldAngularCoordinates<Binary64> rebuilt = new FieldAngularCoordinates<>(rotationUD);
224 Assertions.assertEquals(0.0, FieldRotation.distance(ac.getRotation(), rebuilt.getRotation()).getReal(), 1.0e-15);
225 Assertions.assertEquals(0.0, FieldVector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()).getReal(), 1.0e-15);
226
227 }
228
229 @Test
230 public void testUnivariateDerivative2() {
231 RandomGenerator random = new Well1024a(0x255710c8fa2247ecl);
232
233 FieldRotation<Binary64> r = randomRotation64(random);
234 FieldVector3D<Binary64> o = randomVector64(random, 1.0e-2);
235 FieldVector3D<Binary64> oDot = randomVector64(random, 1.0e-2);
236 FieldAngularCoordinates<Binary64> ac = new FieldAngularCoordinates<>(r, o, oDot);
237 FieldRotation<FieldUnivariateDerivative2<Binary64>> rotationUD = ac.toUnivariateDerivative2Rotation();
238 FieldRotation<FieldDerivativeStructure<Binary64>> rotationDS = ac.toDerivativeStructureRotation(2);
239 Assertions.assertEquals(rotationDS.getQ0().getReal(), rotationUD.getQ0().getReal(), 1.0e-15);
240 Assertions.assertEquals(rotationDS.getQ1().getReal(), rotationUD.getQ1().getReal(), 1.0e-15);
241 Assertions.assertEquals(rotationDS.getQ2().getReal(), rotationUD.getQ2().getReal(), 1.0e-15);
242 Assertions.assertEquals(rotationDS.getQ3().getReal(), rotationUD.getQ3().getReal(), 1.0e-15);
243 Assertions.assertEquals(rotationDS.getQ0().getPartialDerivative(1).getReal(), rotationUD.getQ0().getFirstDerivative().getReal(), 1.0e-15);
244 Assertions.assertEquals(rotationDS.getQ1().getPartialDerivative(1).getReal(), rotationUD.getQ1().getFirstDerivative().getReal(), 1.0e-15);
245 Assertions.assertEquals(rotationDS.getQ2().getPartialDerivative(1).getReal(), rotationUD.getQ2().getFirstDerivative().getReal(), 1.0e-15);
246 Assertions.assertEquals(rotationDS.getQ3().getPartialDerivative(1).getReal(), rotationUD.getQ3().getFirstDerivative().getReal(), 1.0e-15);
247 Assertions.assertEquals(rotationDS.getQ0().getPartialDerivative(2).getReal(), rotationUD.getQ0().getSecondDerivative().getReal(), 1.0e-15);
248 Assertions.assertEquals(rotationDS.getQ1().getPartialDerivative(2).getReal(), rotationUD.getQ1().getSecondDerivative().getReal(), 1.0e-15);
249 Assertions.assertEquals(rotationDS.getQ2().getPartialDerivative(2).getReal(), rotationUD.getQ2().getSecondDerivative().getReal(), 1.0e-15);
250 Assertions.assertEquals(rotationDS.getQ3().getPartialDerivative(2).getReal(), rotationUD.getQ3().getSecondDerivative().getReal(), 1.0e-15);
251
252 FieldAngularCoordinates<Binary64> rebuilt = new FieldAngularCoordinates<>(rotationUD);
253 Assertions.assertEquals(0.0, FieldRotation.distance(ac.getRotation(), rebuilt.getRotation()).getReal(), 1.0e-15);
254 Assertions.assertEquals(0.0, FieldVector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()).getReal(), 1.0e-15);
255 Assertions.assertEquals(0.0, FieldVector3D.distance(ac.getRotationAcceleration(), rebuilt.getRotationAcceleration()).getReal(), 1.0e-15);
256
257 }
258
259 @Test
260 public void testZeroRate() {
261 FieldAngularCoordinates<DerivativeStructure> angularCoordinates =
262 new FieldAngularCoordinates<>(createRotation(0.48, 0.64, 0.36, 0.48, false),
263 createVector(0, 0, 0, 4),
264 createVector(0, 0, 0, 4));
265 Assertions.assertEquals(createVector(0, 0, 0, 4), angularCoordinates.getRotationRate());
266 double dt = 10.0;
267 FieldAngularCoordinates<DerivativeStructure> shifted = angularCoordinates.shiftedBy(dt);
268 Assertions.assertEquals(0.0, shifted.getRotationAcceleration().getNorm().getReal(), 1.0e-15);
269 Assertions.assertEquals(0.0, shifted.getRotationRate().getNorm().getReal(), 1.0e-15);
270 Assertions.assertEquals(0.0, FieldRotation.distance(angularCoordinates.getRotation(), shifted.getRotation()).getReal(), 1.0e-15);
271 }
272
273 @Test
274 public void testShift() {
275 double rate = 2 * FastMath.PI / (12 * 60);
276 FieldAngularCoordinates<DerivativeStructure> angularCoordinates =
277 new FieldAngularCoordinates<>(createRotation(1, 0, 0, 0, false),
278 new FieldVector3D<>(rate, createVector(0, 0, 1, 4)),
279 createVector(0, 0, 0, 4));
280 Assertions.assertEquals(rate, angularCoordinates.getRotationRate().getNorm().getReal(), 1.0e-10);
281 double dt = 10.0;
282 double alpha = rate * dt;
283 FieldAngularCoordinates<DerivativeStructure> shifted = angularCoordinates.shiftedBy(dt);
284 Assertions.assertEquals(rate, shifted.getRotationRate().getNorm().getReal(), 1.0e-10);
285 Assertions.assertEquals(alpha, FieldRotation.distance(angularCoordinates.getRotation(), shifted.getRotation()).getReal(), 1.0e-10);
286
287 FieldVector3D<DerivativeStructure> xSat = shifted.getRotation().applyInverseTo(createVector(1, 0, 0, 4));
288 Assertions.assertEquals(0.0, xSat.subtract(createVector(FastMath.cos(alpha), FastMath.sin(alpha), 0, 4)).getNorm().getReal(), 1.0e-10);
289 FieldVector3D<DerivativeStructure> ySat = shifted.getRotation().applyInverseTo(createVector(0, 1, 0, 4));
290 Assertions.assertEquals(0.0, ySat.subtract(createVector(-FastMath.sin(alpha), FastMath.cos(alpha), 0, 4)).getNorm().getReal(), 1.0e-10);
291 FieldVector3D<DerivativeStructure> zSat = shifted.getRotation().applyInverseTo(createVector(0, 0, 1, 4));
292 Assertions.assertEquals(0.0, zSat.subtract(createVector(0, 0, 1, 4)).getNorm().getReal(), 1.0e-10);
293
294 }
295
296 @Test
297 public void testToAC() {
298 RandomGenerator random = new Well1024a(0xc9b4cf6c371108e0l);
299 for (int i = 0; i < 100; ++i) {
300 FieldRotation<DerivativeStructure> r = randomRotation(random);
301 FieldVector3D<DerivativeStructure> o = randomVector(random, 1.0e-3);
302 FieldVector3D<DerivativeStructure> a = randomVector(random, 1.0e-3);
303 FieldAngularCoordinates<DerivativeStructure> acds = new FieldAngularCoordinates<>(r, o, a);
304 AngularCoordinates ac = acds.toAngularCoordinates();
305 Assertions.assertEquals(0, Rotation.distance(r.toRotation(), ac.getRotation()), 1.0e-15);
306 Assertions.assertEquals(0, FieldVector3D.distance(o, ac.getRotationRate()).getReal(), 1.0e-15);
307 }
308 }
309
310 @Test
311 public void testSpin() {
312 double rate = 2 * FastMath.PI / (12 * 60);
313 FieldAngularCoordinates<DerivativeStructure> angularCoordinates =
314 new FieldAngularCoordinates<>(createRotation(0.48, 0.64, 0.36, 0.48, false),
315 new FieldVector3D<>(rate, createVector(0, 0, 1, 4)),
316 createVector(0, 0, 0, 4));
317 Assertions.assertEquals(rate, angularCoordinates.getRotationRate().getNorm().getReal(), 1.0e-10);
318 double dt = 10.0;
319 FieldAngularCoordinates<DerivativeStructure> shifted = angularCoordinates.shiftedBy(dt);
320 Assertions.assertEquals(rate, shifted.getRotationRate().getNorm().getReal(), 1.0e-10);
321 Assertions.assertEquals(rate * dt, FieldRotation.distance(angularCoordinates.getRotation(), shifted.getRotation()).getReal(), 1.0e-10);
322
323 FieldVector3D<DerivativeStructure> shiftedX = shifted.getRotation().applyInverseTo(createVector(1, 0, 0, 4));
324 FieldVector3D<DerivativeStructure> shiftedY = shifted.getRotation().applyInverseTo(createVector(0, 1, 0, 4));
325 FieldVector3D<DerivativeStructure> shiftedZ = shifted.getRotation().applyInverseTo(createVector(0, 0, 1, 4));
326 FieldVector3D<DerivativeStructure> originalX = angularCoordinates.getRotation().applyInverseTo(createVector(1, 0, 0, 4));
327 FieldVector3D<DerivativeStructure> originalY = angularCoordinates.getRotation().applyInverseTo(createVector(0, 1, 0, 4));
328 FieldVector3D<DerivativeStructure> originalZ = angularCoordinates.getRotation().applyInverseTo(createVector(0, 0, 1, 4));
329 Assertions.assertEquals( FastMath.cos(rate * dt), FieldVector3D.dotProduct(shiftedX, originalX).getReal(), 1.0e-10);
330 Assertions.assertEquals( FastMath.sin(rate * dt), FieldVector3D.dotProduct(shiftedX, originalY).getReal(), 1.0e-10);
331 Assertions.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedX, originalZ).getReal(), 1.0e-10);
332 Assertions.assertEquals(-FastMath.sin(rate * dt), FieldVector3D.dotProduct(shiftedY, originalX).getReal(), 1.0e-10);
333 Assertions.assertEquals( FastMath.cos(rate * dt), FieldVector3D.dotProduct(shiftedY, originalY).getReal(), 1.0e-10);
334 Assertions.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedY, originalZ).getReal(), 1.0e-10);
335 Assertions.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedZ, originalX).getReal(), 1.0e-10);
336 Assertions.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedZ, originalY).getReal(), 1.0e-10);
337 Assertions.assertEquals( 1.0, FieldVector3D.dotProduct(shiftedZ, originalZ).getReal(), 1.0e-10);
338
339 FieldVector3D<DerivativeStructure> forward = FieldAngularCoordinates.estimateRate(angularCoordinates.getRotation(), shifted.getRotation(), dt);
340 Assertions.assertEquals(0.0, forward.subtract(angularCoordinates.getRotationRate()).getNorm().getReal(), 1.0e-10);
341
342 FieldVector3D<DerivativeStructure> reversed = FieldAngularCoordinates.estimateRate(shifted.getRotation(), angularCoordinates.getRotation(), dt);
343 Assertions.assertEquals(0.0, reversed.add(angularCoordinates.getRotationRate()).getNorm().getReal(), 1.0e-10);
344
345 }
346
347 @Test
348 public void testReverseOffset() {
349 RandomGenerator random = new Well1024a(0x4ecca9d57a8f1611l);
350 for (int i = 0; i < 100; ++i) {
351 FieldRotation<DerivativeStructure> r = randomRotation(random);
352 FieldVector3D<DerivativeStructure> o = randomVector(random, 1.0e-3);
353 FieldVector3D<DerivativeStructure> a = randomVector(random, 1.0e-3);
354 FieldAngularCoordinates<DerivativeStructure> ac = new FieldAngularCoordinates<>(r, o, a);
355 FieldAngularCoordinates<DerivativeStructure> sum = ac.addOffset(ac.revert());
356 Assertions.assertEquals(0.0, sum.getRotation().getAngle().getReal(), 1.0e-15);
357 Assertions.assertEquals(0.0, sum.getRotationRate().getNorm().getReal(), 1.0e-15);
358 }
359 }
360
361 @Test
362 public void testNoCommute() {
363 FieldAngularCoordinates<DerivativeStructure> ac1 =
364 new FieldAngularCoordinates<>(createRotation(0.48, 0.64, 0.36, 0.48, false),
365 createVector(0, 0, 0, 4),
366 createVector(0, 0, 0, 4));
367 FieldAngularCoordinates<DerivativeStructure> ac2 =
368 new FieldAngularCoordinates<>(createRotation(0.36, -0.48, 0.48, 0.64, false),
369 createVector(0, 0, 0, 4),
370 createVector(0, 0, 0, 4));
371
372 FieldAngularCoordinates<DerivativeStructure> add12 = ac1.addOffset(ac2);
373 FieldAngularCoordinates<DerivativeStructure> add21 = ac2.addOffset(ac1);
374
375
376 Assertions.assertEquals(2.574, FieldRotation.distance(add12.getRotation(), add21.getRotation()).getReal(), 1.0e-3);
377
378 }
379
380 @Test
381 public void testRoundTripNoOp() {
382 RandomGenerator random = new Well1024a(0x1e610cfe89306669l);
383 for (int i = 0; i < 100; ++i) {
384
385 FieldRotation<DerivativeStructure> r1 = randomRotation(random);
386 FieldVector3D<DerivativeStructure> o1 = randomVector(random, 1.0e-2);
387 FieldVector3D<DerivativeStructure> a1 = randomVector(random, 1.0e-2);
388 FieldAngularCoordinates<DerivativeStructure> ac1 = new FieldAngularCoordinates<>(r1, o1, a1);
389
390 FieldRotation<DerivativeStructure> r2 = randomRotation(random);
391 FieldVector3D<DerivativeStructure> o2 = randomVector(random, 1.0e-2);
392 FieldVector3D<DerivativeStructure> a2 = randomVector(random, 1.0e-2);
393 FieldAngularCoordinates<DerivativeStructure> ac2 = new FieldAngularCoordinates<>(r2, o2, a2);
394
395 FieldAngularCoordinates<DerivativeStructure> roundTripSA = ac1.subtractOffset(ac2).addOffset(ac2);
396 Assertions.assertEquals(0.0, FieldRotation.distance(ac1.getRotation(), roundTripSA.getRotation()).getReal(), 1.0e-15);
397 Assertions.assertEquals(0.0, FieldVector3D.distance(ac1.getRotationRate(), roundTripSA.getRotationRate()).getReal(), 2.0e-17);
398 Assertions.assertEquals(0.0, FieldVector3D.distance(ac1.getRotationAcceleration(), roundTripSA.getRotationAcceleration()).getReal(), 2.0e-17);
399
400 FieldAngularCoordinates<DerivativeStructure> roundTripAS = ac1.addOffset(ac2).subtractOffset(ac2);
401 Assertions.assertEquals(0.0, FieldRotation.distance(ac1.getRotation(), roundTripAS.getRotation()).getReal(), 1.0e-15);
402 Assertions.assertEquals(0.0, FieldVector3D.distance(ac1.getRotationRate(), roundTripAS.getRotationRate()).getReal(), 2.0e-17);
403 Assertions.assertEquals(0.0, FieldVector3D.distance(ac1.getRotationAcceleration(), roundTripAS.getRotationAcceleration()).getReal(), 2.0e-17);
404 }
405 }
406
407 @Test
408 public void testResultAngularCoordinates() {
409 Field<Binary64> field = Binary64Field.getInstance();
410 Binary64 zero = field.getZero();
411 FieldVector3D<Binary64> pos_B = new FieldVector3D<>(zero.add(-0.23723922134606962 ),
412 zero.add(-0.9628700341496187 ),
413 zero.add(0.1288365211879871 ));
414 FieldVector3D<Binary64> vel_B = new FieldVector3D<>(zero.add(2.6031808214929053E-7 ),
415 zero.add(-8.141147978260352E-8 ),
416 zero.add(-1.2908618653852553E-7 ));
417 FieldVector3D<Binary64> acc_B = new FieldVector3D<>(zero.add( -1.395403347295246E-10 ),
418 zero.add( -2.7451871050415643E-12),
419 zero.add( -2.781723303703499E-10 ));
420
421 FieldPVCoordinates<Binary64> B = new FieldPVCoordinates<Binary64>(pos_B, vel_B, acc_B);
422
423
424 FieldVector3D<Binary64> pos_A = new FieldVector3D<>(zero.add(-0.44665912825286425 ),
425 zero.add(-0.00965737694923173 ),
426 zero.add(-0.894652087807798 ));
427 FieldVector3D<Binary64> vel_A = new FieldVector3D<>(zero.add(-8.897373390367405E-4),
428 zero.add(2.7825509772757976E-4),
429 zero.add(4.412017757970883E-4 ));
430 FieldVector3D<Binary64> acc_A = new FieldVector3D<>(zero.add( 4.743595125825107E-7),
431 zero.add( 1.01875177357042E-8 ),
432 zero.add( 9.520371766790574E-7));
433
434 FieldPVCoordinates<Binary64> A = new FieldPVCoordinates<>(pos_A, vel_A, acc_A);
435
436 FieldPVCoordinates<Binary64> PLUS_K = new FieldPVCoordinates<>(new FieldVector3D<>(field.getZero(), field.getZero(), field.getOne()),
437 new FieldVector3D<>(field.getZero(), field.getZero(), field.getZero()),
438 new FieldVector3D<>(field.getZero(), field.getZero(), field.getZero()));
439
440 FieldPVCoordinates<Binary64> PLUS_J = new FieldPVCoordinates<>(new FieldVector3D<>(field.getZero(), field.getOne(), field.getZero()),
441 new FieldVector3D<>(field.getZero(), field.getZero(), field.getZero()),
442 new FieldVector3D<>(field.getZero(), field.getZero(), field.getZero()));
443
444
445 FieldAngularCoordinates<Binary64> fac = new FieldAngularCoordinates<>(A, B, PLUS_K, PLUS_J, 1.0e-6);
446
447 AngularCoordinates ac = new AngularCoordinates(A.toPVCoordinates(), B.toPVCoordinates(), PLUS_K.toPVCoordinates(), PLUS_J.toPVCoordinates(), 1.0e-6);
448
449 Assertions.assertTrue( fac.getRotationRate().toVector3D().equals(ac.getRotationRate()));
450
451 }
452
453 @Test
454 public void testIdentity() {
455 FieldAngularCoordinates<Binary64> identity = FieldAngularCoordinates.getIdentity(Binary64Field.getInstance());
456 Assertions.assertEquals(0.0,
457 identity.getRotation().getAngle().getReal(),
458 1.0e-15);
459 Assertions.assertEquals(0.0,
460 identity.getRotationRate().getNorm().getReal(),
461 1.0e-15);
462 Assertions.assertEquals(0.0,
463 identity.getRotationAcceleration().getNorm().getReal(),
464 1.0e-15);
465 }
466
467 @Test
468 public void testConversionConstructor() {
469 AngularCoordinates ac = new AngularCoordinates(new Rotation(Vector3D.MINUS_J, 0.15, RotationConvention.VECTOR_OPERATOR),
470 new Vector3D(0.001, 0.002, 0.003),
471 new Vector3D(-1.0e-6, -3.0e-6, 7.0e-6));
472 FieldAngularCoordinates<Binary64> ac64 = new FieldAngularCoordinates<>(Binary64Field.getInstance(), ac);
473 Assertions.assertEquals(0.0,
474 Rotation.distance(ac.getRotation(), ac64.getRotation().toRotation()),
475 1.0e-15);
476 Assertions.assertEquals(0.0,
477 Vector3D.distance(ac.getRotationRate(), ac64.getRotationRate().toVector3D()),
478 1.0e-15);
479 Assertions.assertEquals(0.0,
480 Vector3D.distance(ac.getRotationAcceleration(), ac64.getRotationAcceleration().toVector3D()),
481 1.0e-15);
482 }
483
484 @Test
485 public void testApplyTo() {
486
487 RandomGenerator random = new Well1024a(0xbad5894f4c475905l);
488 for (int i = 0; i < 1000; ++i) {
489 FieldRotation<DerivativeStructure> rotation = randomRotation(random);
490 FieldVector3D<DerivativeStructure> rotationRate = randomVector(random, 0.01);
491 FieldVector3D<DerivativeStructure> rotationAcceleration = randomVector(random, 0.01);
492 FieldAngularCoordinates<DerivativeStructure> ac = new FieldAngularCoordinates<>(rotation,
493 rotationRate,
494 rotationAcceleration);
495
496 FieldVector3D<DerivativeStructure> p = randomVector(random, 10.0);
497 FieldVector3D<DerivativeStructure> v = randomVector(random, 10.0);
498 FieldVector3D<DerivativeStructure> a = randomVector(random, 10.0);
499 FieldPVCoordinates<DerivativeStructure> pv = new FieldPVCoordinates<>(p, v, a);
500
501 PVCoordinates reference = ac.toAngularCoordinates().applyTo(pv.toPVCoordinates());
502
503 FieldPVCoordinates<DerivativeStructure> res1 = ac.applyTo(pv.toPVCoordinates());
504 Assertions.assertEquals(0.0, Vector3D.distance(reference.getPosition(), res1.getPosition().toVector3D()), 1.0e-15);
505 Assertions.assertEquals(0.0, Vector3D.distance(reference.getVelocity(), res1.getVelocity().toVector3D()), 1.0e-15);
506 Assertions.assertEquals(0.0, Vector3D.distance(reference.getAcceleration(), res1.getAcceleration().toVector3D()), 1.0e-15);
507
508 FieldPVCoordinates<DerivativeStructure> res2 = ac.applyTo(pv);
509 Assertions.assertEquals(0.0, Vector3D.distance(reference.getPosition(), res2.getPosition().toVector3D()), 1.0e-15);
510 Assertions.assertEquals(0.0, Vector3D.distance(reference.getVelocity(), res2.getVelocity().toVector3D()), 1.0e-15);
511 Assertions.assertEquals(0.0, Vector3D.distance(reference.getAcceleration(), res2.getAcceleration().toVector3D()), 1.0e-15);
512
513
514 TimeStampedFieldPVCoordinates<DerivativeStructure> res3 =
515 ac.applyTo(new TimeStampedFieldPVCoordinates<>(AbsoluteDate.J2000_EPOCH, pv));
516 Assertions.assertEquals(0.0, AbsoluteDate.J2000_EPOCH.durationFrom(res3.getDate().toAbsoluteDate()), 1.0e-15);
517 Assertions.assertEquals(0.0, Vector3D.distance(reference.getPosition(), res3.getPosition().toVector3D()), 1.0e-15);
518 Assertions.assertEquals(0.0, Vector3D.distance(reference.getVelocity(), res3.getVelocity().toVector3D()), 1.0e-15);
519 Assertions.assertEquals(0.0, Vector3D.distance(reference.getAcceleration(), res3.getAcceleration().toVector3D()), 1.0e-15);
520
521 TimeStampedFieldPVCoordinates<DerivativeStructure> res4 =
522 ac.applyTo(new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH, pv.toPVCoordinates()));
523 Assertions.assertEquals(0.0, AbsoluteDate.J2000_EPOCH.durationFrom(res4.getDate().toAbsoluteDate()), 1.0e-15);
524 Assertions.assertEquals(0.0, Vector3D.distance(reference.getPosition(), res4.getPosition().toVector3D()), 1.0e-15);
525 Assertions.assertEquals(0.0, Vector3D.distance(reference.getVelocity(), res4.getVelocity().toVector3D()), 1.0e-15);
526 Assertions.assertEquals(0.0, Vector3D.distance(reference.getAcceleration(), res4.getAcceleration().toVector3D()), 1.0e-15);
527
528 }
529
530 }
531
532 @Test
533 public void testRodriguesVsDouble() {
534
535 RandomGenerator random = new Well1024a(0x4beeee3d8d2abacal);
536 for (int i = 0; i < 1000; ++i) {
537 FieldRotation<DerivativeStructure> rotation = randomRotation(random);
538 FieldVector3D<DerivativeStructure> rotationRate = randomVector(random, 0.01);
539 FieldVector3D<DerivativeStructure> rotationAcceleration = randomVector(random, 0.01);
540 FieldAngularCoordinates<DerivativeStructure> ac = new FieldAngularCoordinates<>(rotation, rotationRate, rotationAcceleration);
541
542 DerivativeStructure[][] rod = ac.getModifiedRodrigues(1.0);
543 double[][] rodRef = ac.toAngularCoordinates().getModifiedRodrigues(1.0);
544 Assertions.assertEquals(rodRef.length, rod.length);
545 for (int k = 0; k < rodRef.length; ++k) {
546 Assertions.assertEquals(rodRef[k].length, rod[k].length);
547 for (int l = 0; l < rodRef[k].length; ++l) {
548 Assertions.assertEquals(rodRef[k][l], rod[k][l].getReal(), 1.0e-15 * FastMath.abs(rodRef[k][l]));
549 }
550 }
551
552 FieldAngularCoordinates<DerivativeStructure> rebuilt = FieldAngularCoordinates.createFromModifiedRodrigues(rod);
553 AngularCoordinates rebuiltRef = AngularCoordinates.createFromModifiedRodrigues(rodRef);
554 Assertions.assertEquals(0.0, Rotation.distance(rebuiltRef.getRotation(), rebuilt.getRotation().toRotation()), 1.0e-14);
555 Assertions.assertEquals(0.0, Vector3D.distance(rebuiltRef.getRotationRate(), rebuilt.getRotationRate().toVector3D()), 1.0e-15);
556 Assertions.assertEquals(0.0, Vector3D.distance(rebuiltRef.getRotationAcceleration(), rebuilt.getRotationAcceleration().toVector3D()), 1.0e-15);
557
558 }
559
560 }
561
562 @Test
563 public void testRodriguesSymmetry() {
564
565
566 RandomGenerator random = new Well1024a(0xb1e615aaa8236b52l);
567 for (int i = 0; i < 1000; ++i) {
568 FieldRotation<DerivativeStructure> rotation = randomRotation(random);
569 FieldVector3D<DerivativeStructure> rotationRate = randomVector(random, 0.01);
570 FieldVector3D<DerivativeStructure> rotationAcceleration = randomVector(random, 0.01);
571 FieldAngularCoordinates<DerivativeStructure> ac = new FieldAngularCoordinates<>(rotation, rotationRate, rotationAcceleration);
572 FieldAngularCoordinates<DerivativeStructure> rebuilt = FieldAngularCoordinates.createFromModifiedRodrigues(ac.getModifiedRodrigues(1.0));
573 Assertions.assertEquals(0.0, FieldRotation.distance(rotation, rebuilt.getRotation()).getReal(), 1.0e-14);
574 Assertions.assertEquals(0.0, FieldVector3D.distance(rotationRate, rebuilt.getRotationRate()).getReal(), 1.0e-15);
575 Assertions.assertEquals(0.0, FieldVector3D.distance(rotationAcceleration, rebuilt.getRotationAcceleration()).getReal(), 1.0e-15);
576 }
577
578 }
579
580 @Test
581 public void testRodriguesSpecialCases() {
582
583
584 DerivativeStructure[][] identity = FieldAngularCoordinates.getIdentity(new DSFactory(2, 2).getDerivativeField()).getModifiedRodrigues(1.0);
585 for (DerivativeStructure[] row : identity) {
586 for (DerivativeStructure element : row) {
587 Assertions.assertEquals(0.0, element.getReal(), Precision.SAFE_MIN);
588 }
589 }
590 FieldAngularCoordinates<DerivativeStructure> acId = FieldAngularCoordinates.createFromModifiedRodrigues(identity);
591 Assertions.assertEquals(0.0, acId.getRotation().getAngle().getReal(), Precision.SAFE_MIN);
592 Assertions.assertEquals(0.0, acId.getRotationRate().getNorm().getReal(), Precision.SAFE_MIN);
593
594
595 RandomGenerator random = new Well1024a(0x4923ec495bca9fb4l);
596 for (int i = 0; i < 100; ++i) {
597 FieldVector3D<DerivativeStructure> axis = randomVector(random, 1.0);
598 final Field<DerivativeStructure> field = axis.getX().getField();
599 FieldAngularCoordinates<DerivativeStructure> original =
600 new FieldAngularCoordinates<>(new FieldRotation<>(axis, field.getZero().add(FastMath.PI), RotationConvention.VECTOR_OPERATOR),
601 FieldVector3D.getZero(field),
602 FieldVector3D.getZero(field));
603 FieldAngularCoordinates<DerivativeStructure> rebuilt = FieldAngularCoordinates.createFromModifiedRodrigues(original.getModifiedRodrigues(1.0));
604 Assertions.assertEquals(FastMath.PI, rebuilt.getRotation().getAngle().getReal(), 1.0e-15);
605 Assertions.assertEquals(0.0, FieldVector3D.angle(axis, rebuilt.getRotation().getAxis(RotationConvention.VECTOR_OPERATOR)).sin().getReal(), 1.0e-15);
606 Assertions.assertEquals(0.0, rebuilt.getRotationRate().getNorm().getReal(), 1.0e-16);
607 }
608
609 }
610
611 @Test
612 public void testInverseCrossProducts()
613 {
614 Binary64Field field = Binary64Field.getInstance();
615 checkInverse(FieldVector3D.getPlusK(field), FieldVector3D.getPlusI(field), FieldVector3D.getPlusJ(field));
616 checkInverse(FieldVector3D.getZero(field), FieldVector3D.getZero(field), FieldVector3D.getZero(field));
617 checkInverse(FieldVector3D.getZero(field), FieldVector3D.getZero(field), FieldVector3D.getPlusJ(field));
618 checkInverse(FieldVector3D.getPlusK(field), FieldVector3D.getPlusK(field), FieldVector3D.getPlusJ(field));
619 checkInverse(FieldVector3D.getZero(field), FieldVector3D.getPlusK(field), FieldVector3D.getZero(field));
620 checkInverse(FieldVector3D.getPlusK(field), FieldVector3D.getPlusI(field), FieldVector3D.getPlusK(field));
621 checkInverse(FieldVector3D.getPlusK(field), FieldVector3D.getPlusI(field), FieldVector3D.getPlusI(field));
622 checkInverse(FieldVector3D.getPlusK(field), FieldVector3D.getPlusI(field), new FieldVector3D<Binary64>(field, new Vector3D(1, 0, -1)).normalize());
623 checkInverse(FieldVector3D.getZero(field), FieldVector3D.getPlusI(field), FieldVector3D.getZero(field), FieldVector3D.getPlusJ(field), FieldVector3D.getZero(field));
624 }
625
626 @Test
627 public void testInverseCrossProductsFailures() {
628 Binary64Field field = Binary64Field.getInstance();
629 checkInverseFailure(FieldVector3D.getPlusK(field), FieldVector3D.getZero(field), FieldVector3D.getPlusJ(field), FieldVector3D.getPlusI(field), FieldVector3D.getPlusK(field));
630 checkInverseFailure(FieldVector3D.getPlusK(field), FieldVector3D.getZero(field), FieldVector3D.getZero(field), FieldVector3D.getZero(field), FieldVector3D.getPlusK(field));
631 checkInverseFailure(FieldVector3D.getPlusI(field), FieldVector3D.getPlusI(field), FieldVector3D.getZero(field), FieldVector3D.getMinusI(field), FieldVector3D.getPlusK(field));
632 checkInverseFailure(FieldVector3D.getPlusI(field), FieldVector3D.getPlusI(field), FieldVector3D.getZero(field), FieldVector3D.getPlusJ(field), FieldVector3D.getPlusJ(field));
633 checkInverseFailure(FieldVector3D.getPlusI(field), FieldVector3D.getPlusI(field), FieldVector3D.getPlusJ(field), FieldVector3D.getPlusJ(field), FieldVector3D.getZero(field));
634 checkInverseFailure(FieldVector3D.getPlusI(field), FieldVector3D.getPlusI(field), FieldVector3D.getPlusJ(field), FieldVector3D.getZero(field), FieldVector3D.getPlusJ(field));
635 }
636
637 @Test
638 public void testRandomInverseCrossProducts() {
639 RandomGenerator generator = new Well1024a(0xda0ee5b245efd438l);
640 for (int i = 0; i < 10000; ++i) {
641 FieldVector3D<DerivativeStructure> omega = randomVector(generator, 10 * generator.nextDouble() + 1.0);
642 FieldVector3D<DerivativeStructure> v1 = randomVector(generator, 10 * generator.nextDouble() + 1.0);
643 FieldVector3D<DerivativeStructure> v2 = randomVector(generator, 10 * generator.nextDouble() + 1.0);
644 checkInverse(omega, v1, v2);
645 }
646 }
647
648 @Test
649 public void testRandomPVCoordinates() {
650 RandomGenerator generator = new Well1024a(0xf978035a328a565bl);
651 for (int i = 0; i < 100; ++i) {
652 FieldRotation<DerivativeStructure> r = randomRotation(generator);
653 FieldVector3D<DerivativeStructure> omega = randomVector(generator, 10 * generator.nextDouble() + 1.0);
654 FieldVector3D<DerivativeStructure> omegaDot = randomVector(generator, 0.1 * generator.nextDouble() + 0.01);
655 FieldAngularCoordinates<DerivativeStructure> ref = new FieldAngularCoordinates<>(r, omega, omegaDot);
656 FieldAngularCoordinates<DerivativeStructure> inv = ref.revert();
657 for (int j = 0; j < 100; ++j) {
658 FieldPVCoordinates<DerivativeStructure> v1 = randomPVCoordinates(generator, 1000, 1.0, 0.001);
659 FieldPVCoordinates<DerivativeStructure> v2 = randomPVCoordinates(generator, 1000, 1.0, 0.0010);
660 FieldPVCoordinates<DerivativeStructure> u1 = inv.applyTo(v1);
661 FieldPVCoordinates<DerivativeStructure> u2 = inv.applyTo(v2);
662 FieldAngularCoordinates<DerivativeStructure> rebuilt = new FieldAngularCoordinates<>(u1, u2, v1, v2, 1.0e-9);
663 Assertions.assertEquals(0.0,
664 FieldRotation.distance(r, rebuilt.getRotation()).getReal(),
665 6.0e-14);
666 Assertions.assertEquals(0.0,
667 FieldVector3D.distance(omega, rebuilt.getRotationRate()).getReal(),
668 3.0e-12 * omega.getNorm().getReal());
669 Assertions.assertEquals(0.0,
670 FieldVector3D.distance(omegaDot, rebuilt.getRotationAcceleration()).getReal(),
671 2.0e-6 * omegaDot.getNorm().getReal());
672 }
673 }
674 }
675
676 @Test
677 public void testCancellingDerivatives() {
678 PVCoordinates u1 = new PVCoordinates(new Vector3D(-0.4466591282528639, -0.009657376949231283, -0.894652087807798),
679 new Vector3D(-8.897296517803556E-4, 2.7825250920407674E-4, 4.411979658413134E-4),
680 new Vector3D( 4.753127475302486E-7, 1.0209400376727623E-8, 9.515403756524403E-7));
681 PVCoordinates u2 = new PVCoordinates(new Vector3D( 0.23723907259910096, 0.9628700806685033, -0.1288364474275361),
682 new Vector3D(-7.98741002062555E-24, 2.4979687659429984E-24, 3.9607863426704016E-24),
683 new Vector3D(-3.150541868418562E-23, 9.856329862034835E-24, 1.5648124883326986E-23));
684 PVCoordinates v1 = new PVCoordinates(Vector3D.PLUS_K, Vector3D.ZERO, Vector3D.ZERO);
685 PVCoordinates v2 = new PVCoordinates(Vector3D.MINUS_J, Vector3D.ZERO, Vector3D.ZERO);
686 AngularCoordinates ac = new AngularCoordinates(u1, u2, v1, v2, 1.0e-9);
687 PVCoordinates v1Computed = ac.applyTo(u1);
688 PVCoordinates v2Computed = ac.applyTo(u2);
689 Assertions.assertEquals(0, Vector3D.distance(v1.getPosition(), v1Computed.getPosition()), 1.0e-15);
690 Assertions.assertEquals(0, Vector3D.distance(v2.getPosition(), v2Computed.getPosition()), 1.0e-15);
691 Assertions.assertEquals(0, Vector3D.distance(v1.getVelocity(), v1Computed.getVelocity()), 1.0e-15);
692 Assertions.assertEquals(0, Vector3D.distance(v2.getVelocity(), v2Computed.getVelocity()), 1.0e-15);
693 Assertions.assertEquals(0, Vector3D.distance(v1.getAcceleration(), v1Computed.getAcceleration()), 1.0e-15);
694 Assertions.assertEquals(0, Vector3D.distance(v2.getAcceleration(), v2Computed.getAcceleration()), 1.0e-15);
695 }
696
697 private <T extends CalculusFieldElement<T>> void checkInverse(FieldVector3D<T> omega, FieldVector3D<T> v1, FieldVector3D<T> v2)
698 {
699 checkInverse(omega,
700 v1, FieldVector3D.crossProduct(omega, v1),
701 v2, FieldVector3D.crossProduct(omega, v2));
702 }
703
704 private <T extends CalculusFieldElement<T>> void checkInverseFailure(FieldVector3D<T> omega,
705 FieldVector3D<T> v1, FieldVector3D<T> c1,
706 FieldVector3D<T> v2, FieldVector3D<T> c2) {
707 try {
708 checkInverse(omega, v1, c1, v2, c2);
709 Assertions.fail("an exception should have been thrown");
710 } catch (MathIllegalArgumentException miae) {
711
712 }
713 }
714
715 private <T extends CalculusFieldElement<T>> void checkInverse(FieldVector3D<T> omega,
716 FieldVector3D<T> v1, FieldVector3D<T> c1,
717 FieldVector3D<T> v2, FieldVector3D<T> c2)
718 throws MathIllegalArgumentException {
719 try {
720 Method inverse;
721 inverse = FieldAngularCoordinates.class.getDeclaredMethod("inverseCrossProducts",
722 FieldVector3D.class, FieldVector3D.class,
723 FieldVector3D.class, FieldVector3D.class,
724 Double.TYPE);
725 inverse.setAccessible(true);
726 @SuppressWarnings("unchecked")
727 FieldVector3D<T> rebuilt = (FieldVector3D<T>) inverse.invoke(null, v1, c1, v2, c2, 1.0e-9);
728 Assertions.assertEquals(0.0, FieldVector3D.distance(omega, rebuilt).getReal(), 5.0e-12 * omega.getNorm().getReal());
729 } catch (NoSuchMethodException e) {
730 Assertions.fail(e.getLocalizedMessage());
731 } catch (SecurityException e) {
732 Assertions.fail(e.getLocalizedMessage());
733 } catch (IllegalAccessException e) {
734 Assertions.fail(e.getLocalizedMessage());
735 } catch (IllegalArgumentException e) {
736 Assertions.fail(e.getLocalizedMessage());
737 } catch (InvocationTargetException e) {
738 throw (MathIllegalArgumentException) e.getCause();
739 }
740 }
741
742 private FieldVector3D<DerivativeStructure> randomVector(RandomGenerator random, double norm) {
743 double n = random.nextDouble() * norm;
744 double x = random.nextDouble();
745 double y = random.nextDouble();
746 double z = random.nextDouble();
747 return new FieldVector3D<>(n, createVector(x, y, z, 4).normalize());
748 }
749
750 private FieldPVCoordinates<DerivativeStructure> randomPVCoordinates(RandomGenerator random,
751 double norm0, double norm1, double norm2) {
752 FieldVector3D<DerivativeStructure> p0 = randomVector(random, norm0);
753 FieldVector3D<DerivativeStructure> p1 = randomVector(random, norm1);
754 FieldVector3D<DerivativeStructure> p2 = randomVector(random, norm2);
755 return new FieldPVCoordinates<>(p0, p1, p2);
756 }
757
758 private FieldRotation<DerivativeStructure> randomRotation(RandomGenerator random) {
759 double q0 = random.nextDouble() * 2 - 1;
760 double q1 = random.nextDouble() * 2 - 1;
761 double q2 = random.nextDouble() * 2 - 1;
762 double q3 = random.nextDouble() * 2 - 1;
763 double q = FastMath.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
764 return createRotation(q0 / q, q1 / q, q2 / q, q3 / q, false);
765 }
766
767 private FieldRotation<DerivativeStructure> createRotation(double q0, double q1, double q2, double q3,
768 boolean needsNormalization) {
769 DSFactory factory = new DSFactory(4, 1);
770 return new FieldRotation<>(factory.variable(0, q0),
771 factory.variable(1, q1),
772 factory.variable(2, q2),
773 factory.variable(3, q3),
774 needsNormalization);
775 }
776
777 private FieldVector3D<DerivativeStructure> createVector(double x, double y, double z, int params) {
778 DSFactory factory = new DSFactory(params, 1);
779 return new FieldVector3D<>(factory.variable(0, x),
780 factory.variable(1, y),
781 factory.variable(2, z));
782 }
783
784 private FieldRotation<Binary64> randomRotation64(RandomGenerator random) {
785 double q0 = random.nextDouble() * 2 - 1;
786 double q1 = random.nextDouble() * 2 - 1;
787 double q2 = random.nextDouble() * 2 - 1;
788 double q3 = random.nextDouble() * 2 - 1;
789 double q = FastMath.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
790 return new FieldRotation<>(new Binary64(q0 / q),
791 new Binary64(q1 / q),
792 new Binary64(q2 / q),
793 new Binary64(q3 / q),
794 false);
795 }
796
797 private FieldVector3D<Binary64> randomVector64(RandomGenerator random, double norm) {
798 double n = random.nextDouble() * norm;
799 double x = random.nextDouble();
800 double y = random.nextDouble();
801 double z = random.nextDouble();
802 return new FieldVector3D<>(n, new FieldVector3D<>(new Binary64(x), new Binary64(y), new Binary64(z)).normalize());
803 }
804
805 }