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