1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23 package org.hipparchus.geometry.euclidean.threed;
24
25 import org.hipparchus.dfp.Dfp;
26 import org.hipparchus.dfp.DfpField;
27 import org.hipparchus.exception.MathIllegalArgumentException;
28 import org.hipparchus.exception.MathIllegalStateException;
29 import org.hipparchus.exception.MathRuntimeException;
30 import org.hipparchus.geometry.LocalizedGeometryFormats;
31 import org.hipparchus.random.UnitSphereRandomVectorGenerator;
32 import org.hipparchus.random.Well1024a;
33 import org.hipparchus.util.FastMath;
34 import org.hipparchus.util.MathUtils;
35 import org.junit.jupiter.api.AfterEach;
36 import org.junit.jupiter.api.BeforeEach;
37 import org.junit.jupiter.api.Test;
38
39 import java.util.Arrays;
40
41 import static org.junit.jupiter.api.Assertions.assertEquals;
42 import static org.junit.jupiter.api.Assertions.assertTrue;
43 import static org.junit.jupiter.api.Assertions.fail;
44
45 class FieldRotationDfpTest {
46
47 @Test
48 void testIssue304Cardan() {
49 for (final RotationConvention convention : RotationConvention.values()) {
50 for (final RotationOrder order : Arrays.asList(RotationOrder.XYZ,
51 RotationOrder.XZY,
52 RotationOrder.YXZ,
53 RotationOrder.YZX,
54 RotationOrder.ZXY,
55 RotationOrder.ZYX)) {
56
57
58 FieldRotation<Dfp> singularPlus =
59 new FieldRotation<>(order, convention,
60 field.getZero(), field.getPi().multiply(0.5), field.getZero().newInstance(0.125));
61 assertEquals(0.0, singularPlus.getAngles(order, convention)[0].getReal(), 1.0e-20);
62 assertEquals(MathUtils.SEMI_PI, singularPlus.getAngles(order, convention)[1].getReal(), 1.0e-20);
63 assertEquals(0.125, singularPlus.getAngles(order, convention)[2].getReal(), 1.0e-20);
64
65
66 FieldRotation<Dfp> singularMinus =
67 new FieldRotation<>(order, convention,
68 field.getZero(), field.getPi().multiply(-0.5), field.getZero().newInstance(0.125));
69 assertEquals(0.0, singularMinus.getAngles(order, convention)[0].getReal(), 1.0e-20);
70 assertEquals(-MathUtils.SEMI_PI, singularMinus.getAngles(order, convention)[1].getReal(), 1.0e-20);
71 assertEquals(0.125, singularMinus.getAngles(order, convention)[2].getReal(), 1.0e-20);
72
73 }
74 }
75 }
76
77 @Test
78 void testIssue304Euler() {
79 for (final RotationConvention convention : RotationConvention.values()) {
80 for (final RotationOrder order : Arrays.asList(RotationOrder.XYX,
81 RotationOrder.XZX,
82 RotationOrder.YXY,
83 RotationOrder.YZY,
84 RotationOrder.ZXZ,
85 RotationOrder.ZYZ)) {
86
87
88 FieldRotation<Dfp> singularZero =
89 new FieldRotation<>(order, convention,
90 field.getZero().newInstance(0.125), field.getZero(), field.getZero() );
91 assertEquals(0.125, singularZero.getAngles(order, convention)[0].getReal(), 1.0e-20);
92 assertEquals(0.0, singularZero.getAngles(order, convention)[1].getReal(), 1.0e-20);
93 assertEquals(0.0, singularZero.getAngles(order, convention)[2].getReal(), 1.0e-20);
94
95
96 FieldRotation<Dfp> singularPi =
97 new FieldRotation<>(order, convention,
98 field.getZero().newInstance(0.125), field.getPi(), field.getZero());
99 assertEquals(0.125, singularPi.getAngles(order, convention)[0].getReal(), 1.0e-20);
100 assertEquals(FastMath.PI, singularPi.getAngles(order, convention)[1].getReal(), 1.0e-20);
101 assertEquals(0.0, singularPi.getAngles(order, convention)[2].getReal(), 1.0e-20);
102
103 }
104 }
105 }
106
107 @Test
108 void testIdentity() {
109
110 FieldRotation<Dfp> r = createRotation(1, 0, 0, 0, false);
111 checkVector(r.applyTo(createVector(1, 0, 0)), createVector(1, 0, 0));
112 checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 1, 0));
113 checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 0, 1));
114 checkAngle(r.getAngle(), 0);
115
116 r = createRotation(-1, 0, 0, 0, false);
117 checkVector(r.applyTo(createVector(1, 0, 0)), createVector(1, 0, 0));
118 checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 1, 0));
119 checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 0, 1));
120 checkAngle(r.getAngle(), 0);
121
122 r = createRotation(42, 0, 0, 0, true);
123 checkVector(r.applyTo(createVector(1, 0, 0)), createVector(1, 0, 0));
124 checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 1, 0));
125 checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 0, 1));
126 checkAngle(r.getAngle(), 0);
127
128 r = FieldRotation.getIdentity(new DfpField(20));
129 checkVector(r.applyTo(createVector(1, 0, 0)), createVector(1, 0, 0));
130 checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 1, 0));
131 checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 0, 1));
132 checkAngle(r.getAngle(), 0);
133
134 }
135
136 @Test
137 void testAxisAngleVectorOperator() throws MathIllegalArgumentException {
138
139 FieldRotation<Dfp> r = new FieldRotation<>(createAxis(10, 10, 10),
140 createAngle(2 * FastMath.PI / 3) ,
141 RotationConvention.VECTOR_OPERATOR);
142 checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 1, 0));
143 checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 0, 1));
144 checkVector(r.applyTo(createVector(0, 0, 1)), createVector(1, 0, 0));
145 double s = 1 / FastMath.sqrt(3);
146 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector( s, s, s));
147 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(-s, -s, -s));
148 checkAngle(r.getAngle(), 2 * FastMath.PI / 3);
149
150 try {
151 new FieldRotation<>(createAxis(0, 0, 0),
152 createAngle(2 * FastMath.PI / 3),
153 RotationConvention.VECTOR_OPERATOR);
154 fail("an exception should have been thrown");
155 } catch (MathIllegalArgumentException e) {
156 assertEquals(LocalizedGeometryFormats.ZERO_NORM_FOR_ROTATION_AXIS, e.getSpecifier());
157 }
158
159 r = new FieldRotation<>(createAxis(0, 0, 1),
160 createAngle(1.5 * FastMath.PI),
161 RotationConvention.VECTOR_OPERATOR);
162 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, 0, -1));
163 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, 0, +1));
164 checkAngle(r.getAngle(), MathUtils.SEMI_PI);
165
166 r = new FieldRotation<>(createAxis(0, 1, 0),
167 createAngle(FastMath.PI),
168 RotationConvention.VECTOR_OPERATOR);
169 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, +1, 0));
170 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, -1, 0));
171 checkAngle(r.getAngle(), FastMath.PI);
172
173 checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.VECTOR_OPERATOR), createVector(+1, 0, 0));
174 checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.FRAME_TRANSFORM), createVector(-1, 0, 0));
175
176 }
177
178 @Test
179 void testAxisAngleFrameTransform() throws MathIllegalArgumentException {
180
181 FieldRotation<Dfp> r = new FieldRotation<>(createAxis(10, 10, 10),
182 createAngle(2 * FastMath.PI / 3) ,
183 RotationConvention.FRAME_TRANSFORM);
184 checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 0, 1));
185 checkVector(r.applyTo(createVector(0, 1, 0)), createVector(1, 0, 0));
186 checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 1, 0));
187 double s = 1 / FastMath.sqrt(3);
188 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector( s, s, s));
189 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(-s, -s, -s));
190 checkAngle(r.getAngle(), 2 * FastMath.PI / 3);
191
192 try {
193 new FieldRotation<>(createAxis(0, 0, 0),
194 createAngle(2 * FastMath.PI / 3),
195 RotationConvention.FRAME_TRANSFORM);
196 fail("an exception should have been thrown");
197 } catch (MathIllegalArgumentException e) {
198 assertEquals(LocalizedGeometryFormats.ZERO_NORM_FOR_ROTATION_AXIS, e.getSpecifier());
199 }
200
201 r = new FieldRotation<>(createAxis(0, 0, 1),
202 createAngle(1.5 * FastMath.PI),
203 RotationConvention.FRAME_TRANSFORM);
204 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, 0, -1));
205 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, 0, +1));
206 checkAngle(r.getAngle(), MathUtils.SEMI_PI);
207
208 r = new FieldRotation<>(createAxis(0, 1, 0),
209 createAngle(FastMath.PI),
210 RotationConvention.FRAME_TRANSFORM);
211 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, +1, 0));
212 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, -1, 0));
213 checkAngle(r.getAngle(), FastMath.PI);
214
215 checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.FRAME_TRANSFORM), createVector(-1, 0, 0));
216 checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.VECTOR_OPERATOR), createVector(+1, 0, 0));
217
218 }
219
220 @Test
221 void testRevert() {
222 double a = 0.001;
223 double b = 0.36;
224 double c = 0.48;
225 double d = 0.8;
226 FieldRotation<Dfp> r = createRotation(a, b, c, d, true);
227 FieldRotation<Dfp> reverted = r.revert();
228 FieldRotation<Dfp> rrT = r.applyTo(reverted);
229 checkRotationDS(rrT, 1, 0, 0, 0);
230 FieldRotation<Dfp> rTr = reverted.applyTo(r);
231 checkRotationDS(rTr, 1, 0, 0, 0);
232 assertEquals(r.getAngle().getReal(), reverted.getAngle().getReal(), 1.0e-15);
233 assertEquals(-1,
234 FieldVector3D.dotProduct(r.getAxis(RotationConvention.VECTOR_OPERATOR),
235 reverted.getAxis(RotationConvention.VECTOR_OPERATOR)).getReal(),
236 1.0e-15);
237 }
238
239 @Test
240 void testRevertVectorOperator() {
241 double a = 0.001;
242 double b = 0.36;
243 double c = 0.48;
244 double d = 0.8;
245 FieldRotation<Dfp> r = createRotation(a, b, c, d, true);
246 FieldRotation<Dfp> reverted = r.revert();
247 FieldRotation<Dfp> rrT = r.compose(reverted, RotationConvention.VECTOR_OPERATOR);
248 checkRotationDS(rrT, 1, 0, 0, 0);
249 FieldRotation<Dfp> rTr = reverted.compose(r, RotationConvention.VECTOR_OPERATOR);
250 checkRotationDS(rTr, 1, 0, 0, 0);
251 assertEquals(r.getAngle().getReal(), reverted.getAngle().getReal(), 1.0e-15);
252 assertEquals(-1,
253 FieldVector3D.dotProduct(r.getAxis(RotationConvention.VECTOR_OPERATOR),
254 reverted.getAxis(RotationConvention.VECTOR_OPERATOR)).getReal(),
255 1.0e-15);
256 }
257
258 @Test
259 void testRevertFrameTransform() {
260 double a = 0.001;
261 double b = 0.36;
262 double c = 0.48;
263 double d = 0.8;
264 FieldRotation<Dfp> r = createRotation(a, b, c, d, true);
265 FieldRotation<Dfp> reverted = r.revert();
266 FieldRotation<Dfp> rrT = r.compose(reverted, RotationConvention.FRAME_TRANSFORM);
267 checkRotationDS(rrT, 1, 0, 0, 0);
268 FieldRotation<Dfp> rTr = reverted.compose(r, RotationConvention.FRAME_TRANSFORM);
269 checkRotationDS(rTr, 1, 0, 0, 0);
270 assertEquals(r.getAngle().getReal(), reverted.getAngle().getReal(), 1.0e-15);
271 assertEquals(-1,
272 FieldVector3D.dotProduct(r.getAxis(RotationConvention.FRAME_TRANSFORM),
273 reverted.getAxis(RotationConvention.FRAME_TRANSFORM)).getReal(),
274 1.0e-15);
275 }
276
277 @Test
278 void testVectorOnePair() throws MathRuntimeException {
279
280 FieldVector3D<Dfp> u = createVector(3, 2, 1);
281 FieldVector3D<Dfp> v = createVector(-4, 2, 2);
282 FieldRotation<Dfp> r = new FieldRotation<>(u, v);
283 checkVector(r.applyTo(u.scalarMultiply(v.getNorm())), v.scalarMultiply(u.getNorm()));
284
285 checkAngle(new FieldRotation<>(u, u.negate()).getAngle(), FastMath.PI);
286
287 try {
288 new FieldRotation<>(u, createVector(0, 0, 0));
289 fail("an exception should have been thrown");
290 } catch (MathRuntimeException e) {
291
292 }
293
294 }
295
296 @Test
297 void testVectorTwoPairs() throws MathRuntimeException {
298
299 FieldVector3D<Dfp> u1 = createVector(3, 0, 0);
300 FieldVector3D<Dfp> u2 = createVector(0, 5, 0);
301 FieldVector3D<Dfp> v1 = createVector(0, 0, 2);
302 FieldVector3D<Dfp> v2 = createVector(-2, 0, 2);
303 FieldRotation<Dfp> r = new FieldRotation<>(u1, u2, v1, v2);
304 checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 0, 1));
305 checkVector(r.applyTo(createVector(0, 1, 0)), createVector(-1, 0, 0));
306
307 r = new FieldRotation<>(u1, u2, u1.negate(), u2.negate());
308 FieldVector3D<Dfp> axis = r.getAxis(RotationConvention.VECTOR_OPERATOR);
309 if (FieldVector3D.dotProduct(axis, createVector(0, 0, 1)).getReal() > 0) {
310 checkVector(axis, createVector(0, 0, 1));
311 } else {
312 checkVector(axis, createVector(0, 0, -1));
313 }
314 checkAngle(r.getAngle(), FastMath.PI);
315
316 double sqrt = FastMath.sqrt(2) / 2;
317 r = new FieldRotation<>(createVector(1, 0, 0), createVector(0, 1, 0),
318 createVector(0.5, 0.5, sqrt),
319 createVector(0.5, 0.5, -sqrt));
320 checkRotationDS(r, sqrt, 0.5, 0.5, 0);
321
322 r = new FieldRotation<>(u1, u2, u1, FieldVector3D.crossProduct(u1, u2));
323 checkRotationDS(r, sqrt, -sqrt, 0, 0);
324
325 checkRotationDS(new FieldRotation<>(u1, u2, u1, u2), 1, 0, 0, 0);
326
327 try {
328 new FieldRotation<>(u1, u2, createVector(0, 0, 0), v2);
329 fail("an exception should have been thrown");
330 } catch (MathRuntimeException e) {
331
332 }
333
334 }
335
336 @Test
337 void testMatrix()
338 throws MathIllegalArgumentException {
339
340 try {
341 createRotation(new double[][] {
342 { 0.0, 1.0, 0.0 },
343 { 1.0, 0.0, 0.0 }
344 }, 1.0e-7);
345 fail("Expecting MathIllegalArgumentException");
346 } catch (MathIllegalArgumentException nrme) {
347
348 }
349
350 try {
351 createRotation(new double[][] {
352 { 0.445888, 0.797184, -0.407040 },
353 { 0.821760, -0.184320, 0.539200 },
354 { -0.354816, 0.574912, 0.737280 }
355 }, 1.0e-7);
356 fail("Expecting MathIllegalArgumentException");
357 } catch (MathIllegalArgumentException nrme) {
358
359 }
360
361 try {
362 createRotation(new double[][] {
363 { 0.4, 0.8, -0.4 },
364 { -0.4, 0.6, 0.7 },
365 { 0.8, -0.2, 0.5 }
366 }, 1.0e-15);
367 fail("Expecting MathIllegalArgumentException");
368 } catch (MathIllegalArgumentException nrme) {
369
370 }
371
372 checkRotationDS(createRotation(new double[][] {
373 { 0.445888, 0.797184, -0.407040 },
374 { -0.354816, 0.574912, 0.737280 },
375 { 0.821760, -0.184320, 0.539200 }
376 }, 1.0e-10),
377 0.8, 0.288, 0.384, 0.36);
378
379 checkRotationDS(createRotation(new double[][] {
380 { 0.539200, 0.737280, 0.407040 },
381 { 0.184320, -0.574912, 0.797184 },
382 { 0.821760, -0.354816, -0.445888 }
383 }, 1.0e-10),
384 0.36, 0.8, 0.288, 0.384);
385
386 checkRotationDS(createRotation(new double[][] {
387 { -0.445888, 0.797184, -0.407040 },
388 { 0.354816, 0.574912, 0.737280 },
389 { 0.821760, 0.184320, -0.539200 }
390 }, 1.0e-10),
391 0.384, 0.36, 0.8, 0.288);
392
393 checkRotationDS(createRotation(new double[][] {
394 { -0.539200, 0.737280, 0.407040 },
395 { -0.184320, -0.574912, 0.797184 },
396 { 0.821760, 0.354816, 0.445888 }
397 }, 1.0e-10),
398 0.288, 0.384, 0.36, 0.8);
399
400 double[][] m1 = { { 0.0, 1.0, 0.0 },
401 { 0.0, 0.0, 1.0 },
402 { 1.0, 0.0, 0.0 } };
403 FieldRotation<Dfp> r = createRotation(m1, 1.0e-7);
404 checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 0, 1));
405 checkVector(r.applyTo(createVector(0, 1, 0)), createVector(1, 0, 0));
406 checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 1, 0));
407
408 double[][] m2 = { { 0.83203, -0.55012, -0.07139 },
409 { 0.48293, 0.78164, -0.39474 },
410 { 0.27296, 0.29396, 0.91602 } };
411 r = createRotation(m2, 1.0e-12);
412
413 Dfp[][] m3 = r.getMatrix();
414 double d00 = m2[0][0] - m3[0][0].getReal();
415 double d01 = m2[0][1] - m3[0][1].getReal();
416 double d02 = m2[0][2] - m3[0][2].getReal();
417 double d10 = m2[1][0] - m3[1][0].getReal();
418 double d11 = m2[1][1] - m3[1][1].getReal();
419 double d12 = m2[1][2] - m3[1][2].getReal();
420 double d20 = m2[2][0] - m3[2][0].getReal();
421 double d21 = m2[2][1] - m3[2][1].getReal();
422 double d22 = m2[2][2] - m3[2][2].getReal();
423
424 assertTrue(FastMath.abs(d00) < 6.0e-6);
425 assertTrue(FastMath.abs(d01) < 6.0e-6);
426 assertTrue(FastMath.abs(d02) < 6.0e-6);
427 assertTrue(FastMath.abs(d10) < 6.0e-6);
428 assertTrue(FastMath.abs(d11) < 6.0e-6);
429 assertTrue(FastMath.abs(d12) < 6.0e-6);
430 assertTrue(FastMath.abs(d20) < 6.0e-6);
431 assertTrue(FastMath.abs(d21) < 6.0e-6);
432 assertTrue(FastMath.abs(d22) < 6.0e-6);
433
434 assertTrue(FastMath.abs(d00) > 4.0e-7);
435 assertTrue(FastMath.abs(d01) > 4.0e-7);
436 assertTrue(FastMath.abs(d02) > 4.0e-7);
437 assertTrue(FastMath.abs(d10) > 4.0e-7);
438 assertTrue(FastMath.abs(d11) > 4.0e-7);
439 assertTrue(FastMath.abs(d12) > 4.0e-7);
440 assertTrue(FastMath.abs(d20) > 4.0e-7);
441 assertTrue(FastMath.abs(d21) > 4.0e-7);
442 assertTrue(FastMath.abs(d22) > 4.0e-7);
443
444 for (int i = 0; i < 3; ++i) {
445 for (int j = 0; j < 3; ++j) {
446 double m3tm3 = m3[i][0].getReal() * m3[j][0].getReal() +
447 m3[i][1].getReal() * m3[j][1].getReal() +
448 m3[i][2].getReal() * m3[j][2].getReal();
449 if (i == j) {
450 assertTrue(FastMath.abs(m3tm3 - 1.0) < 1.0e-10);
451 } else {
452 assertTrue(FastMath.abs(m3tm3) < 1.0e-10);
453 }
454 }
455 }
456
457 checkVector(r.applyTo(createVector(1, 0, 0)),
458 new FieldVector3D<>(m3[0][0], m3[1][0], m3[2][0]));
459 checkVector(r.applyTo(createVector(0, 1, 0)),
460 new FieldVector3D<>(m3[0][1], m3[1][1], m3[2][1]));
461 checkVector(r.applyTo(createVector(0, 0, 1)),
462 new FieldVector3D<>(m3[0][2], m3[1][2], m3[2][2]));
463
464 double[][] m4 = { { 1.0, 0.0, 0.0 },
465 { 0.0, -1.0, 0.0 },
466 { 0.0, 0.0, -1.0 } };
467 r = createRotation(m4, 1.0e-7);
468 checkAngle(r.getAngle(), FastMath.PI);
469
470 try {
471 double[][] m5 = { { 0.0, 0.0, 1.0 },
472 { 0.0, 1.0, 0.0 },
473 { 1.0, 0.0, 0.0 } };
474 r = createRotation(m5, 1.0e-7);
475 fail("got " + r + ", should have caught an exception");
476 } catch (MathIllegalArgumentException e) {
477
478 }
479
480 }
481
482 @Test
483 void testAngles()
484 throws MathIllegalStateException {
485
486 DfpField field = new DfpField(15);
487
488 for (RotationConvention convention : RotationConvention.values()) {
489 RotationOrder[] CardanOrders = {
490 RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
491 RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
492 };
493
494 for (final RotationOrder cardanOrder : CardanOrders) {
495 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 2.0) {
496 for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.8) {
497 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 2.0) {
498 FieldRotation<Dfp> r = new FieldRotation<>(cardanOrder,
499 convention,
500 field.newDfp(alpha1),
501 field.newDfp(alpha2),
502 field.newDfp(alpha3));
503 Dfp[] angles = r.getAngles(cardanOrder, convention);
504 checkAngle(angles[0], alpha1);
505 checkAngle(angles[1], alpha2);
506 checkAngle(angles[2], alpha3);
507 }
508 }
509 }
510 }
511
512 RotationOrder[] EulerOrders = {
513 RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
514 RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
515 };
516
517 for (final RotationOrder eulerOrder : EulerOrders) {
518 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 2.0) {
519 for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.8) {
520 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 2.0) {
521 FieldRotation<Dfp> r = new FieldRotation<>(eulerOrder,
522 convention,
523 field.newDfp(alpha1),
524 field.newDfp(alpha2),
525 field.newDfp(alpha3));
526 Dfp[] angles = r.getAngles(eulerOrder, convention);
527 checkAngle(angles[0], alpha1);
528 checkAngle(angles[1], alpha2);
529 checkAngle(angles[2], alpha3);
530 }
531 }
532 }
533 }
534 }
535
536 }
537
538 @Test
539 void testSingularities() {
540
541 DfpField field = new DfpField(20);
542 for (RotationConvention convention : RotationConvention.values()) {
543 RotationOrder[] CardanOrders = {
544 RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
545 RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
546 };
547
548 double[] singularCardanAngle = {
549 -FastMath.PI / 2, -FastMath.PI / 2 + 1.0e-12, -FastMath.PI / 2 + 1.0e-10,
550 FastMath.PI / 2 - 1.0e-10, FastMath.PI / 2 - 1.0e-12, FastMath.PI / 2
551 };
552 for (final RotationOrder cardanOrder : CardanOrders) {
553 for (final double v : singularCardanAngle) {
554 FieldRotation<Dfp> r = new FieldRotation<>(cardanOrder, convention,
555 field.newDfp(0.1), field.newDfp(v), field.newDfp(0.3));
556 assertEquals(v, r.getAngles(cardanOrder, convention)[1].getReal(), 4.5e-16);
557 }
558 }
559
560 RotationOrder[] EulerOrders = {
561 RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
562 RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
563 };
564
565 double[] singularEulerAngle = { 0, 1.0e-12, 1.0e-10, FastMath.PI - 1.0e-10, FastMath.PI - 1.0e-12, FastMath.PI };
566 for (final RotationOrder eulerOrder : EulerOrders) {
567 for (final double v : singularEulerAngle) {
568 FieldRotation<Dfp> r = new FieldRotation<>(eulerOrder, convention,
569 field.newDfp(0.1), field.newDfp(v), field.newDfp(0.3));
570 assertEquals(v, r.getAngles(eulerOrder, convention)[1].getReal(), 1.0e-24);
571 }
572 }
573
574 }
575 }
576
577 @Test
578 void testQuaternion() throws MathIllegalArgumentException {
579
580 FieldRotation<Dfp> r1 = new FieldRotation<>(createVector(2, -3, 5),
581 createAngle(1.7),
582 RotationConvention.VECTOR_OPERATOR);
583 double n = 23.5;
584 FieldRotation<Dfp> r2 = new FieldRotation<>(r1.getQ0().multiply(n), r1.getQ1().multiply(n),
585 r1.getQ2().multiply(n), r1.getQ3().multiply(n),
586 true);
587 for (double x = -0.9; x < 0.9; x += 0.2) {
588 for (double y = -0.9; y < 0.9; y += 0.2) {
589 for (double z = -0.9; z < 0.9; z += 0.2) {
590 FieldVector3D<Dfp> u = createVector(x, y, z);
591 checkVector(r2.applyTo(u), r1.applyTo(u));
592 }
593 }
594 }
595
596 r1 = createRotation(0.288, 0.384, 0.36, 0.8, false);
597 checkRotationDS(r1,
598 -r1.getQ0().getReal(), -r1.getQ1().getReal(),
599 -r1.getQ2().getReal(), -r1.getQ3().getReal());
600
601 }
602
603 @Test
604 void testApplyToRotation() throws MathIllegalArgumentException {
605
606 FieldRotation<Dfp> r1 = new FieldRotation<>(createVector(2, -3, 5),
607 createAngle(1.7),
608 RotationConvention.VECTOR_OPERATOR);
609 FieldRotation<Dfp> r2 = new FieldRotation<>(createVector(-1, 3, 2),
610 createAngle(0.3),
611 RotationConvention.VECTOR_OPERATOR);
612 FieldRotation<Dfp> r3 = r2.applyTo(r1);
613 FieldRotation<Dfp> r3Double = r2.applyTo(new Rotation(r1.getQ0().getReal(),
614 r1.getQ1().getReal(),
615 r1.getQ2().getReal(),
616 r1.getQ3().getReal(),
617 false));
618
619 for (double x = -0.9; x < 0.9; x += 0.2) {
620 for (double y = -0.9; y < 0.9; y += 0.2) {
621 for (double z = -0.9; z < 0.9; z += 0.2) {
622 FieldVector3D<Dfp> u = createVector(x, y, z);
623 checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
624 checkVector(r2.applyTo(r1.applyTo(u)), r3Double.applyTo(u));
625 }
626 }
627 }
628
629 }
630
631 @Test
632 void testComposeVectorOperator() throws MathIllegalArgumentException {
633
634 FieldRotation<Dfp> r1 = new FieldRotation<>(createVector(2, -3, 5),
635 createAngle(1.7),
636 RotationConvention.VECTOR_OPERATOR);
637 FieldRotation<Dfp> r2 = new FieldRotation<>(createVector(-1, 3, 2),
638 createAngle(0.3),
639 RotationConvention.VECTOR_OPERATOR);
640 FieldRotation<Dfp> r3 = r2.compose(r1, RotationConvention.VECTOR_OPERATOR);
641 FieldRotation<Dfp> r3Double = r2.compose(new Rotation(r1.getQ0().getReal(),
642 r1.getQ1().getReal(),
643 r1.getQ2().getReal(),
644 r1.getQ3().getReal(),
645 false),
646 RotationConvention.VECTOR_OPERATOR);
647
648 for (double x = -0.9; x < 0.9; x += 0.2) {
649 for (double y = -0.9; y < 0.9; y += 0.2) {
650 for (double z = -0.9; z < 0.9; z += 0.2) {
651 FieldVector3D<Dfp> u = createVector(x, y, z);
652 checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
653 checkVector(r2.applyTo(r1.applyTo(u)), r3Double.applyTo(u));
654 }
655 }
656 }
657
658 }
659
660 @Test
661 void testComposeFrameTransform() throws MathIllegalArgumentException {
662
663 FieldRotation<Dfp> r1 = new FieldRotation<>(createVector(2, -3, 5),
664 createAngle(1.7),
665 RotationConvention.FRAME_TRANSFORM);
666 FieldRotation<Dfp> r2 = new FieldRotation<>(createVector(-1, 3, 2),
667 createAngle(0.3),
668 RotationConvention.FRAME_TRANSFORM);
669 FieldRotation<Dfp> r3 = r2.compose(r1, RotationConvention.FRAME_TRANSFORM);
670 FieldRotation<Dfp> r3Double = r2.compose(new Rotation(r1.getQ0().getReal(),
671 r1.getQ1().getReal(),
672 r1.getQ2().getReal(),
673 r1.getQ3().getReal(),
674 false),
675 RotationConvention.FRAME_TRANSFORM);
676 FieldRotation<Dfp> r4 = r1.compose(r2, RotationConvention.VECTOR_OPERATOR);
677 assertEquals(0.0, FieldRotation.distance(r3, r4).getReal(), 1.0e-15);
678
679 for (double x = -0.9; x < 0.9; x += 0.2) {
680 for (double y = -0.9; y < 0.9; y += 0.2) {
681 for (double z = -0.9; z < 0.9; z += 0.2) {
682 FieldVector3D<Dfp> u = createVector(x, y, z);
683 checkVector(r1.applyTo(r2.applyTo(u)), r3.applyTo(u));
684 checkVector(r1.applyTo(r2.applyTo(u)), r3Double.applyTo(u));
685 }
686 }
687 }
688
689 }
690
691 @Test
692 void testApplyInverseToRotation() throws MathIllegalArgumentException {
693
694 FieldRotation<Dfp> r1 = new FieldRotation<>(createVector(2, -3, 5),
695 createAngle(1.7),
696 RotationConvention.VECTOR_OPERATOR);
697 FieldRotation<Dfp> r2 = new FieldRotation<>(createVector(-1, 3, 2),
698 createAngle(0.3),
699 RotationConvention.VECTOR_OPERATOR);
700 FieldRotation<Dfp> r3 = r2.applyInverseTo(r1);
701 FieldRotation<Dfp> r3Double = r2.applyInverseTo(new Rotation(r1.getQ0().getReal(),
702 r1.getQ1().getReal(),
703 r1.getQ2().getReal(),
704 r1.getQ3().getReal(),
705 false));
706
707 for (double x = -0.9; x < 0.9; x += 0.2) {
708 for (double y = -0.9; y < 0.9; y += 0.2) {
709 for (double z = -0.9; z < 0.9; z += 0.2) {
710 FieldVector3D<Dfp> u = createVector(x, y, z);
711 checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
712 checkVector(r2.applyInverseTo(r1.applyTo(u)), r3Double.applyTo(u));
713 }
714 }
715 }
716
717 }
718
719 @Test
720 void testComposeInverseVectorOperator() throws MathIllegalArgumentException {
721
722 FieldRotation<Dfp> r1 = new FieldRotation<>(createVector(2, -3, 5),
723 createAngle(1.7),
724 RotationConvention.VECTOR_OPERATOR);
725 FieldRotation<Dfp> r2 = new FieldRotation<>(createVector(-1, 3, 2),
726 createAngle(0.3),
727 RotationConvention.VECTOR_OPERATOR);
728 FieldRotation<Dfp> r3 = r2.composeInverse(r1, RotationConvention.VECTOR_OPERATOR);
729 FieldRotation<Dfp> r3Double = r2.composeInverse(new Rotation(r1.getQ0().getReal(),
730 r1.getQ1().getReal(),
731 r1.getQ2().getReal(),
732 r1.getQ3().getReal(),
733 false),
734 RotationConvention.VECTOR_OPERATOR);
735
736 for (double x = -0.9; x < 0.9; x += 0.2) {
737 for (double y = -0.9; y < 0.9; y += 0.2) {
738 for (double z = -0.9; z < 0.9; z += 0.2) {
739 FieldVector3D<Dfp> u = createVector(x, y, z);
740 checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
741 checkVector(r2.applyInverseTo(r1.applyTo(u)), r3Double.applyTo(u));
742 }
743 }
744 }
745
746 }
747
748 @Test
749 void testComposeInverseFrameTransform() throws MathIllegalArgumentException {
750
751 FieldRotation<Dfp> r1 = new FieldRotation<>(createVector(2, -3, 5),
752 createAngle(1.7),
753 RotationConvention.FRAME_TRANSFORM);
754 FieldRotation<Dfp> r2 = new FieldRotation<>(createVector(-1, 3, 2),
755 createAngle(0.3),
756 RotationConvention.FRAME_TRANSFORM);
757 FieldRotation<Dfp> r3 = r2.composeInverse(r1, RotationConvention.FRAME_TRANSFORM);
758 FieldRotation<Dfp> r3Double = r2.composeInverse(new Rotation(r1.getQ0().getReal(),
759 r1.getQ1().getReal(),
760 r1.getQ2().getReal(),
761 r1.getQ3().getReal(),
762 false),
763 RotationConvention.FRAME_TRANSFORM);
764 FieldRotation<Dfp> r4 = r1.revert().composeInverse(r2.revert(), RotationConvention.VECTOR_OPERATOR);
765 assertEquals(0.0, FieldRotation.distance(r3, r4).getReal(), 1.0e-15);
766
767 for (double x = -0.9; x < 0.9; x += 0.2) {
768 for (double y = -0.9; y < 0.9; y += 0.2) {
769 for (double z = -0.9; z < 0.9; z += 0.2) {
770 FieldVector3D<Dfp> u = createVector(x, y, z);
771 checkVector(r1.applyTo(r2.applyInverseTo(u)), r3.applyTo(u));
772 checkVector(r1.applyTo(r2.applyInverseTo(u)), r3Double.applyTo(u));
773 }
774 }
775 }
776
777 }
778
779 @Test
780 void testDoubleVectors() throws MathIllegalArgumentException {
781
782 Well1024a random = new Well1024a(0x180b41cfeeffaf67L);
783 UnitSphereRandomVectorGenerator g = new UnitSphereRandomVectorGenerator(3, random);
784 for (int i = 0; i < 10; ++i) {
785 double[] unit = g.nextVector();
786 FieldRotation<Dfp> r = new FieldRotation<>(createVector(unit[0], unit[1], unit[2]),
787 createAngle(random.nextDouble()),
788 RotationConvention.VECTOR_OPERATOR);
789
790 for (double x = -0.9; x < 0.9; x += 0.4) {
791 for (double y = -0.9; y < 0.9; y += 0.4) {
792 for (double z = -0.9; z < 0.9; z += 0.4) {
793 FieldVector3D<Dfp> uds = createVector(x, y, z);
794 FieldVector3D<Dfp> ruds = r.applyTo(uds);
795 FieldVector3D<Dfp> rIuds = r.applyInverseTo(uds);
796 Vector3D u = new Vector3D(x, y, z);
797 FieldVector3D<Dfp> ru = r.applyTo(u);
798 FieldVector3D<Dfp> rIu = r.applyInverseTo(u);
799 Dfp[] ruArray = new Dfp[3];
800 r.applyTo(new double[] { x, y, z}, ruArray);
801 Dfp[] rIuArray = new Dfp[3];
802 r.applyInverseTo(new double[] { x, y, z}, rIuArray);
803 checkVector(ruds, ru);
804 checkVector(ruds, new FieldVector3D<>(ruArray));
805 checkVector(rIuds, rIu);
806 checkVector(rIuds, new FieldVector3D<>(rIuArray));
807 }
808 }
809 }
810 }
811
812 }
813
814 @Test
815 void testDoubleRotations() throws MathIllegalArgumentException {
816
817 DfpField field = new DfpField(20);
818 Well1024a random = new Well1024a(0x180b41cfeeffaf67L);
819 UnitSphereRandomVectorGenerator g = new UnitSphereRandomVectorGenerator(3, random);
820 for (int i = 0; i < 10; ++i) {
821 double[] unit1 = g.nextVector();
822 Rotation r1 = new Rotation(new Vector3D(unit1[0], unit1[1], unit1[2]),
823 random.nextDouble(), RotationConvention.VECTOR_OPERATOR);
824 FieldRotation<Dfp> r1Prime = new FieldRotation<>(field.newDfp(r1.getQ0()),
825 field.newDfp(r1.getQ1()),
826 field.newDfp(r1.getQ2()),
827 field.newDfp(r1.getQ3()),
828 false);
829 double[] unit2 = g.nextVector();
830 FieldRotation<Dfp> r2 = new FieldRotation<>(createVector(unit2[0], unit2[1], unit2[2]),
831 createAngle(random.nextDouble()),
832 RotationConvention.VECTOR_OPERATOR);
833
834 FieldRotation<Dfp> rA = FieldRotation.applyTo(r1, r2);
835 FieldRotation<Dfp> rB = r1Prime.compose(r2, RotationConvention.VECTOR_OPERATOR);
836 FieldRotation<Dfp> rC = FieldRotation.applyInverseTo(r1, r2);
837 FieldRotation<Dfp> rD = r1Prime.composeInverse(r2, RotationConvention.VECTOR_OPERATOR);
838
839 for (double x = -0.9; x < 0.9; x += 0.4) {
840 for (double y = -0.9; y < 0.9; y += 0.4) {
841 for (double z = -0.9; z < 0.9; z += 0.4) {
842
843 FieldVector3D<Dfp> uds = createVector(x, y, z);
844 checkVector(r1Prime.applyTo(uds), FieldRotation.applyTo(r1, uds));
845 checkVector(r1Prime.applyInverseTo(uds), FieldRotation.applyInverseTo(r1, uds));
846 checkVector(rA.applyTo(uds), rB.applyTo(uds));
847 checkVector(rA.applyInverseTo(uds), rB.applyInverseTo(uds));
848 checkVector(rC.applyTo(uds), rD.applyTo(uds));
849 checkVector(rC.applyInverseTo(uds), rD.applyInverseTo(uds));
850
851 }
852 }
853 }
854 }
855
856 }
857
858 @Test
859 void testArray() throws MathIllegalArgumentException {
860
861 FieldRotation<Dfp> r = new FieldRotation<>(createAxis(2, -3, 5),
862 createAngle(1.7),
863 RotationConvention.VECTOR_OPERATOR);
864
865 for (double x = -0.9; x < 0.9; x += 0.2) {
866 for (double y = -0.9; y < 0.9; y += 0.2) {
867 for (double z = -0.9; z < 0.9; z += 0.2) {
868 FieldVector3D<Dfp> u = createVector(x, y, z);
869 FieldVector3D<Dfp> v = r.applyTo(u);
870 Dfp[] out = new Dfp[3];
871 r.applyTo(new Dfp[] { u.getX(), u.getY(), u.getZ() }, out);
872 assertEquals(v.getX().getReal(), out[0].getReal(), 1.0e-10);
873 assertEquals(v.getY().getReal(), out[1].getReal(), 1.0e-10);
874 assertEquals(v.getZ().getReal(), out[2].getReal(), 1.0e-10);
875 r.applyInverseTo(out, out);
876 assertEquals(u.getX().getReal(), out[0].getReal(), 1.0e-10);
877 assertEquals(u.getY().getReal(), out[1].getReal(), 1.0e-10);
878 assertEquals(u.getZ().getReal(), out[2].getReal(), 1.0e-10);
879 }
880 }
881 }
882
883 }
884
885 @Test
886 void testApplyInverseTo() throws MathIllegalArgumentException {
887
888 Dfp[] in = new Dfp[3];
889 Dfp[] out = new Dfp[3];
890 Dfp[] rebuilt = new Dfp[3];
891 FieldRotation<Dfp> r = new FieldRotation<>(createVector(2, -3, 5),
892 createAngle(1.7),
893 RotationConvention.VECTOR_OPERATOR);
894 for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
895 for (double phi = -1.55; phi < 1.55; phi += 0.2) {
896 FieldVector3D<Dfp> u = createVector(FastMath.cos(lambda) * FastMath.cos(phi),
897 FastMath.sin(lambda) * FastMath.cos(phi),
898 FastMath.sin(phi));
899 r.applyInverseTo(r.applyTo(u));
900 checkVector(u, r.applyInverseTo(r.applyTo(u)));
901 checkVector(u, r.applyTo(r.applyInverseTo(u)));
902 in[0] = u.getX();
903 in[1] = u.getY();
904 in[2] = u.getZ();
905 r.applyTo(in, out);
906 r.applyInverseTo(out, rebuilt);
907 assertEquals(in[0].getReal(), rebuilt[0].getReal(), 1.0e-12);
908 assertEquals(in[1].getReal(), rebuilt[1].getReal(), 1.0e-12);
909 assertEquals(in[2].getReal(), rebuilt[2].getReal(), 1.0e-12);
910 }
911 }
912
913 r = createRotation(1, 0, 0, 0, false);
914 for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
915 for (double phi = -1.55; phi < 1.55; phi += 0.2) {
916 FieldVector3D<Dfp> u = createVector(FastMath.cos(lambda) * FastMath.cos(phi),
917 FastMath.sin(lambda) * FastMath.cos(phi),
918 FastMath.sin(phi));
919 checkVector(u, r.applyInverseTo(r.applyTo(u)));
920 checkVector(u, r.applyTo(r.applyInverseTo(u)));
921 }
922 }
923
924 r = new FieldRotation<>(createVector(0, 0, 1), createAngle(FastMath.PI), RotationConvention.VECTOR_OPERATOR);
925 for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
926 for (double phi = -1.55; phi < 1.55; phi += 0.2) {
927 FieldVector3D<Dfp> u = createVector(FastMath.cos(lambda) * FastMath.cos(phi),
928 FastMath.sin(lambda) * FastMath.cos(phi),
929 FastMath.sin(phi));
930 checkVector(u, r.applyInverseTo(r.applyTo(u)));
931 checkVector(u, r.applyTo(r.applyInverseTo(u)));
932 }
933 }
934
935 }
936
937 @Test
938 void testIssue639() throws MathRuntimeException{
939 FieldVector3D<Dfp> u1 = createVector(-1321008684645961.0 / 268435456.0,
940 -5774608829631843.0 / 268435456.0,
941 -3822921525525679.0 / 4294967296.0);
942 FieldVector3D<Dfp> u2 =createVector( -5712344449280879.0 / 2097152.0,
943 -2275058564560979.0 / 1048576.0,
944 4423475992255071.0 / 65536.0);
945 FieldRotation<Dfp> rot = new FieldRotation<>(u1, u2, createVector(1, 0, 0),createVector(0, 0, 1));
946 assertEquals( 0.6228370359608200639829222, rot.getQ0().getReal(), 1.0e-15);
947 assertEquals( 0.0257707621456498790029987, rot.getQ1().getReal(), 1.0e-15);
948 assertEquals(-0.0000000002503012255839931, rot.getQ2().getReal(), 1.0e-15);
949 assertEquals(-0.7819270390861109450724902, rot.getQ3().getReal(), 1.0e-15);
950 }
951
952 @Test
953 void testIssue801() throws MathRuntimeException {
954 FieldVector3D<Dfp> u1 = createVector(0.9999988431610581, -0.0015210774290851095, 0.0);
955 FieldVector3D<Dfp> u2 = createVector(0.0, 0.0, 1.0);
956
957 FieldVector3D<Dfp> v1 = createVector(0.9999999999999999, 0.0, 0.0);
958 FieldVector3D<Dfp> v2 = createVector(0.0, 0.0, -1.0);
959
960 FieldRotation<Dfp> quat = new FieldRotation<>(u1, u2, v1, v2);
961 double q2 = quat.getQ0().getReal() * quat.getQ0().getReal() +
962 quat.getQ1().getReal() * quat.getQ1().getReal() +
963 quat.getQ2().getReal() * quat.getQ2().getReal() +
964 quat.getQ3().getReal() * quat.getQ3().getReal();
965 assertEquals(1.0, q2, 1.0e-14);
966 assertEquals(0.0, FieldVector3D.angle(v1, quat.applyTo(u1)).getReal(), 1.0e-14);
967 assertEquals(0.0, FieldVector3D.angle(v2, quat.applyTo(u2)).getReal(), 1.0e-14);
968
969 }
970
971 private void checkAngle(Dfp a1, double a2) {
972 assertEquals(a1.getReal(), MathUtils.normalizeAngle(a2, a1.getReal()), 1.0e-10);
973 }
974
975 private void checkRotationDS(FieldRotation<Dfp> r, double q0, double q1, double q2, double q3) {
976 FieldRotation<Dfp> rPrime = createRotation(q0, q1, q2, q3, false);
977 assertEquals(0, FieldRotation.distance(r, rPrime).getReal(), 1.0e-12);
978 }
979
980 private FieldRotation<Dfp> createRotation(double q0, double q1, double q2, double q3,
981 boolean needsNormalization) {
982 return new FieldRotation<>(field.newDfp(q0),
983 field.newDfp(q1),
984 field.newDfp(q2),
985 field.newDfp(q3),
986 needsNormalization);
987 }
988
989 private FieldRotation<Dfp> createRotation(double[][] m, double threshold) {
990 Dfp[][] mds = new Dfp[m.length][m[0].length];
991 for (int i = 0; i < m.length; ++i) {
992 for (int j = 0; j < m[i].length; ++j) {
993 mds[i][j] = field.newDfp(m[i][j]);
994 }
995 }
996 return new FieldRotation<>(mds, threshold);
997 }
998
999 private FieldVector3D<Dfp> createVector(double x, double y, double z) {
1000 return new FieldVector3D<>(field.newDfp(x), field.newDfp(y), field.newDfp(z));
1001 }
1002
1003 private FieldVector3D<Dfp> createAxis(double x, double y, double z) {
1004 return new FieldVector3D<>(field.newDfp(x), field.newDfp(y), field.newDfp(z));
1005 }
1006
1007 private Dfp createAngle(double alpha) {
1008 return field.newDfp(alpha);
1009 }
1010
1011 private void checkVector(FieldVector3D<Dfp> u, FieldVector3D<Dfp> v) {
1012 assertEquals(u.getX().getReal(), v.getX().getReal(), 1.0e-12);
1013 assertEquals(u.getY().getReal(), v.getY().getReal(), 1.0e-12);
1014 assertEquals(u.getZ().getReal(), v.getZ().getReal(), 1.0e-12);
1015 }
1016
1017 @BeforeEach
1018 void setUp() {
1019 field = new DfpField(20);
1020 }
1021
1022 @AfterEach
1023 void tearDown() {
1024 field = null;
1025 }
1026
1027 private DfpField field;
1028
1029 }