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.exception.MathIllegalArgumentException;
26 import org.hipparchus.exception.MathIllegalStateException;
27 import org.hipparchus.exception.MathRuntimeException;
28 import org.hipparchus.geometry.LocalizedGeometryFormats;
29 import org.hipparchus.util.FastMath;
30 import org.hipparchus.util.MathUtils;
31 import org.junit.jupiter.api.Test;
32
33 import java.util.Arrays;
34
35 import static org.junit.jupiter.api.Assertions.assertEquals;
36 import static org.junit.jupiter.api.Assertions.assertTrue;
37 import static org.junit.jupiter.api.Assertions.fail;
38
39 class RotationTest {
40
41 @Test
42 void testIssue304Cardan() {
43 for (final RotationConvention convention : RotationConvention.values()) {
44 for (final RotationOrder order : Arrays.asList(RotationOrder.XYZ,
45 RotationOrder.XZY,
46 RotationOrder.YXZ,
47 RotationOrder.YZX,
48 RotationOrder.ZXY,
49 RotationOrder.ZYX)) {
50
51
52 Rotation singularPlus = new Rotation(order, convention, 0.0, MathUtils.SEMI_PI, 0.125);
53 assertEquals(0.0, singularPlus.getAngles(order, convention)[0], 1.0e-16);
54 assertEquals(MathUtils.SEMI_PI, singularPlus.getAngles(order, convention)[1], 1.0e-16);
55 assertEquals(0.125, singularPlus.getAngles(order, convention)[2], 1.0e-16);
56
57
58 Rotation singularMinus = new Rotation(order, convention, 0.0, -MathUtils.SEMI_PI, 0.125);
59 assertEquals(0.0, singularMinus.getAngles(order, convention)[0], 1.0e-16);
60 assertEquals(-MathUtils.SEMI_PI, singularMinus.getAngles(order, convention)[1], 1.0e-16);
61 assertEquals(0.125, singularMinus.getAngles(order, convention)[2], 1.0e-16);
62
63 }
64 }
65 }
66
67 @Test
68 void testIssue304Euler() {
69 for (final RotationConvention convention : RotationConvention.values()) {
70 for (final RotationOrder order : Arrays.asList(RotationOrder.XYX,
71 RotationOrder.XZX,
72 RotationOrder.YXY,
73 RotationOrder.YZY,
74 RotationOrder.ZXZ,
75 RotationOrder.ZYZ)) {
76
77
78 Rotation singularZero = new Rotation(order, convention, 0.125, 0.0, 0.0);
79 assertEquals(0.125, singularZero.getAngles(order, convention)[0], 1.0e-16);
80 assertEquals(0.0, singularZero.getAngles(order, convention)[1], 1.0e-16);
81 assertEquals(0.0, singularZero.getAngles(order, convention)[2], 1.0e-16);
82
83
84 Rotation singularPi = new Rotation(order, convention, 0.125, FastMath.PI, 0.0);
85 assertEquals(0.125, singularPi.getAngles(order, convention)[0], 1.0e-16);
86 assertEquals(FastMath.PI, singularPi.getAngles(order, convention)[1], 1.0e-16);
87 assertEquals(0.0, singularPi.getAngles(order, convention)[2], 1.0e-16);
88
89 }
90 }
91 }
92
93 @Test
94 void testIdentity() {
95
96 Rotation r = Rotation.IDENTITY;
97 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
98 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
99 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
100 checkAngle(r.getAngle(), 0);
101
102 r = new Rotation(-1, 0, 0, 0, false);
103 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
104 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
105 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
106 checkAngle(r.getAngle(), 0);
107
108 r = new Rotation(42, 0, 0, 0, true);
109 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
110 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
111 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
112 checkAngle(r.getAngle(), 0);
113
114 }
115
116 @Test
117 void testAxisAngleVectorOperator() throws MathIllegalArgumentException {
118
119 Rotation r = new Rotation(new Vector3D(10, 10, 10), 2 * FastMath.PI / 3, RotationConvention.VECTOR_OPERATOR);
120 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_J);
121 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_K);
122 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_I);
123 double s = 1 / FastMath.sqrt(3);
124 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), new Vector3D( s, s, s));
125 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), new Vector3D(-s, -s, -s));
126 checkAngle(r.getAngle(), 2 * FastMath.PI / 3);
127
128 try {
129 new Rotation(new Vector3D(0, 0, 0), 2 * FastMath.PI / 3, RotationConvention.VECTOR_OPERATOR);
130 fail("an exception should have been thrown");
131 } catch (MathIllegalArgumentException e) {
132 assertEquals(LocalizedGeometryFormats.ZERO_NORM_FOR_ROTATION_AXIS, e.getSpecifier());
133 }
134
135 r = new Rotation(Vector3D.PLUS_K, 1.5 * FastMath.PI, RotationConvention.VECTOR_OPERATOR);
136 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), new Vector3D(0, 0, -1));
137 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), new Vector3D(0, 0, +1));
138 checkAngle(r.getAngle(), MathUtils.SEMI_PI);
139
140 r = new Rotation(Vector3D.PLUS_J, FastMath.PI, RotationConvention.VECTOR_OPERATOR);
141 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.PLUS_J);
142 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.MINUS_J);
143 checkAngle(r.getAngle(), FastMath.PI);
144
145 checkVector(Rotation.IDENTITY.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.PLUS_I);
146 checkVector(Rotation.IDENTITY.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.MINUS_I);
147
148 }
149
150 @Test
151 void testAxisAngleFrameTransform() throws MathIllegalArgumentException {
152
153 Rotation r = new Rotation(new Vector3D(10, 10, 10), 2 * FastMath.PI / 3, RotationConvention.FRAME_TRANSFORM);
154 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_K);
155 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_I);
156 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_J);
157 double s = 1 / FastMath.sqrt(3);
158 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), new Vector3D( s, s, s));
159 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), new Vector3D(-s, -s, -s));
160 checkAngle(r.getAngle(), 2 * FastMath.PI / 3);
161
162 try {
163 new Rotation(new Vector3D(0, 0, 0), 2 * FastMath.PI / 3, RotationConvention.FRAME_TRANSFORM);
164 fail("an exception should have been thrown");
165 } catch (MathIllegalArgumentException e) {
166 assertEquals(LocalizedGeometryFormats.ZERO_NORM_FOR_ROTATION_AXIS, e.getSpecifier());
167 }
168
169 r = new Rotation(Vector3D.PLUS_K, 1.5 * FastMath.PI, RotationConvention.FRAME_TRANSFORM);
170 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), new Vector3D(0, 0, -1));
171 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), new Vector3D(0, 0, +1));
172 checkAngle(r.getAngle(), MathUtils.SEMI_PI);
173
174 r = new Rotation(Vector3D.PLUS_J, FastMath.PI, RotationConvention.FRAME_TRANSFORM);
175 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.PLUS_J);
176 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.MINUS_J);
177 checkAngle(r.getAngle(), FastMath.PI);
178
179 checkVector(Rotation.IDENTITY.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.MINUS_I);
180 checkVector(Rotation.IDENTITY.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.PLUS_I);
181
182 }
183
184 @Test
185 void testWrongMatrix() {
186 checkWrongMatrix(new double[2][2]);
187 checkWrongMatrix(new double[][] { new double[2], new double[3], new double[3]});
188 checkWrongMatrix(new double[][] { new double[3], new double[2], new double[3]});
189 checkWrongMatrix(new double[][] { new double[3], new double[3], new double[2]});
190 }
191
192 private void checkWrongMatrix(final double[][] m) {
193 try {
194 new Rotation(m, 0.001);
195 fail("an exception should have been thrown");
196 } catch (MathIllegalArgumentException miae) {
197 assertEquals(LocalizedGeometryFormats.ROTATION_MATRIX_DIMENSIONS, miae.getSpecifier());
198 }
199 }
200
201 @Test
202 void testRevertVectorOperator() {
203 Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
204 Rotation reverted = r.revert();
205 checkRotation(r.compose(reverted, RotationConvention.VECTOR_OPERATOR), 1, 0, 0, 0);
206 checkRotation(reverted.compose(r, RotationConvention.VECTOR_OPERATOR), 1, 0, 0, 0);
207 assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
208 assertEquals(-1,
209 Vector3D.dotProduct(r.getAxis(RotationConvention.VECTOR_OPERATOR),
210 reverted.getAxis(RotationConvention.VECTOR_OPERATOR)),
211 1.0e-12);
212 }
213
214 @Test
215 void testRevertFrameTransform() {
216 Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
217 Rotation reverted = r.revert();
218 checkRotation(r.compose(reverted, RotationConvention.FRAME_TRANSFORM), 1, 0, 0, 0);
219 checkRotation(reverted.compose(r, RotationConvention.FRAME_TRANSFORM), 1, 0, 0, 0);
220 assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
221 assertEquals(-1,
222 Vector3D.dotProduct(r.getAxis(RotationConvention.FRAME_TRANSFORM),
223 reverted.getAxis(RotationConvention.FRAME_TRANSFORM)),
224 1.0e-12);
225 }
226
227 @Test
228 void testVectorOnePair() throws MathRuntimeException {
229
230 Vector3D u = new Vector3D(3, 2, 1);
231 Vector3D v = new Vector3D(-4, 2, 2);
232 Rotation r = new Rotation(u, v);
233 checkVector(r.applyTo(u.scalarMultiply(v.getNorm())), v.scalarMultiply(u.getNorm()));
234
235 checkAngle(new Rotation(u, u.negate()).getAngle(), FastMath.PI);
236
237 try {
238 new Rotation(u, Vector3D.ZERO);
239 fail("an exception should have been thrown");
240 } catch (MathRuntimeException e) {
241
242 }
243
244 }
245
246 @Test
247 void testVectorTwoPairs() throws MathRuntimeException {
248
249 Vector3D u1 = new Vector3D(3, 0, 0);
250 Vector3D u2 = new Vector3D(0, 5, 0);
251 Vector3D v1 = new Vector3D(0, 0, 2);
252 Vector3D v2 = new Vector3D(-2, 0, 2);
253 Rotation r = new Rotation(u1, u2, v1, v2);
254 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_K);
255 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.MINUS_I);
256
257 r = new Rotation(u1, u2, u1.negate(), u2.negate());
258 Vector3D axis = r.getAxis(RotationConvention.VECTOR_OPERATOR);
259 if (Vector3D.dotProduct(axis, Vector3D.PLUS_K) > 0) {
260 checkVector(axis, Vector3D.PLUS_K);
261 } else {
262 checkVector(axis, Vector3D.MINUS_K);
263 }
264 checkAngle(r.getAngle(), FastMath.PI);
265
266 double sqrt = FastMath.sqrt(2) / 2;
267 r = new Rotation(Vector3D.PLUS_I, Vector3D.PLUS_J,
268 new Vector3D(0.5, 0.5, sqrt),
269 new Vector3D(0.5, 0.5, -sqrt));
270 checkRotation(r, sqrt, 0.5, 0.5, 0);
271
272 r = new Rotation(u1, u2, u1, Vector3D.crossProduct(u1, u2));
273 checkRotation(r, sqrt, -sqrt, 0, 0);
274
275 checkRotation(new Rotation(u1, u2, u1, u2), 1, 0, 0, 0);
276
277 try {
278 new Rotation(u1, u2, Vector3D.ZERO, v2);
279 fail("an exception should have been thrown");
280 } catch (MathRuntimeException e) {
281
282 }
283
284 }
285
286 @Test
287 void testMatrix()
288 throws MathIllegalArgumentException {
289
290 try {
291 new Rotation(new double[][] {
292 { 0.0, 1.0, 0.0 },
293 { 1.0, 0.0, 0.0 }
294 }, 1.0e-7);
295 fail("Expecting MathIllegalArgumentException");
296 } catch (MathIllegalArgumentException nrme) {
297
298 }
299
300 try {
301 new Rotation(new double[][] {
302 { 0.445888, 0.797184, -0.407040 },
303 { 0.821760, -0.184320, 0.539200 },
304 { -0.354816, 0.574912, 0.737280 }
305 }, 1.0e-7);
306 fail("Expecting MathIllegalArgumentException");
307 } catch (MathIllegalArgumentException nrme) {
308
309 }
310
311 try {
312 new Rotation(new double[][] {
313 { 0.4, 0.8, -0.4 },
314 { -0.4, 0.6, 0.7 },
315 { 0.8, -0.2, 0.5 }
316 }, 1.0e-15);
317 fail("Expecting MathIllegalArgumentException");
318 } catch (MathIllegalArgumentException nrme) {
319
320 }
321
322 checkRotation(new Rotation(new double[][] {
323 { 0.445888, 0.797184, -0.407040 },
324 { -0.354816, 0.574912, 0.737280 },
325 { 0.821760, -0.184320, 0.539200 }
326 }, 1.0e-10),
327 0.8, 0.288, 0.384, 0.36);
328
329 checkRotation(new Rotation(new double[][] {
330 { 0.539200, 0.737280, 0.407040 },
331 { 0.184320, -0.574912, 0.797184 },
332 { 0.821760, -0.354816, -0.445888 }
333 }, 1.0e-10),
334 0.36, 0.8, 0.288, 0.384);
335
336 checkRotation(new Rotation(new double[][] {
337 { -0.445888, 0.797184, -0.407040 },
338 { 0.354816, 0.574912, 0.737280 },
339 { 0.821760, 0.184320, -0.539200 }
340 }, 1.0e-10),
341 0.384, 0.36, 0.8, 0.288);
342
343 checkRotation(new Rotation(new double[][] {
344 { -0.539200, 0.737280, 0.407040 },
345 { -0.184320, -0.574912, 0.797184 },
346 { 0.821760, 0.354816, 0.445888 }
347 }, 1.0e-10),
348 0.288, 0.384, 0.36, 0.8);
349
350 double[][] m1 = { { 0.0, 1.0, 0.0 },
351 { 0.0, 0.0, 1.0 },
352 { 1.0, 0.0, 0.0 } };
353 Rotation r = new Rotation(m1, 1.0e-7);
354 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_K);
355 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_I);
356 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_J);
357
358 double[][] m2 = { { 0.83203, -0.55012, -0.07139 },
359 { 0.48293, 0.78164, -0.39474 },
360 { 0.27296, 0.29396, 0.91602 } };
361 r = new Rotation(m2, 1.0e-12);
362
363 double[][] m3 = r.getMatrix();
364 double d00 = m2[0][0] - m3[0][0];
365 double d01 = m2[0][1] - m3[0][1];
366 double d02 = m2[0][2] - m3[0][2];
367 double d10 = m2[1][0] - m3[1][0];
368 double d11 = m2[1][1] - m3[1][1];
369 double d12 = m2[1][2] - m3[1][2];
370 double d20 = m2[2][0] - m3[2][0];
371 double d21 = m2[2][1] - m3[2][1];
372 double d22 = m2[2][2] - m3[2][2];
373
374 assertTrue(FastMath.abs(d00) < 6.0e-6);
375 assertTrue(FastMath.abs(d01) < 6.0e-6);
376 assertTrue(FastMath.abs(d02) < 6.0e-6);
377 assertTrue(FastMath.abs(d10) < 6.0e-6);
378 assertTrue(FastMath.abs(d11) < 6.0e-6);
379 assertTrue(FastMath.abs(d12) < 6.0e-6);
380 assertTrue(FastMath.abs(d20) < 6.0e-6);
381 assertTrue(FastMath.abs(d21) < 6.0e-6);
382 assertTrue(FastMath.abs(d22) < 6.0e-6);
383
384 assertTrue(FastMath.abs(d00) > 4.0e-7);
385 assertTrue(FastMath.abs(d01) > 4.0e-7);
386 assertTrue(FastMath.abs(d02) > 4.0e-7);
387 assertTrue(FastMath.abs(d10) > 4.0e-7);
388 assertTrue(FastMath.abs(d11) > 4.0e-7);
389 assertTrue(FastMath.abs(d12) > 4.0e-7);
390 assertTrue(FastMath.abs(d20) > 4.0e-7);
391 assertTrue(FastMath.abs(d21) > 4.0e-7);
392 assertTrue(FastMath.abs(d22) > 4.0e-7);
393
394 for (int i = 0; i < 3; ++i) {
395 for (int j = 0; j < 3; ++j) {
396 double m3tm3 = m3[i][0] * m3[j][0]
397 + m3[i][1] * m3[j][1]
398 + m3[i][2] * m3[j][2];
399 if (i == j) {
400 assertTrue(FastMath.abs(m3tm3 - 1.0) < 1.0e-10);
401 } else {
402 assertTrue(FastMath.abs(m3tm3) < 1.0e-10);
403 }
404 }
405 }
406
407 checkVector(r.applyTo(Vector3D.PLUS_I),
408 new Vector3D(m3[0][0], m3[1][0], m3[2][0]));
409 checkVector(r.applyTo(Vector3D.PLUS_J),
410 new Vector3D(m3[0][1], m3[1][1], m3[2][1]));
411 checkVector(r.applyTo(Vector3D.PLUS_K),
412 new Vector3D(m3[0][2], m3[1][2], m3[2][2]));
413
414 double[][] m4 = { { 1.0, 0.0, 0.0 },
415 { 0.0, -1.0, 0.0 },
416 { 0.0, 0.0, -1.0 } };
417 r = new Rotation(m4, 1.0e-7);
418 checkAngle(r.getAngle(), FastMath.PI);
419
420 try {
421 double[][] m5 = { { 0.0, 0.0, 1.0 },
422 { 0.0, 1.0, 0.0 },
423 { 1.0, 0.0, 0.0 } };
424 r = new Rotation(m5, 1.0e-7);
425 fail("got " + r + ", should have caught an exception");
426 } catch (MathIllegalArgumentException e) {
427
428 }
429
430 }
431
432 @Test
433 void testAngles()
434 throws MathIllegalStateException {
435
436 for (RotationConvention convention : RotationConvention.values()) {
437 RotationOrder[] CardanOrders = {
438 RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
439 RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
440 };
441
442 for (final RotationOrder cardanOrder : CardanOrders) {
443 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
444 for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.3) {
445 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
446 Rotation r = new Rotation(cardanOrder, convention, alpha1, alpha2, alpha3);
447 double[] angles = r.getAngles(cardanOrder, convention);
448 checkAngle(angles[0], alpha1);
449 checkAngle(angles[1], alpha2);
450 checkAngle(angles[2], alpha3);
451 }
452 }
453 }
454 }
455
456 RotationOrder[] EulerOrders = {
457 RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
458 RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
459 };
460
461 for (final RotationOrder eulerOrder : EulerOrders) {
462 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
463 for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.3) {
464 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
465 Rotation r = new Rotation(eulerOrder, convention,
466 alpha1, alpha2, alpha3);
467 double[] angles = r.getAngles(eulerOrder, convention);
468 checkAngle(angles[0], alpha1);
469 checkAngle(angles[1], alpha2);
470 checkAngle(angles[2], alpha3);
471 }
472 }
473 }
474 }
475 }
476
477 }
478
479 @Test
480 void testSingularities() {
481
482 for (RotationConvention convention : RotationConvention.values()) {
483 RotationOrder[] CardanOrders = {
484 RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
485 RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
486 };
487
488 double[] singularCardanAngle = {
489 -FastMath.PI / 2, -FastMath.PI / 2 + 1.0e-12, -FastMath.PI / 2 + 1.0e-10,
490 FastMath.PI / 2 - 1.0e-10, FastMath.PI / 2 - 1.0e-12, FastMath.PI / 2
491 };
492 for (final RotationOrder cardanOrder : CardanOrders) {
493 for (final double v : singularCardanAngle) {
494 Rotation r = new Rotation(cardanOrder, convention, 0.1, v, 0.3);
495 assertEquals(v, r.getAngles(cardanOrder, convention)[1], 4.5e-16);
496 }
497 }
498
499 RotationOrder[] EulerOrders = {
500 RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
501 RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
502 };
503
504 double[] singularEulerAngle = { 0, 1.0e-12, 1.0e-10, FastMath.PI - 1.0e-10, FastMath.PI - 1.0e-12, FastMath.PI };
505 for (final RotationOrder eulerOrder : EulerOrders) {
506 for (final double v : singularEulerAngle) {
507 Rotation r = new Rotation(eulerOrder, convention, 0.1, v, 0.3);
508 assertEquals(v, r.getAngles(eulerOrder, convention)[1], 1.0e-24);
509 }
510 }
511 }
512
513
514 }
515
516 @Test
517 void testQuaternion() throws MathIllegalArgumentException {
518
519 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
520 double n = 23.5;
521 Rotation r2 = new Rotation(n * r1.getQ0(), n * r1.getQ1(),
522 n * r1.getQ2(), n * r1.getQ3(),
523 true);
524 for (double x = -0.9; x < 0.9; x += 0.2) {
525 for (double y = -0.9; y < 0.9; y += 0.2) {
526 for (double z = -0.9; z < 0.9; z += 0.2) {
527 Vector3D u = new Vector3D(x, y, z);
528 checkVector(r2.applyTo(u), r1.applyTo(u));
529 }
530 }
531 }
532
533 r1 = new Rotation( 0.288, 0.384, 0.36, 0.8, false);
534 checkRotation(r1, -r1.getQ0(), -r1.getQ1(), -r1.getQ2(), -r1.getQ3());
535
536 }
537
538 @Test
539 void testApplyTo() throws MathIllegalArgumentException {
540
541 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
542 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
543 Rotation r3 = r2.applyTo(r1);
544
545 for (double x = -0.9; x < 0.9; x += 0.2) {
546 for (double y = -0.9; y < 0.9; y += 0.2) {
547 for (double z = -0.9; z < 0.9; z += 0.2) {
548 Vector3D u = new Vector3D(x, y, z);
549 checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
550 }
551 }
552 }
553
554 }
555
556 @Test
557 void testComposeVectorOperator() throws MathIllegalArgumentException {
558
559 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
560 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
561 Rotation r3 = r2.compose(r1, RotationConvention.VECTOR_OPERATOR);
562
563 for (double x = -0.9; x < 0.9; x += 0.2) {
564 for (double y = -0.9; y < 0.9; y += 0.2) {
565 for (double z = -0.9; z < 0.9; z += 0.2) {
566 Vector3D u = new Vector3D(x, y, z);
567 checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
568 }
569 }
570 }
571
572 }
573
574 @Test
575 void testComposeFrameTransform() throws MathIllegalArgumentException {
576
577 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.FRAME_TRANSFORM);
578 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.FRAME_TRANSFORM);
579 Rotation r3 = r2.compose(r1, RotationConvention.FRAME_TRANSFORM);
580 Rotation r4 = r1.compose(r2, RotationConvention.VECTOR_OPERATOR);
581 assertEquals(0.0, Rotation.distance(r3, r4), 1.0e-15);
582
583 for (double x = -0.9; x < 0.9; x += 0.2) {
584 for (double y = -0.9; y < 0.9; y += 0.2) {
585 for (double z = -0.9; z < 0.9; z += 0.2) {
586 Vector3D u = new Vector3D(x, y, z);
587 checkVector(r1.applyTo(r2.applyTo(u)), r3.applyTo(u));
588 }
589 }
590 }
591
592 }
593
594 @Test
595 void testApplyInverseToRotation() throws MathIllegalArgumentException {
596
597 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
598 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
599 Rotation r3 = r2.applyInverseTo(r1);
600
601 for (double x = -0.9; x < 0.9; x += 0.2) {
602 for (double y = -0.9; y < 0.9; y += 0.2) {
603 for (double z = -0.9; z < 0.9; z += 0.2) {
604 Vector3D u = new Vector3D(x, y, z);
605 checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
606 }
607 }
608 }
609
610 }
611
612 @Test
613 void testComposeInverseVectorOperator() throws MathIllegalArgumentException {
614
615 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
616 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
617 Rotation r3 = r2.composeInverse(r1, RotationConvention.VECTOR_OPERATOR);
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 Vector3D u = new Vector3D(x, y, z);
623 checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
624 }
625 }
626 }
627
628 }
629
630 @Test
631 void testComposeInverseFrameTransform() throws MathIllegalArgumentException {
632
633 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.FRAME_TRANSFORM);
634 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.FRAME_TRANSFORM);
635 Rotation r3 = r2.composeInverse(r1, RotationConvention.FRAME_TRANSFORM);
636 Rotation r4 = r1.revert().composeInverse(r2.revert(), RotationConvention.VECTOR_OPERATOR);
637 assertEquals(0.0, Rotation.distance(r3, r4), 1.0e-15);
638
639 for (double x = -0.9; x < 0.9; x += 0.2) {
640 for (double y = -0.9; y < 0.9; y += 0.2) {
641 for (double z = -0.9; z < 0.9; z += 0.2) {
642 Vector3D u = new Vector3D(x, y, z);
643 checkVector(r1.applyTo(r2.applyInverseTo(u)), r3.applyTo(u));
644 }
645 }
646 }
647
648 }
649
650 @Test
651 void testArray() throws MathIllegalArgumentException {
652
653 Rotation r = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
654
655 for (double x = -0.9; x < 0.9; x += 0.2) {
656 for (double y = -0.9; y < 0.9; y += 0.2) {
657 for (double z = -0.9; z < 0.9; z += 0.2) {
658 Vector3D u = new Vector3D(x, y, z);
659 Vector3D v = r.applyTo(u);
660 double[] inOut = new double[] { x, y, z };
661 r.applyTo(inOut, inOut);
662 assertEquals(v.getX(), inOut[0], 1.0e-10);
663 assertEquals(v.getY(), inOut[1], 1.0e-10);
664 assertEquals(v.getZ(), inOut[2], 1.0e-10);
665 r.applyInverseTo(inOut, inOut);
666 assertEquals(u.getX(), inOut[0], 1.0e-10);
667 assertEquals(u.getY(), inOut[1], 1.0e-10);
668 assertEquals(u.getZ(), inOut[2], 1.0e-10);
669 }
670 }
671 }
672
673 }
674
675 @Test
676 void testApplyInverseTo() throws MathIllegalArgumentException {
677
678 Rotation r = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
679 for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
680 for (double phi = -1.55; phi < 1.55; phi += 0.2) {
681 Vector3D u = new Vector3D(FastMath.cos(lambda) * FastMath.cos(phi),
682 FastMath.sin(lambda) * FastMath.cos(phi),
683 FastMath.sin(phi));
684 r.applyInverseTo(r.applyTo(u));
685 checkVector(u, r.applyInverseTo(r.applyTo(u)));
686 checkVector(u, r.applyTo(r.applyInverseTo(u)));
687 }
688 }
689
690 r = Rotation.IDENTITY;
691 for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
692 for (double phi = -1.55; phi < 1.55; phi += 0.2) {
693 Vector3D u = new Vector3D(FastMath.cos(lambda) * FastMath.cos(phi),
694 FastMath.sin(lambda) * FastMath.cos(phi),
695 FastMath.sin(phi));
696 checkVector(u, r.applyInverseTo(r.applyTo(u)));
697 checkVector(u, r.applyTo(r.applyInverseTo(u)));
698 }
699 }
700
701 r = new Rotation(Vector3D.PLUS_K, FastMath.PI, RotationConvention.VECTOR_OPERATOR);
702 for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
703 for (double phi = -1.55; phi < 1.55; phi += 0.2) {
704 Vector3D u = new Vector3D(FastMath.cos(lambda) * FastMath.cos(phi),
705 FastMath.sin(lambda) * FastMath.cos(phi),
706 FastMath.sin(phi));
707 checkVector(u, r.applyInverseTo(r.applyTo(u)));
708 checkVector(u, r.applyTo(r.applyInverseTo(u)));
709 }
710 }
711
712 }
713
714 @Test
715 void testIssue639() throws MathRuntimeException{
716 Vector3D u1 = new Vector3D(-1321008684645961.0 / 268435456.0,
717 -5774608829631843.0 / 268435456.0,
718 -3822921525525679.0 / 4294967296.0);
719 Vector3D u2 =new Vector3D( -5712344449280879.0 / 2097152.0,
720 -2275058564560979.0 / 1048576.0,
721 4423475992255071.0 / 65536.0);
722 Rotation rot = new Rotation(u1, u2, Vector3D.PLUS_I,Vector3D.PLUS_K);
723 assertEquals( 0.6228370359608200639829222, rot.getQ0(), 1.0e-15);
724 assertEquals( 0.0257707621456498790029987, rot.getQ1(), 1.0e-15);
725 assertEquals(-0.0000000002503012255839931, rot.getQ2(), 1.0e-15);
726 assertEquals(-0.7819270390861109450724902, rot.getQ3(), 1.0e-15);
727 }
728
729 @Test
730 void testIssue801() throws MathRuntimeException {
731 Vector3D u1 = new Vector3D(0.9999988431610581, -0.0015210774290851095, 0.0);
732 Vector3D u2 = new Vector3D(0.0, 0.0, 1.0);
733
734 Vector3D v1 = new Vector3D(0.9999999999999999, 0.0, 0.0);
735 Vector3D v2 = new Vector3D(0.0, 0.0, -1.0);
736
737 Rotation quat = new Rotation(u1, u2, v1, v2);
738 double q2 = quat.getQ0() * quat.getQ0() +
739 quat.getQ1() * quat.getQ1() +
740 quat.getQ2() * quat.getQ2() +
741 quat.getQ3() * quat.getQ3();
742 assertEquals(1.0, q2, 1.0e-14);
743 assertEquals(0.0, Vector3D.angle(v1, quat.applyTo(u1)), 1.0e-14);
744 assertEquals(0.0, Vector3D.angle(v2, quat.applyTo(u2)), 1.0e-14);
745
746 }
747
748 @Test
749 void testGithubPullRequest22A() {
750 final RotationOrder order = RotationOrder.ZYX;
751 final double xRotation = FastMath.toDegrees(30);
752 final double yRotation = FastMath.toDegrees(20);
753 final double zRotation = FastMath.toDegrees(10);
754 final Vector3D startingVector = Vector3D.PLUS_I;
755 Vector3D appliedIndividually = startingVector;
756 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, zRotation, 0, 0).applyTo(appliedIndividually);
757 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, yRotation, 0).applyTo(appliedIndividually);
758 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, 0, xRotation).applyTo(appliedIndividually);
759
760 final Vector3D bad = new Rotation(order, RotationConvention.FRAME_TRANSFORM, zRotation, yRotation, xRotation).applyTo(startingVector);
761
762 assertEquals(bad.getX(), appliedIndividually.getX(), 1e-12);
763 assertEquals(bad.getY(), appliedIndividually.getY(), 1e-12);
764 assertEquals(bad.getZ(), appliedIndividually.getZ(), 1e-12);
765 }
766
767 @Test
768 void testGithubPullRequest22B() {
769 final RotationOrder order = RotationOrder.ZYX;
770 final double xRotation = FastMath.toDegrees(30);
771 final double yRotation = FastMath.toDegrees(20);
772 final double zRotation = FastMath.toDegrees(10);
773 final Vector3D startingVector = Vector3D.PLUS_I;
774 Vector3D appliedIndividually = startingVector;
775 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, zRotation, 0, 0).applyTo(appliedIndividually);
776 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, yRotation, 0).applyTo(appliedIndividually);
777 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, 0, xRotation).applyTo(appliedIndividually);
778
779 final Rotation r1 = new Rotation(order.getA1(), zRotation, RotationConvention.FRAME_TRANSFORM);
780 final Rotation r2 = new Rotation(order.getA2(), yRotation, RotationConvention.FRAME_TRANSFORM);
781 final Rotation r3 = new Rotation(order.getA3(), xRotation, RotationConvention.FRAME_TRANSFORM);
782 final Rotation composite = r1.compose(r2.compose(r3,
783 RotationConvention.FRAME_TRANSFORM),
784 RotationConvention.FRAME_TRANSFORM);
785 final Vector3D good = composite.applyTo(startingVector);
786
787 assertEquals(good.getX(), appliedIndividually.getX(), 1e-12);
788 assertEquals(good.getY(), appliedIndividually.getY(), 1e-12);
789 assertEquals(good.getZ(), appliedIndividually.getZ(), 1e-12);
790 }
791
792 private void checkVector(Vector3D v1, Vector3D v2) {
793 assertTrue(v1.subtract(v2).getNorm() < 1.0e-10);
794 }
795
796 private void checkAngle(double a1, double a2) {
797 assertEquals(a1, MathUtils.normalizeAngle(a2, a1), 1.0e-10);
798 }
799
800 private void checkRotation(Rotation r, double q0, double q1, double q2, double q3) {
801 assertEquals(0, Rotation.distance(r, new Rotation(q0, q1, q2, q3, false)), 1.0e-12);
802 }
803
804 }