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