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