View Javadoc
1   /*
2    * Licensed to the Apache Software Foundation (ASF) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * The ASF licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *      http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  package org.apache.commons.geometry.euclidean.threed.rotation;
18  
19  import java.util.List;
20  import java.util.function.DoubleFunction;
21  import java.util.function.UnaryOperator;
22  import java.util.stream.Collectors;
23  import java.util.stream.Stream;
24  
25  import org.apache.commons.geometry.core.GeometryTestUtils;
26  import org.apache.commons.geometry.core.internal.SimpleTupleFormat;
27  import org.apache.commons.geometry.euclidean.EuclideanTestUtils;
28  import org.apache.commons.geometry.euclidean.threed.AffineTransformMatrix3D;
29  import org.apache.commons.geometry.euclidean.threed.Vector3D;
30  import org.apache.commons.numbers.angle.Angle;
31  import org.apache.commons.numbers.core.Precision;
32  import org.apache.commons.numbers.quaternion.Quaternion;
33  import org.apache.commons.rng.UniformRandomProvider;
34  import org.apache.commons.rng.simple.RandomSource;
35  import org.junit.jupiter.api.Assertions;
36  import org.junit.jupiter.api.Test;
37  
38  class QuaternionRotationTest {
39  
40      private static final double EPS = 1e-12;
41  
42      // use non-normalized axes to ensure that the axis is normalized
43      private static final Vector3D PLUS_X_DIR = Vector3D.of(2, 0, 0);
44      private static final Vector3D MINUS_X_DIR = Vector3D.of(-2, 0, 0);
45  
46      private static final Vector3D PLUS_Y_DIR = Vector3D.of(0, 3, 0);
47      private static final Vector3D MINUS_Y_DIR = Vector3D.of(0, -3, 0);
48  
49      private static final Vector3D PLUS_Z_DIR = Vector3D.of(0, 0, 4);
50      private static final Vector3D MINUS_Z_DIR = Vector3D.of(0, 0, -4);
51  
52      private static final Vector3D PLUS_DIAGONAL = Vector3D.of(1, 1, 1);
53      private static final Vector3D MINUS_DIAGONAL = Vector3D.of(-1, -1, -1);
54  
55      private static final double TWO_THIRDS_PI = 2.0 * Math.PI / 3.0;
56      private static final double MINUS_TWO_THIRDS_PI = -TWO_THIRDS_PI;
57  
58      @Test
59      void testOf_quaternion() {
60          // act/assert
61          checkQuaternion(QuaternionRotation.of(Quaternion.of(1, 0, 0, 0)), 1, 0, 0, 0);
62          checkQuaternion(QuaternionRotation.of(Quaternion.of(-1, 0, 0, 0)), 1, 0, 0, 0);
63          checkQuaternion(QuaternionRotation.of(Quaternion.of(0, 1, 0, 0)), 0, 1, 0, 0);
64          checkQuaternion(QuaternionRotation.of(Quaternion.of(0, 0, 1, 0)), 0, 0, 1, 0);
65          checkQuaternion(QuaternionRotation.of(Quaternion.of(0, 0, 0, 1)), 0, 0, 0, 1);
66  
67          checkQuaternion(QuaternionRotation.of(Quaternion.of(1, 1, 1, 1)), 0.5, 0.5, 0.5, 0.5);
68          checkQuaternion(QuaternionRotation.of(Quaternion.of(-1, -1, -1, -1)), 0.5, 0.5, 0.5, 0.5);
69      }
70  
71      @Test
72      void testOf_quaternion_illegalNorm() {
73          // act/assert
74          Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(Quaternion.of(0, 0, 0, 0)));
75          Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(Quaternion.of(1, 1, 1, Double.NaN)));
76          Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(Quaternion.of(1, 1, Double.POSITIVE_INFINITY, 1)));
77          Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(Quaternion.of(1, Double.NEGATIVE_INFINITY, 1, 1)));
78          Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(Quaternion.of(Double.NaN, 1, 1, 1)));
79      }
80  
81      @Test
82      void testOf_components() {
83          // act/assert
84          checkQuaternion(QuaternionRotation.of(1, 0, 0, 0), 1, 0, 0, 0);
85          checkQuaternion(QuaternionRotation.of(-1, 0, 0, 0), 1, 0, 0, 0);
86          checkQuaternion(QuaternionRotation.of(0, 1, 0, 0), 0, 1, 0, 0);
87          checkQuaternion(QuaternionRotation.of(0, 0, 1, 0), 0, 0, 1, 0);
88          checkQuaternion(QuaternionRotation.of(0, 0, 0, 1), 0, 0, 0, 1);
89  
90          checkQuaternion(QuaternionRotation.of(1, 1, 1, 1), 0.5, 0.5, 0.5, 0.5);
91          checkQuaternion(QuaternionRotation.of(-1, -1, -1, -1), 0.5, 0.5, 0.5, 0.5);
92      }
93  
94      @Test
95      void testOf_components_illegalNorm() {
96          // act/assert
97          Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(0, 0, 0, 0));
98          Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(1, 1, 1, Double.NaN));
99          Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(1, 1, Double.POSITIVE_INFINITY, 1));
100         Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(1, Double.NEGATIVE_INFINITY, 1, 1));
101         Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(Double.NaN, 1, 1, 1));
102     }
103 
104     @Test
105     void testIdentity() {
106         // act
107         final QuaternionRotation q = QuaternionRotation.identity();
108 
109         // assert
110         assertRotationEquals(StandardRotations.IDENTITY, q);
111     }
112 
113     @Test
114     void testIdentity_axis() {
115         // arrange
116         final QuaternionRotation q = QuaternionRotation.identity();
117 
118         // act/assert
119         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, q.getAxis(), EPS);
120     }
121 
122     @Test
123     void testGetAxis() {
124         // act/assert
125         checkVector(QuaternionRotation.of(0, 1, 0, 0).getAxis(), 1, 0, 0);
126         checkVector(QuaternionRotation.of(0, -1, 0, 0).getAxis(), -1, 0, 0);
127 
128         checkVector(QuaternionRotation.of(0, 0, 1, 0).getAxis(), 0, 1, 0);
129         checkVector(QuaternionRotation.of(0, 0, -1, 0).getAxis(), 0, -1, 0);
130 
131         checkVector(QuaternionRotation.of(0, 0, 0, 1).getAxis(), 0, 0, 1);
132         checkVector(QuaternionRotation.of(0, 0, 0, -1).getAxis(), 0, 0, -1);
133     }
134 
135     @Test
136     void testGetAxis_noAxis() {
137         // arrange
138         final QuaternionRotation rot = QuaternionRotation.of(1, 0, 0, 0);
139 
140         // act/assert
141         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, rot.getAxis(), EPS);
142     }
143 
144     @Test
145     void testGetAxis_matchesAxisAngleConstruction() {
146         EuclideanTestUtils.permuteSkipZero(-5, 5, 1, (x, y, z) -> {
147             // arrange
148             final Vector3D vec = Vector3D.of(x, y, z);
149             final Vector3D norm = vec.normalize();
150 
151             // act/assert
152 
153             // positive angle results in the axis being the normalized input axis
154             EuclideanTestUtils.assertCoordinatesEqual(norm,
155                     QuaternionRotation.fromAxisAngle(vec, Angle.PI_OVER_TWO).getAxis(), EPS);
156 
157             // negative angle results in the axis being the negated normalized input axis
158             EuclideanTestUtils.assertCoordinatesEqual(norm,
159                     QuaternionRotation.fromAxisAngle(vec.negate(), -Angle.PI_OVER_TWO).getAxis(), EPS);
160         });
161     }
162 
163     @Test
164     void testGetAngle() {
165         // act/assert
166         Assertions.assertEquals(0.0, QuaternionRotation.of(1, 0, 0, 0).getAngle(), EPS);
167         Assertions.assertEquals(0.0, QuaternionRotation.of(-1, 0, 0, 0).getAngle(), EPS);
168 
169         Assertions.assertEquals(Angle.PI_OVER_TWO, QuaternionRotation.of(1, 0, 0, 1).getAngle(), EPS);
170         Assertions.assertEquals(Angle.PI_OVER_TWO, QuaternionRotation.of(-1, 0, 0, -1).getAngle(), EPS);
171 
172         Assertions.assertEquals(Math.PI  * 2.0 / 3.0, QuaternionRotation.of(1, 1, 1, 1).getAngle(), EPS);
173 
174         Assertions.assertEquals(Math.PI, QuaternionRotation.of(0, 0, 0, 1).getAngle(), EPS);
175     }
176 
177     @Test
178     void testGetAngle_matchesAxisAngleConstruction() {
179         for (double theta = -2 * Math.PI; theta <= 2 * Math.PI; theta += 0.1) {
180             // arrange
181             final QuaternionRotation rot = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, theta);
182 
183             // act
184             final double angle = rot.getAngle();
185 
186             // assert
187             // make sure that we're in the [0, pi] range
188             Assertions.assertTrue(angle >= 0.0);
189             Assertions.assertTrue(angle <= Math.PI);
190 
191             double expected = Angle.Rad.WITHIN_MINUS_PI_AND_PI.applyAsDouble(theta);
192             if (PLUS_DIAGONAL.dot(rot.getAxis()) < 0) {
193                 // if the axis ended up being flipped, then negate the expected angle
194                 expected *= -1;
195             }
196 
197             Assertions.assertEquals(expected, angle, EPS);
198         }
199     }
200 
201     @Test
202     void testFromAxisAngle_apply() {
203         // act/assert
204 
205         // --- x axes
206         assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, 0.0));
207 
208         assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Angle.PI_OVER_TWO));
209         assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, -Angle.PI_OVER_TWO));
210 
211         assertRotationEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Angle.PI_OVER_TWO));
212         assertRotationEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, -Angle.PI_OVER_TWO));
213 
214         assertRotationEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Math.PI));
215         assertRotationEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Math.PI));
216 
217         // --- y axes
218         assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, 0.0));
219 
220         assertRotationEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Angle.PI_OVER_TWO));
221         assertRotationEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, -Angle.PI_OVER_TWO));
222 
223         assertRotationEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Angle.PI_OVER_TWO));
224         assertRotationEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, -Angle.PI_OVER_TWO));
225 
226         assertRotationEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Math.PI));
227         assertRotationEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Math.PI));
228 
229         // --- z axes
230         assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, 0.0));
231 
232         assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Angle.PI_OVER_TWO));
233         assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, -Angle.PI_OVER_TWO));
234 
235         assertRotationEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Angle.PI_OVER_TWO));
236         assertRotationEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, -Angle.PI_OVER_TWO));
237 
238         assertRotationEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Math.PI));
239         assertRotationEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Math.PI));
240 
241         // --- diagonal
242         assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, TWO_THIRDS_PI));
243         assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, MINUS_TWO_THIRDS_PI));
244 
245         assertRotationEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, TWO_THIRDS_PI));
246         assertRotationEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, MINUS_TWO_THIRDS_PI));
247     }
248 
249     @Test
250     void testFromAxisAngle_invalidAxisNorm() {
251         // act/assert
252         Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.fromAxisAngle(Vector3D.ZERO, Angle.PI_OVER_TWO));
253         Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.fromAxisAngle(Vector3D.NaN, Angle.PI_OVER_TWO));
254         Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.fromAxisAngle(Vector3D.POSITIVE_INFINITY, Angle.PI_OVER_TWO));
255         Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.fromAxisAngle(Vector3D.NEGATIVE_INFINITY, Angle.PI_OVER_TWO));
256     }
257 
258     @Test
259     void testFromAxisAngle_invalidAngle() {
260         // act/assert
261         GeometryTestUtils.assertThrowsWithMessage(() -> QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Double.NaN),
262                 IllegalArgumentException.class, "Invalid angle: NaN");
263         GeometryTestUtils.assertThrowsWithMessage(() -> QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Double.POSITIVE_INFINITY),
264                 IllegalArgumentException.class, "Invalid angle: Infinity");
265         GeometryTestUtils.assertThrowsWithMessage(() -> QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Double.NEGATIVE_INFINITY),
266                 IllegalArgumentException.class, "Invalid angle: -Infinity");
267     }
268 
269     @Test
270     void testApplyVector() {
271         // arrange
272         final QuaternionRotation q = QuaternionRotation.fromAxisAngle(Vector3D.of(1, 1, 1), Angle.PI_OVER_TWO);
273 
274         EuclideanTestUtils.permute(-2, 2, 0.2, (x, y, z) -> {
275             final Vector3D input = Vector3D.of(x, y, z);
276 
277             // act
278             final Vector3D pt = q.apply(input);
279             final Vector3D vec = q.applyVector(input);
280 
281             EuclideanTestUtils.assertCoordinatesEqual(pt, vec, EPS);
282         });
283     }
284 
285     @Test
286     void testInverse() {
287         // arrange
288         final QuaternionRotation rot = QuaternionRotation.of(0.5, 0.5, 0.5, 0.5);
289 
290         // act
291         final QuaternionRotation neg = rot.inverse();
292 
293         // assert
294         Assertions.assertEquals(-0.5, neg.getQuaternion().getX(), EPS);
295         Assertions.assertEquals(-0.5, neg.getQuaternion().getY(), EPS);
296         Assertions.assertEquals(-0.5, neg.getQuaternion().getZ(), EPS);
297         Assertions.assertEquals(0.5, neg.getQuaternion().getW(), EPS);
298     }
299 
300     @Test
301     void testInverse_apply() {
302         // act/assert
303 
304         // --- x axes
305         assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, 0.0).inverse());
306 
307         assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, -Angle.PI_OVER_TWO).inverse());
308         assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Angle.PI_OVER_TWO).inverse());
309 
310         assertRotationEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, -Angle.PI_OVER_TWO).inverse());
311         assertRotationEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Angle.PI_OVER_TWO).inverse());
312 
313         assertRotationEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Math.PI).inverse());
314         assertRotationEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Math.PI).inverse());
315 
316         // --- y axes
317         assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, 0.0).inverse());
318 
319         assertRotationEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, -Angle.PI_OVER_TWO).inverse());
320         assertRotationEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Angle.PI_OVER_TWO).inverse());
321 
322         assertRotationEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, -Angle.PI_OVER_TWO).inverse());
323         assertRotationEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Angle.PI_OVER_TWO).inverse());
324 
325         assertRotationEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Math.PI).inverse());
326         assertRotationEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Math.PI).inverse());
327 
328         // --- z axes
329         assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, 0.0).inverse());
330 
331         assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, -Angle.PI_OVER_TWO).inverse());
332         assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Angle.PI_OVER_TWO).inverse());
333 
334         assertRotationEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, -Angle.PI_OVER_TWO).inverse());
335         assertRotationEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Angle.PI_OVER_TWO).inverse());
336 
337         assertRotationEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Math.PI).inverse());
338         assertRotationEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Math.PI).inverse());
339 
340         // --- diagonal
341         assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, MINUS_TWO_THIRDS_PI).inverse());
342         assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, TWO_THIRDS_PI).inverse());
343 
344         assertRotationEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, MINUS_TWO_THIRDS_PI).inverse());
345         assertRotationEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, TWO_THIRDS_PI).inverse());
346     }
347 
348     @Test
349     void testInverse_undoesOriginalRotation() {
350         EuclideanTestUtils.permuteSkipZero(-5, 5, 1, (x, y, z) -> {
351             // arrange
352             final Vector3D vec = Vector3D.of(x, y, z);
353 
354             final QuaternionRotation rot = QuaternionRotation.fromAxisAngle(vec, 0.75 * Math.PI);
355             final QuaternionRotation neg = rot.inverse();
356 
357             // act/assert
358             EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL, neg.apply(rot.apply(PLUS_DIAGONAL)), EPS);
359             EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL, rot.apply(neg.apply(PLUS_DIAGONAL)), EPS);
360         });
361     }
362 
363     @Test
364     void testMultiply_sameAxis_simple() {
365         // arrange
366         final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, 0.1 * Math.PI);
367         final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, 0.4 * Math.PI);
368 
369         // act
370         final QuaternionRotation result = q1.multiply(q2);
371 
372         // assert
373         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, result.getAxis(), EPS);
374         Assertions.assertEquals(Angle.PI_OVER_TWO, result.getAngle(), EPS);
375 
376         assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, result);
377     }
378 
379     @Test
380     void testMultiply_sameAxis_multiple() {
381         // arrange
382         final double oneThird = 1.0 / 3.0;
383         final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, 0.1 * Math.PI);
384         final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, oneThird * Math.PI);
385         final QuaternionRotation q3 = QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, 0.4 * Math.PI);
386         final QuaternionRotation q4 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, 0.3 * Math.PI);
387         final QuaternionRotation q5 = QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, -oneThird * Math.PI);
388 
389         // act
390         final QuaternionRotation result = q1.multiply(q2).multiply(q3).multiply(q4).multiply(q5);
391 
392         // assert
393         EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL.normalize(), result.getAxis(), EPS);
394         Assertions.assertEquals(2.0 * Math.PI / 3.0, result.getAngle(), EPS);
395 
396         assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, result);
397     }
398 
399     @Test
400     void testMultiply_differentAxes() {
401         // arrange
402         final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Angle.PI_OVER_TWO);
403         final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Y, Angle.PI_OVER_TWO);
404 
405         // act
406         final QuaternionRotation result = q1.multiply(q2);
407 
408         // assert
409         EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL.normalize(), result.getAxis(), EPS);
410         Assertions.assertEquals(2.0 * Math.PI / 3.0, result.getAngle(), EPS);
411 
412         assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, result);
413 
414         assertRotationEquals(v -> {
415             final Vector3D temp = StandardRotations.PLUS_Y_HALF_PI.apply(v);
416             return StandardRotations.PLUS_X_HALF_PI.apply(temp);
417         }, result);
418     }
419 
420     @Test
421     void testMultiply_orderOfOperations() {
422         // arrange
423         final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Angle.PI_OVER_TWO);
424         final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Y, Math.PI);
425         final QuaternionRotation q3 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_Z, Angle.PI_OVER_TWO);
426 
427         // act
428         final QuaternionRotation result = q3.multiply(q2).multiply(q1);
429 
430         // assert
431         assertRotationEquals(v -> {
432             Vector3D temp = StandardRotations.PLUS_X_HALF_PI.apply(v);
433             temp = StandardRotations.Y_PI.apply(temp);
434             return StandardRotations.MINUS_Z_HALF_PI.apply(temp);
435         }, result);
436     }
437 
438     @Test
439     void testMultiply_numericalStability() {
440         // arrange
441         final int slices = 1024;
442         final double delta = (8.0 * Math.PI / 3.0) / slices;
443 
444         QuaternionRotation q = QuaternionRotation.identity();
445 
446         final UniformRandomProvider rand = RandomSource.create(RandomSource.JDK, 2L);
447 
448         // act
449         for (int i = 0; i < slices; ++i) {
450             final double angle = rand.nextDouble();
451             final QuaternionRotation forward = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, angle);
452             final QuaternionRotation backward = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, delta - angle);
453 
454             q = q.multiply(forward).multiply(backward);
455         }
456 
457         // assert
458         Assertions.assertTrue(q.getQuaternion().getW() > 0);
459         Assertions.assertEquals(1.0, q.getQuaternion().norm(), EPS);
460 
461         assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, q);
462     }
463 
464     @Test
465     void testPremultiply_sameAxis_simple() {
466         // arrange
467         final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, 0.1 * Math.PI);
468         final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, 0.4 * Math.PI);
469 
470         // act
471         final QuaternionRotation result = q1.premultiply(q2);
472 
473         // assert
474         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, result.getAxis(), EPS);
475         Assertions.assertEquals(Angle.PI_OVER_TWO, result.getAngle(), EPS);
476 
477         assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, result);
478     }
479 
480     @Test
481     void testPremultiply_sameAxis_multiple() {
482         // arrange
483         final double oneThird = 1.0 / 3.0;
484         final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, 0.1 * Math.PI);
485         final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, oneThird * Math.PI);
486         final QuaternionRotation q3 = QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, 0.4 * Math.PI);
487         final QuaternionRotation q4 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, 0.3 * Math.PI);
488         final QuaternionRotation q5 = QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, -oneThird * Math.PI);
489 
490         // act
491         final QuaternionRotation result = q1.premultiply(q2).premultiply(q3).premultiply(q4).premultiply(q5);
492 
493         // assert
494         EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL.normalize(), result.getAxis(), EPS);
495         Assertions.assertEquals(2.0 * Math.PI / 3.0, result.getAngle(), EPS);
496 
497         assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, result);
498     }
499 
500     @Test
501     void testPremultiply_differentAxes() {
502         // arrange
503         final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Angle.PI_OVER_TWO);
504         final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Y, Angle.PI_OVER_TWO);
505 
506         // act
507         final QuaternionRotation result = q2.premultiply(q1);
508 
509         // assert
510         EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL.normalize(), result.getAxis(), EPS);
511         Assertions.assertEquals(2.0 * Math.PI / 3.0, result.getAngle(), EPS);
512 
513         assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, result);
514 
515         assertRotationEquals(v -> {
516             final Vector3D temp = StandardRotations.PLUS_Y_HALF_PI.apply(v);
517             return StandardRotations.PLUS_X_HALF_PI.apply(temp);
518         }, result);
519     }
520 
521     @Test
522     void testPremultiply_orderOfOperations() {
523         // arrange
524         final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Angle.PI_OVER_TWO);
525         final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Y, Math.PI);
526         final QuaternionRotation q3 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_Z, Angle.PI_OVER_TWO);
527 
528         // act
529         final QuaternionRotation result = q1.premultiply(q2).premultiply(q3);
530 
531         // assert
532         assertRotationEquals(v -> {
533             Vector3D temp = StandardRotations.PLUS_X_HALF_PI.apply(v);
534             temp = StandardRotations.Y_PI.apply(temp);
535             return StandardRotations.MINUS_Z_HALF_PI.apply(temp);
536         }, result);
537     }
538 
539     @Test
540     void testSlerp_simple() {
541         // arrange
542         final QuaternionRotation q0 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, 0.0);
543         final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, Math.PI);
544         final DoubleFunction<QuaternionRotation> fn = q0.slerp(q1);
545         final Vector3D v = Vector3D.of(2, 0, 1);
546 
547         final double sqrt2 = Math.sqrt(2);
548 
549         // act
550         checkVector(fn.apply(0).apply(v), 2, 0, 1);
551         checkVector(fn.apply(0.25).apply(v), sqrt2, sqrt2, 1);
552         checkVector(fn.apply(0.5).apply(v), 0, 2, 1);
553         checkVector(fn.apply(0.75).apply(v), -sqrt2, sqrt2, 1);
554         checkVector(fn.apply(1).apply(v), -2, 0, 1);
555     }
556 
557     @Test
558     void testSlerp_multipleCombinations() {
559         // arrange
560         final QuaternionRotation[] rotations = {
561                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, 0.0),
562                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Angle.PI_OVER_TWO),
563                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Math.PI),
564 
565                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_X, 0.0),
566                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_X, Angle.PI_OVER_TWO),
567                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_X, Math.PI),
568 
569                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Y, 0.0),
570                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Y, Angle.PI_OVER_TWO),
571                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Y, Math.PI),
572 
573                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_Y, 0.0),
574                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_Y, Angle.PI_OVER_TWO),
575                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_Y, Math.PI),
576 
577                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, 0.0),
578                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, Angle.PI_OVER_TWO),
579                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, Math.PI),
580 
581                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_Z, 0.0),
582                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_Z, Angle.PI_OVER_TWO),
583                 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_Z, Math.PI),
584         };
585 
586         // act/assert
587         // test each rotation against all of the others (including itself)
588         for (final QuaternionRotation quaternionRotation : rotations) {
589             for (final QuaternionRotation rotation : rotations) {
590                 checkSlerpCombination(quaternionRotation, rotation);
591             }
592         }
593     }
594 
595     private void checkSlerpCombination(final QuaternionRotation start, final QuaternionRotation end) {
596         final DoubleFunction<QuaternionRotation> slerp = start.slerp(end);
597         final Vector3D vec = Vector3D.of(1, 1, 1).normalize();
598 
599         final Vector3D startVec = start.apply(vec);
600         final Vector3D endVec = end.apply(vec);
601 
602         // check start and end values
603         EuclideanTestUtils.assertCoordinatesEqual(startVec, slerp.apply(0).apply(vec), EPS);
604         EuclideanTestUtils.assertCoordinatesEqual(endVec, slerp.apply(1).apply(vec), EPS);
605 
606         // check intermediate values
607         double prevAngle = -1;
608         final int numSteps = 100;
609         final double delta = 1d / numSteps;
610         for (int step = 0; step <= numSteps; step++) {
611             final double t = step * delta;
612             final QuaternionRotation result = slerp.apply(t);
613 
614             final Vector3D slerpVec = result.apply(vec);
615             Assertions.assertEquals(1, slerpVec.norm(), EPS);
616 
617             // make sure that we're steadily progressing to the end angle
618             final double angle = slerpVec.angle(startVec);
619             Assertions.assertTrue(Precision.compareTo(angle, prevAngle, EPS) >= 0, "Expected slerp angle to continuously increase; previous angle was " +
620                     prevAngle + " and new angle is " + angle);
621 
622             prevAngle = angle;
623         }
624     }
625 
626     @Test
627     void testSlerp_followsShortestPath() {
628         // arrange
629         final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, 0.75 * Math.PI);
630         final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, -0.75 * Math.PI);
631 
632         // act
633         final QuaternionRotation result = q1.slerp(q2).apply(0.5);
634 
635         // assert
636         // the slerp should have followed the path around the pi coordinate of the circle rather than
637         // the one through the zero coordinate
638         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.MINUS_X, result.apply(Vector3D.Unit.PLUS_X), EPS);
639 
640         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_Z, result.getAxis(), EPS);
641         Assertions.assertEquals(Math.PI, result.getAngle(), EPS);
642     }
643 
644     @Test
645     void testSlerp_inputQuaternionsHaveMinusOneDotProduct() {
646         // arrange
647         final QuaternionRotation q1 = QuaternionRotation.of(1, 0, 0, 1); // pi/2 around +z
648         final QuaternionRotation q2 = QuaternionRotation.of(-1, 0, 0, -1); // 3pi/2 around -z
649 
650         // act
651         final QuaternionRotation result = q1.slerp(q2).apply(0.5);
652 
653         // assert
654         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_Y, result.apply(Vector3D.Unit.PLUS_X), EPS);
655 
656         Assertions.assertEquals(Angle.PI_OVER_TWO, result.getAngle(), EPS);
657         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_Z, result.getAxis(), EPS);
658     }
659 
660     @Test
661     void testSlerp_outputQuaternionIsNormalizedForAllT() {
662         // arrange
663         final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, 0.25 * Math.PI);
664         final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, 0.75 * Math.PI);
665 
666         final int numSteps = 200;
667         final double delta = 1d / numSteps;
668         for (int step = 0; step <= numSteps; step++) {
669             final double t = -10 + step * delta;
670 
671             // act
672             final QuaternionRotation result = q1.slerp(q2).apply(t);
673 
674             // assert
675             Assertions.assertEquals(1.0, result.getQuaternion().norm(), EPS);
676         }
677     }
678 
679     @Test
680     void testSlerp_tOutsideOfZeroToOne_apply() {
681         // arrange
682         final Vector3D vec = Vector3D.Unit.PLUS_X;
683 
684         final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, 0.25 * Math.PI);
685         final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, 0.75 * Math.PI);
686 
687         // act/assert
688         final DoubleFunction<QuaternionRotation> slerp12 = q1.slerp(q2);
689         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, slerp12.apply(-4.5).apply(vec), EPS);
690         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, slerp12.apply(-0.5).apply(vec), EPS);
691         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.MINUS_X, slerp12.apply(1.5).apply(vec), EPS);
692         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.MINUS_X, slerp12.apply(5.5).apply(vec), EPS);
693 
694         final DoubleFunction<QuaternionRotation> slerp21 = q2.slerp(q1);
695         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.MINUS_X, slerp21.apply(-4.5).apply(vec), EPS);
696         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.MINUS_X, slerp21.apply(-0.5).apply(vec), EPS);
697         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, slerp21.apply(1.5).apply(vec), EPS);
698         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, slerp21.apply(5.5).apply(vec), EPS);
699     }
700 
701     @Test
702     void testToMatrix() {
703         // act/assert
704         // --- x axes
705         assertTransformEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, 0.0).toMatrix());
706 
707         assertTransformEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Angle.PI_OVER_TWO).toMatrix());
708         assertTransformEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, -Angle.PI_OVER_TWO).toMatrix());
709 
710         assertTransformEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Angle.PI_OVER_TWO).toMatrix());
711         assertTransformEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, -Angle.PI_OVER_TWO).toMatrix());
712 
713         assertTransformEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Math.PI).toMatrix());
714         assertTransformEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Math.PI).toMatrix());
715 
716         // --- y axes
717         assertTransformEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, 0.0).toMatrix());
718 
719         assertTransformEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Angle.PI_OVER_TWO).toMatrix());
720         assertTransformEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, -Angle.PI_OVER_TWO).toMatrix());
721 
722         assertTransformEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Angle.PI_OVER_TWO).toMatrix());
723         assertTransformEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, -Angle.PI_OVER_TWO).toMatrix());
724 
725         assertTransformEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Math.PI).toMatrix());
726         assertTransformEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Math.PI).toMatrix());
727 
728         // --- z axes
729         assertTransformEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, 0.0).toMatrix());
730 
731         assertTransformEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Angle.PI_OVER_TWO).toMatrix());
732         assertTransformEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, -Angle.PI_OVER_TWO).toMatrix());
733 
734         assertTransformEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Angle.PI_OVER_TWO).toMatrix());
735         assertTransformEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, -Angle.PI_OVER_TWO).toMatrix());
736 
737         assertTransformEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Math.PI).toMatrix());
738         assertTransformEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Math.PI).toMatrix());
739 
740         // --- diagonal
741         assertTransformEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, TWO_THIRDS_PI).toMatrix());
742         assertTransformEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, MINUS_TWO_THIRDS_PI).toMatrix());
743 
744         assertTransformEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, TWO_THIRDS_PI).toMatrix());
745         assertTransformEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, MINUS_TWO_THIRDS_PI).toMatrix());
746     }
747 
748     @Test
749     void testAxisAngleSequenceConversion_relative() {
750         for (final AxisSequence axes : AxisSequence.values()) {
751             checkAxisAngleSequenceToQuaternionRoundtrip(AxisReferenceFrame.RELATIVE, axes);
752             checkQuaternionToAxisAngleSequenceRoundtrip(AxisReferenceFrame.RELATIVE, axes);
753         }
754     }
755 
756     @Test
757     void testAxisAngleSequenceConversion_absolute() {
758         for (final AxisSequence axes : AxisSequence.values()) {
759             checkAxisAngleSequenceToQuaternionRoundtrip(AxisReferenceFrame.ABSOLUTE, axes);
760             checkQuaternionToAxisAngleSequenceRoundtrip(AxisReferenceFrame.ABSOLUTE, axes);
761         }
762     }
763 
764     private void checkAxisAngleSequenceToQuaternionRoundtrip(final AxisReferenceFrame frame, final AxisSequence axes) {
765         final double step = 0.3;
766         final double angle2Start = axes.getType() == AxisSequenceType.EULER ? 0.0 + 0.1 : -Angle.PI_OVER_TWO + 0.1;
767         final double angle2Stop = angle2Start + Math.PI;
768 
769         for (double angle1 = 0.0; angle1 <= Angle.TWO_PI; angle1 += step) {
770             for (double angle2 = angle2Start; angle2 < angle2Stop; angle2 += step) {
771                 for (double angle3 = 0.0; angle3 <= Angle.TWO_PI; angle3 += 0.3) {
772                     // arrange
773                     final AxisAngleSequence angles = new AxisAngleSequence(frame, axes, angle1, angle2, angle3);
774 
775                     // act
776                     final QuaternionRotation q = QuaternionRotation.fromAxisAngleSequence(angles);
777                     final AxisAngleSequence result = q.toAxisAngleSequence(frame, axes);
778 
779                     // assert
780                     Assertions.assertEquals(frame, result.getReferenceFrame());
781                     Assertions.assertEquals(axes, result.getAxisSequence());
782 
783                     assertRadiansEquals(angle1, result.getAngle1());
784                     assertRadiansEquals(angle2, result.getAngle2());
785                     assertRadiansEquals(angle3, result.getAngle3());
786                 }
787             }
788         }
789     }
790 
791     private void checkQuaternionToAxisAngleSequenceRoundtrip(final AxisReferenceFrame frame, final AxisSequence axes) {
792         final double step = 0.1;
793 
794         EuclideanTestUtils.permuteSkipZero(-1, 1, 0.5, (x, y, z) -> {
795             final Vector3D axis = Vector3D.of(x, y, z);
796 
797             for (double angle = -Angle.TWO_PI; angle <= Angle.TWO_PI; angle += step) {
798                 // arrange
799                 final QuaternionRotation q = QuaternionRotation.fromAxisAngle(axis, angle);
800 
801                 // act
802                 final AxisAngleSequence seq = q.toAxisAngleSequence(frame, axes);
803                 final QuaternionRotation result = QuaternionRotation.fromAxisAngleSequence(seq);
804 
805                 // assert
806                 checkQuaternion(result, q.getQuaternion().getW(), q.getQuaternion().getX(), q.getQuaternion().getY(), q.getQuaternion().getZ());
807             }
808         });
809     }
810 
811     @Test
812     void testAxisAngleSequenceConversion_relative_eulerSingularities() {
813         // arrange
814         final double[] eulerSingularities = {
815             0.0,
816             Math.PI
817         };
818 
819         final double angle1 = 0.1;
820         final double angle2 = 0.3;
821 
822         final AxisReferenceFrame frame = AxisReferenceFrame.RELATIVE;
823 
824         for (final AxisSequence axes : getAxes(AxisSequenceType.EULER)) {
825             for (final double singularityAngle : eulerSingularities) {
826 
827                 final AxisAngleSequence inputSeq = new AxisAngleSequence(frame, axes, angle1, singularityAngle, angle2);
828                 final QuaternionRotation inputQuat = QuaternionRotation.fromAxisAngleSequence(inputSeq);
829 
830                 // act
831                 final AxisAngleSequence resultSeq = inputQuat.toAxisAngleSequence(frame, axes);
832                 final QuaternionRotation resultQuat = QuaternionRotation.fromAxisAngleSequence(resultSeq);
833 
834                 // assert
835                 Assertions.assertEquals(frame, resultSeq.getReferenceFrame());
836                 Assertions.assertEquals(axes, resultSeq.getAxisSequence());
837 
838                 assertRadiansEquals(singularityAngle, resultSeq.getAngle2());
839                 assertRadiansEquals(0.0, resultSeq.getAngle3());
840 
841                 checkQuaternion(resultQuat, inputQuat.getQuaternion().getW(), inputQuat.getQuaternion().getX(), inputQuat.getQuaternion().getY(), inputQuat.getQuaternion().getZ());
842             }
843         }
844     }
845 
846     @Test
847     void testAxisAngleSequenceConversion_absolute_eulerSingularities() {
848         // arrange
849         final double[] eulerSingularities = {
850             0.0,
851             Math.PI
852         };
853 
854         final double angle1 = 0.1;
855         final double angle2 = 0.3;
856 
857         final AxisReferenceFrame frame = AxisReferenceFrame.ABSOLUTE;
858 
859         for (final AxisSequence axes : getAxes(AxisSequenceType.EULER)) {
860             for (final double singularityAngle : eulerSingularities) {
861 
862                 final AxisAngleSequence inputSeq = new AxisAngleSequence(frame, axes, angle1, singularityAngle, angle2);
863                 final QuaternionRotation inputQuat = QuaternionRotation.fromAxisAngleSequence(inputSeq);
864 
865                 // act
866                 final AxisAngleSequence resultSeq = inputQuat.toAxisAngleSequence(frame, axes);
867                 final QuaternionRotation resultQuat = QuaternionRotation.fromAxisAngleSequence(resultSeq);
868 
869                 // assert
870                 Assertions.assertEquals(frame, resultSeq.getReferenceFrame());
871                 Assertions.assertEquals(axes, resultSeq.getAxisSequence());
872 
873                 assertRadiansEquals(0.0, resultSeq.getAngle1());
874                 assertRadiansEquals(singularityAngle, resultSeq.getAngle2());
875 
876                 checkQuaternion(resultQuat, inputQuat.getQuaternion().getW(), inputQuat.getQuaternion().getX(), inputQuat.getQuaternion().getY(), inputQuat.getQuaternion().getZ());
877             }
878         }
879     }
880 
881     @Test
882     void testAxisAngleSequenceConversion_relative_taitBryanSingularities() {
883         // arrange
884         final double[] taitBryanSingularities = {
885             -Angle.PI_OVER_TWO,
886             Angle.PI_OVER_TWO
887         };
888 
889         final double angle1 = 0.1;
890         final double angle2 = 0.3;
891 
892         final AxisReferenceFrame frame = AxisReferenceFrame.RELATIVE;
893 
894         for (final AxisSequence axes : getAxes(AxisSequenceType.TAIT_BRYAN)) {
895             for (final double singularityAngle : taitBryanSingularities) {
896 
897                 final AxisAngleSequence inputSeq = new AxisAngleSequence(frame, axes, angle1, singularityAngle, angle2);
898                 final QuaternionRotation inputQuat = QuaternionRotation.fromAxisAngleSequence(inputSeq);
899 
900                 // act
901                 final AxisAngleSequence resultSeq = inputQuat.toAxisAngleSequence(frame, axes);
902                 final QuaternionRotation resultQuat = QuaternionRotation.fromAxisAngleSequence(resultSeq);
903 
904                 // assert
905                 Assertions.assertEquals(frame, resultSeq.getReferenceFrame());
906                 Assertions.assertEquals(axes, resultSeq.getAxisSequence());
907 
908                 assertRadiansEquals(singularityAngle, resultSeq.getAngle2());
909                 assertRadiansEquals(0.0, resultSeq.getAngle3());
910 
911                 checkQuaternion(resultQuat, inputQuat.getQuaternion().getW(), inputQuat.getQuaternion().getX(), inputQuat.getQuaternion().getY(), inputQuat.getQuaternion().getZ());
912             }
913         }
914     }
915 
916     @Test
917     void testAxisAngleSequenceConversion_absolute_taitBryanSingularities() {
918         // arrange
919         final double[] taitBryanSingularities = {
920             -Angle.PI_OVER_TWO,
921             Angle.PI_OVER_TWO
922         };
923 
924         final double angle1 = 0.1;
925         final double angle2 = 0.3;
926 
927         final AxisReferenceFrame frame = AxisReferenceFrame.ABSOLUTE;
928 
929         for (final AxisSequence axes : getAxes(AxisSequenceType.TAIT_BRYAN)) {
930             for (final double singularityAngle : taitBryanSingularities) {
931 
932                 final AxisAngleSequence inputSeq = new AxisAngleSequence(frame, axes, angle1, singularityAngle, angle2);
933                 final QuaternionRotation inputQuat = QuaternionRotation.fromAxisAngleSequence(inputSeq);
934 
935                 // act
936                 final AxisAngleSequence resultSeq = inputQuat.toAxisAngleSequence(frame, axes);
937                 final QuaternionRotation resultQuat = QuaternionRotation.fromAxisAngleSequence(resultSeq);
938 
939                 // assert
940                 Assertions.assertEquals(frame, resultSeq.getReferenceFrame());
941                 Assertions.assertEquals(axes, resultSeq.getAxisSequence());
942 
943                 assertRadiansEquals(0.0, resultSeq.getAngle1());
944                 assertRadiansEquals(singularityAngle, resultSeq.getAngle2());
945 
946                 checkQuaternion(resultQuat, inputQuat.getQuaternion().getW(), inputQuat.getQuaternion().getX(), inputQuat.getQuaternion().getY(), inputQuat.getQuaternion().getZ());
947             }
948         }
949     }
950 
951     private List<AxisSequence> getAxes(final AxisSequenceType type) {
952         return Stream.of(AxisSequence.values())
953                 .filter(a -> type.equals(a.getType()))
954                 .collect(Collectors.toList());
955     }
956 
957     @Test
958     void testToAxisAngleSequence_invalidArgs() {
959         // arrange
960         final QuaternionRotation q = QuaternionRotation.identity();
961 
962         // act/assert
963         Assertions.assertThrows(IllegalArgumentException.class, () -> q.toAxisAngleSequence(null, AxisSequence.XYZ));
964         Assertions.assertThrows(IllegalArgumentException.class, () -> q.toAxisAngleSequence(AxisReferenceFrame.ABSOLUTE, null));
965     }
966 
967     @Test
968     void testToRelativeAxisAngleSequence() {
969         // arrange
970         final QuaternionRotation q = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, TWO_THIRDS_PI);
971 
972         // act
973         final AxisAngleSequence seq = q.toRelativeAxisAngleSequence(AxisSequence.YZX);
974 
975         // assert
976         Assertions.assertEquals(AxisReferenceFrame.RELATIVE, seq.getReferenceFrame());
977         Assertions.assertEquals(AxisSequence.YZX, seq.getAxisSequence());
978         Assertions.assertEquals(Angle.PI_OVER_TWO, seq.getAngle1(), EPS);
979         Assertions.assertEquals(Angle.PI_OVER_TWO, seq.getAngle2(), EPS);
980         Assertions.assertEquals(0, seq.getAngle3(), EPS);
981     }
982 
983     @Test
984     void testToAbsoluteAxisAngleSequence() {
985         // arrange
986         final QuaternionRotation q = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, TWO_THIRDS_PI);
987 
988         // act
989         final AxisAngleSequence seq = q.toAbsoluteAxisAngleSequence(AxisSequence.YZX);
990 
991         // assert
992         Assertions.assertEquals(AxisReferenceFrame.ABSOLUTE, seq.getReferenceFrame());
993         Assertions.assertEquals(AxisSequence.YZX, seq.getAxisSequence());
994         Assertions.assertEquals(Angle.PI_OVER_TWO, seq.getAngle1(), EPS);
995         Assertions.assertEquals(0, seq.getAngle2(), EPS);
996         Assertions.assertEquals(Angle.PI_OVER_TWO, seq.getAngle3(), EPS);
997     }
998 
999     @Test
1000     void testHashCode() {
1001         // arrange
1002         final double delta = 100 * Precision.EPSILON;
1003         final QuaternionRotation q1 = QuaternionRotation.of(1, 2, 3, 4);
1004         final QuaternionRotation q2 = QuaternionRotation.of(1, 2, 3, 4);
1005 
1006         // act/assert
1007         Assertions.assertEquals(q1.hashCode(), q2.hashCode());
1008 
1009         Assertions.assertNotEquals(q1.hashCode(), QuaternionRotation.of(1 + delta, 2, 3, 4).hashCode());
1010         Assertions.assertNotEquals(q1.hashCode(), QuaternionRotation.of(1, 2 + delta, 3, 4).hashCode());
1011         Assertions.assertNotEquals(q1.hashCode(), QuaternionRotation.of(1, 2, 3 + delta, 4).hashCode());
1012         Assertions.assertNotEquals(q1.hashCode(), QuaternionRotation.of(1, 2, 3, 4 + delta).hashCode());
1013     }
1014 
1015     @Test
1016     void testEquals() {
1017         // arrange
1018         final double delta = 100 * Precision.EPSILON;
1019         final QuaternionRotation q1 = QuaternionRotation.of(1, 2, 3, 4);
1020         final QuaternionRotation q2 = QuaternionRotation.of(1, 2, 3, 4);
1021 
1022         // act/assert
1023         GeometryTestUtils.assertSimpleEqualsCases(q1);
1024         Assertions.assertEquals(q1, q2);
1025 
1026         Assertions.assertNotEquals(q1, QuaternionRotation.of(-1, -2, -3, 4));
1027         Assertions.assertNotEquals(q1, QuaternionRotation.of(1, 2, 3, -4));
1028 
1029         Assertions.assertNotEquals(q1, QuaternionRotation.of(1 + delta, 2, 3, 4));
1030         Assertions.assertNotEquals(q1, QuaternionRotation.of(1, 2 + delta, 3, 4));
1031         Assertions.assertNotEquals(q1, QuaternionRotation.of(1, 2, 3 + delta, 4));
1032         Assertions.assertNotEquals(q1, QuaternionRotation.of(1, 2, 3, 4 + delta));
1033     }
1034 
1035     @Test
1036     void testToString() {
1037         // arrange
1038         final QuaternionRotation q = QuaternionRotation.of(1, 2, 3, 4);
1039         final Quaternion qField = q.getQuaternion();
1040 
1041         // assert
1042         Assertions.assertEquals(qField.toString(), q.toString());
1043     }
1044 
1045     @Test
1046     void testCreateVectorRotation_simple() {
1047         // arrange
1048         final Vector3D u1 = Vector3D.Unit.PLUS_X;
1049         final Vector3D u2 = Vector3D.Unit.PLUS_Y;
1050 
1051         // act
1052         final QuaternionRotation q = QuaternionRotation.createVectorRotation(u1, u2);
1053 
1054         // assert
1055         final double val = Math.sqrt(2) * 0.5;
1056 
1057         checkQuaternion(q, val, 0, 0, val);
1058 
1059         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_Z, q.getAxis(), EPS);
1060         Assertions.assertEquals(Angle.PI_OVER_TWO, q.getAngle(), EPS);
1061 
1062         EuclideanTestUtils.assertCoordinatesEqual(u2, q.apply(u1), EPS);
1063         EuclideanTestUtils.assertCoordinatesEqual(u1, q.inverse().apply(u2), EPS);
1064     }
1065 
1066     @Test
1067     void testCreateVectorRotation_identity() {
1068         // arrange
1069         final Vector3D u1 = Vector3D.of(0, 2, 0);
1070 
1071         // act
1072         final QuaternionRotation q = QuaternionRotation.createVectorRotation(u1, u1);
1073 
1074         // assert
1075         checkQuaternion(q, 1, 0, 0, 0);
1076 
1077         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, q.getAxis(), EPS);
1078         Assertions.assertEquals(0.0, q.getAngle(), EPS);
1079 
1080         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 2, 0), q.apply(u1), EPS);
1081         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 2, 0), q.inverse().apply(u1), EPS);
1082     }
1083 
1084     @Test
1085     void testCreateVectorRotation_parallel() {
1086         // arrange
1087         final Vector3D u1 = Vector3D.of(0, 2, 0);
1088         final Vector3D u2 = Vector3D.of(0, 3, 0);
1089 
1090         // act
1091         final QuaternionRotation q = QuaternionRotation.createVectorRotation(u1, u2);
1092 
1093         // assert
1094         checkQuaternion(q, 1, 0, 0, 0);
1095 
1096         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, q.getAxis(), EPS);
1097         Assertions.assertEquals(0.0, q.getAngle(), EPS);
1098 
1099         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 2, 0), q.apply(u1), EPS);
1100         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 3, 0), q.inverse().apply(u2), EPS);
1101     }
1102 
1103     @Test
1104     void testCreateVectorRotation_antiparallel() {
1105         // arrange
1106         final Vector3D u1 = Vector3D.of(0, 2, 0);
1107         final Vector3D u2 = Vector3D.of(0, -3, 0);
1108 
1109         // act
1110         final QuaternionRotation q = QuaternionRotation.createVectorRotation(u1, u2);
1111 
1112         // assert
1113         final Vector3D axis = q.getAxis();
1114         Assertions.assertEquals(0.0, axis.dot(u1), EPS);
1115         Assertions.assertEquals(0.0, axis.dot(u2), EPS);
1116         Assertions.assertEquals(Math.PI, q.getAngle(), EPS);
1117 
1118         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, -2, 0), q.apply(u1), EPS);
1119         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 3, 0), q.inverse().apply(u2), EPS);
1120     }
1121 
1122     @Test
1123     void testCreateVectorRotation_permute() {
1124         EuclideanTestUtils.permuteSkipZero(-5, 5, 0.1, (x, y, z) -> {
1125             // arrange
1126             final Vector3D u1 = Vector3D.of(x, y, z);
1127             final Vector3D u2 = PLUS_DIAGONAL;
1128 
1129             // act
1130             final QuaternionRotation q = QuaternionRotation.createVectorRotation(u1, u2);
1131 
1132             // assert
1133             Assertions.assertEquals(0.0, q.apply(u1).angle(u2), EPS);
1134             Assertions.assertEquals(0.0, q.inverse().apply(u2).angle(u1), EPS);
1135 
1136             final double angle = q.getAngle();
1137             Assertions.assertTrue(angle >= 0.0);
1138             Assertions.assertTrue(angle <= Math.PI);
1139         });
1140     }
1141 
1142     @Test
1143     void testCreateVectorRotation_invalidArgs() {
1144         // act/assert
1145         Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createVectorRotation(Vector3D.ZERO, Vector3D.Unit.PLUS_X));
1146         Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createVectorRotation(Vector3D.Unit.PLUS_X, Vector3D.ZERO));
1147         Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createVectorRotation(Vector3D.NaN, Vector3D.Unit.PLUS_X));
1148         Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createVectorRotation(Vector3D.Unit.PLUS_X, Vector3D.POSITIVE_INFINITY));
1149         Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createVectorRotation(Vector3D.Unit.PLUS_X, Vector3D.NEGATIVE_INFINITY));
1150     }
1151 
1152     @Test
1153     void testCreateBasisRotation_simple() {
1154         // arrange
1155         final Vector3D u1 = Vector3D.Unit.PLUS_X;
1156         final Vector3D u2 = Vector3D.Unit.PLUS_Y;
1157 
1158         final Vector3D v1 = Vector3D.Unit.PLUS_Y;
1159         final Vector3D v2 = Vector3D.Unit.MINUS_X;
1160 
1161         // act
1162         final QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, v1, v2);
1163 
1164         // assert
1165         final QuaternionRotation qInv = q.inverse();
1166 
1167         EuclideanTestUtils.assertCoordinatesEqual(v1, q.apply(u1), EPS);
1168         EuclideanTestUtils.assertCoordinatesEqual(v2, q.apply(u2), EPS);
1169 
1170         EuclideanTestUtils.assertCoordinatesEqual(u1, qInv.apply(v1), EPS);
1171         EuclideanTestUtils.assertCoordinatesEqual(u2, qInv.apply(v2), EPS);
1172 
1173         assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, q);
1174     }
1175 
1176     @Test
1177     void testCreateBasisRotation_diagonalAxis() {
1178         // arrange
1179         final Vector3D u1 = Vector3D.Unit.PLUS_X;
1180         final Vector3D u2 = Vector3D.Unit.PLUS_Y;
1181 
1182         final Vector3D v1 = Vector3D.Unit.PLUS_Y;
1183         final Vector3D v2 = Vector3D.Unit.PLUS_Z;
1184 
1185         // act
1186         final QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, v1, v2);
1187 
1188         // assert
1189         final QuaternionRotation qInv = q.inverse();
1190 
1191         EuclideanTestUtils.assertCoordinatesEqual(v1, q.apply(u1), EPS);
1192         EuclideanTestUtils.assertCoordinatesEqual(v2, q.apply(u2), EPS);
1193 
1194         EuclideanTestUtils.assertCoordinatesEqual(u1, qInv.apply(v1), EPS);
1195         EuclideanTestUtils.assertCoordinatesEqual(u2, qInv.apply(v2), EPS);
1196 
1197         assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, q);
1198         assertRotationEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, q.inverse());
1199     }
1200 
1201     @Test
1202     void testCreateBasisRotation_identity() {
1203         // arrange
1204         final Vector3D u1 = Vector3D.Unit.PLUS_X;
1205         final Vector3D u2 = Vector3D.Unit.PLUS_Y;
1206 
1207         // act
1208         final QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, u1, u2);
1209 
1210         // assert
1211         final QuaternionRotation qInv = q.inverse();
1212 
1213         EuclideanTestUtils.assertCoordinatesEqual(u1, q.apply(u1), EPS);
1214         EuclideanTestUtils.assertCoordinatesEqual(u2, q.apply(u2), EPS);
1215 
1216         EuclideanTestUtils.assertCoordinatesEqual(u1, qInv.apply(u1), EPS);
1217         EuclideanTestUtils.assertCoordinatesEqual(u2, qInv.apply(u2), EPS);
1218 
1219         assertRotationEquals(StandardRotations.IDENTITY, q);
1220     }
1221 
1222     @Test
1223     void testCreateBasisRotation_equivalentBases() {
1224         // arrange
1225         final Vector3D u1 = Vector3D.of(2, 0, 0);
1226         final Vector3D u2 = Vector3D.of(0, 3, 0);
1227 
1228         final Vector3D v1 = Vector3D.of(4, 0, 0);
1229         final Vector3D v2 = Vector3D.of(0, 5, 0);
1230 
1231         // act
1232         final QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, v1, v2);
1233 
1234         // assert
1235         final QuaternionRotation qInv = q.inverse();
1236 
1237         EuclideanTestUtils.assertCoordinatesEqual(u1, q.apply(u1), EPS);
1238         EuclideanTestUtils.assertCoordinatesEqual(u2, q.apply(u2), EPS);
1239 
1240         EuclideanTestUtils.assertCoordinatesEqual(v1, qInv.apply(v1), EPS);
1241         EuclideanTestUtils.assertCoordinatesEqual(v2, qInv.apply(v2), EPS);
1242 
1243         assertRotationEquals(StandardRotations.IDENTITY, q);
1244     }
1245 
1246     @Test
1247     void testCreateBasisRotation_nonOrthogonalVectors() {
1248         // arrange
1249         final Vector3D u1 = Vector3D.of(2, 0, 0);
1250         final Vector3D u2 = Vector3D.of(1, 0.5, 0);
1251 
1252         final Vector3D v1 = Vector3D.of(0, 1.5, 0);
1253         final Vector3D v2 = Vector3D.of(-1, 1.5, 0);
1254 
1255         // act
1256         final QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, v1, v2);
1257 
1258         // assert
1259         final QuaternionRotation qInv = q.inverse();
1260 
1261         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 2, 0), q.apply(u1), EPS);
1262         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(-0.5, 1, 0), q.apply(u2), EPS);
1263 
1264         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(1.5, 0, 0), qInv.apply(v1), EPS);
1265         EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(1.5, 1, 0), qInv.apply(v2), EPS);
1266 
1267         assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, q);
1268     }
1269 
1270     @Test
1271     void testCreateBasisRotation_permute() {
1272         // arrange
1273         final Vector3D u1 = Vector3D.of(1, 2, 3);
1274         final Vector3D u2 = Vector3D.of(0, 4, 0);
1275 
1276         final Vector3D u1Dir = u1.normalize();
1277         final Vector3D u2Dir = u1Dir.orthogonal(u2);
1278 
1279         EuclideanTestUtils.permuteSkipZero(-5, 5, 0.2, (x, y, z) -> {
1280             final Vector3D v1 = Vector3D.of(x, y, z);
1281             final Vector3D v2 = v1.orthogonal();
1282 
1283             final Vector3D v1Dir = v1.normalize();
1284             final Vector3D v2Dir = v2.normalize();
1285 
1286             // act
1287             final QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, v1, v2);
1288             final QuaternionRotation qInv = q.inverse();
1289 
1290             // assert
1291             EuclideanTestUtils.assertCoordinatesEqual(v1Dir, q.apply(u1Dir), EPS);
1292             EuclideanTestUtils.assertCoordinatesEqual(v2Dir, q.apply(u2Dir), EPS);
1293 
1294             EuclideanTestUtils.assertCoordinatesEqual(u1Dir, qInv.apply(v1Dir), EPS);
1295             EuclideanTestUtils.assertCoordinatesEqual(u2Dir, qInv.apply(v2Dir), EPS);
1296 
1297             final double angle = q.getAngle();
1298             Assertions.assertTrue(angle >= 0.0);
1299             Assertions.assertTrue(angle <= Math.PI);
1300 
1301             final Vector3D transformedX = q.apply(Vector3D.Unit.PLUS_X);
1302             final Vector3D transformedY = q.apply(Vector3D.Unit.PLUS_Y);
1303             final Vector3D transformedZ = q.apply(Vector3D.Unit.PLUS_Z);
1304 
1305             Assertions.assertEquals(1.0, transformedX.norm(), EPS);
1306             Assertions.assertEquals(1.0, transformedY.norm(), EPS);
1307             Assertions.assertEquals(1.0, transformedZ.norm(), EPS);
1308 
1309             Assertions.assertEquals(0.0, transformedX.dot(transformedY), EPS);
1310             Assertions.assertEquals(0.0, transformedX.dot(transformedZ), EPS);
1311             Assertions.assertEquals(0.0, transformedY.dot(transformedZ), EPS);
1312 
1313             EuclideanTestUtils.assertCoordinatesEqual(transformedZ.normalize(),
1314                     transformedX.normalize().cross(transformedY.normalize()), EPS);
1315 
1316             Assertions.assertEquals(1.0, q.getQuaternion().norm(), EPS);
1317         });
1318     }
1319 
1320     @Test
1321     void testCreateBasisRotation_invalidArgs() {
1322         // act/assert
1323         Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createBasisRotation(
1324                 Vector3D.ZERO, Vector3D.Unit.PLUS_Y, Vector3D.Unit.PLUS_Y, Vector3D.Unit.MINUS_X));
1325         Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createBasisRotation(
1326                 Vector3D.Unit.PLUS_X, Vector3D.NaN, Vector3D.Unit.PLUS_Y, Vector3D.Unit.MINUS_X));
1327         Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createBasisRotation(
1328                 Vector3D.Unit.PLUS_X, Vector3D.Unit.PLUS_Y, Vector3D.POSITIVE_INFINITY, Vector3D.Unit.MINUS_X));
1329         Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createBasisRotation(
1330                 Vector3D.Unit.PLUS_X, Vector3D.Unit.PLUS_Y, Vector3D.Unit.PLUS_Y, Vector3D.NEGATIVE_INFINITY));
1331         Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createBasisRotation(
1332                 Vector3D.Unit.PLUS_X, Vector3D.Unit.PLUS_X, Vector3D.Unit.PLUS_Y, Vector3D.Unit.MINUS_X));
1333         Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createBasisRotation(
1334                 Vector3D.Unit.PLUS_X, Vector3D.Unit.PLUS_Y, Vector3D.Unit.PLUS_Y, Vector3D.Unit.MINUS_Y));
1335     }
1336 
1337     @Test
1338     void testFromEulerAngles_identity() {
1339         for (final AxisSequence axes : AxisSequence.values()) {
1340 
1341             // act/assert
1342             assertRotationEquals(StandardRotations.IDENTITY,
1343                     QuaternionRotation.fromAxisAngleSequence(AxisAngleSequence.createRelative(axes, 0, 0, 0)));
1344             assertRotationEquals(StandardRotations.IDENTITY,
1345                     QuaternionRotation.fromAxisAngleSequence(AxisAngleSequence.createRelative(axes, Angle.TWO_PI, Angle.TWO_PI, Angle.TWO_PI)));
1346 
1347             assertRotationEquals(StandardRotations.IDENTITY,
1348                     QuaternionRotation.fromAxisAngleSequence(AxisAngleSequence.createAbsolute(axes, 0, 0, 0)));
1349             assertRotationEquals(StandardRotations.IDENTITY,
1350                     QuaternionRotation.fromAxisAngleSequence(AxisAngleSequence.createAbsolute(axes, Angle.TWO_PI, Angle.TWO_PI, Angle.TWO_PI)));
1351         }
1352     }
1353 
1354     @Test
1355     void testFromEulerAngles_relative() {
1356 
1357         // --- act/assert
1358 
1359         // XYZ
1360         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.XYZ, Angle.PI_OVER_TWO, 0, 0);
1361         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.XYZ, 0, Angle.PI_OVER_TWO, 0);
1362         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.XYZ, 0, 0, Angle.PI_OVER_TWO);
1363         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.XYZ, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1364 
1365         // XZY
1366         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.XZY, Angle.PI_OVER_TWO, 0, 0);
1367         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.XZY, 0, 0, Angle.PI_OVER_TWO);
1368         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.XZY, 0, Angle.PI_OVER_TWO, 0);
1369         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.XZY, Angle.PI_OVER_TWO, 0, Angle.PI_OVER_TWO);
1370 
1371         // YXZ
1372         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YXZ, 0, Angle.PI_OVER_TWO, 0);
1373         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YXZ, Angle.PI_OVER_TWO, 0, 0);
1374         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YXZ, 0, 0, Angle.PI_OVER_TWO);
1375         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YXZ, Angle.PI_OVER_TWO, 0, Angle.PI_OVER_TWO);
1376 
1377         // YZX
1378         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YZX, 0, 0, Angle.PI_OVER_TWO);
1379         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YZX, Angle.PI_OVER_TWO, 0, 0);
1380         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YZX, 0, Angle.PI_OVER_TWO, 0);
1381         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YZX, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1382 
1383         // ZXY
1384         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YZX, 0, 0, Angle.PI_OVER_TWO);
1385         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YZX, Angle.PI_OVER_TWO, 0, 0);
1386         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YZX, 0, Angle.PI_OVER_TWO, 0);
1387         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YZX, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1388 
1389         // ZYX
1390         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.ZYX, 0, 0, Angle.PI_OVER_TWO);
1391         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.ZYX, 0, Angle.PI_OVER_TWO, 0);
1392         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.ZYX, Angle.PI_OVER_TWO, 0, 0);
1393         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.ZYX, Angle.PI_OVER_TWO, 0, Angle.PI_OVER_TWO);
1394 
1395         // XYX
1396         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.XYX, Angle.PI_OVER_TWO, 0, 0);
1397         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.XYX, 0, Angle.PI_OVER_TWO, 0);
1398         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.XYX, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO);
1399         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.XYX, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1400 
1401         // XZX
1402         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.XZX, Angle.PI_OVER_TWO, 0, 0);
1403         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.XZX, -Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1404         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.XZX, 0, Angle.PI_OVER_TWO, 0);
1405         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.XZX, 0, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1406 
1407         // YXY
1408         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YXY, 0, Angle.PI_OVER_TWO, 0);
1409         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YXY, Angle.PI_OVER_TWO, 0, 0);
1410         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YXY, -Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1411         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YXY, 0, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1412 
1413         // YZY
1414         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YZY, -Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1415         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YZY, Angle.PI_OVER_TWO, 0, 0);
1416         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YZY, 0, Angle.PI_OVER_TWO, 0);
1417         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YZY, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1418 
1419         // ZXZ
1420         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.ZXZ, 0, Angle.PI_OVER_TWO, 0);
1421         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.ZXZ, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO);
1422         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.ZXZ, Angle.PI_OVER_TWO, 0, 0);
1423         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.ZXZ, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1424 
1425         // ZYZ
1426         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.ZYZ, Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO);
1427         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.ZYZ, 0, Angle.PI_OVER_TWO, 0);
1428         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.ZYZ, Angle.PI_OVER_TWO, 0, 0);
1429         checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.ZYZ, 0, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1430     }
1431 
1432     /** Helper method for verifying that a relative euler angles instance constructed with the given arguments
1433      * is correctly converted to a QuaternionRotation that matches the given operator.
1434      * @param rotation
1435      * @param axes
1436      * @param angle1
1437      * @param angle2
1438      * @param angle3
1439      */
1440     private void checkFromAxisAngleSequenceRelative(final UnaryOperator<Vector3D> rotation, final AxisSequence axes, final double angle1, final double angle2, final double angle3) {
1441         final AxisAngleSequence angles = AxisAngleSequence.createRelative(axes, angle1, angle2, angle3);
1442 
1443         assertRotationEquals(rotation, QuaternionRotation.fromAxisAngleSequence(angles));
1444     }
1445 
1446     @Test
1447     void testFromEulerAngles_absolute() {
1448 
1449         // --- act/assert
1450 
1451         // XYZ
1452         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.XYZ, Angle.PI_OVER_TWO, 0, 0);
1453         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.XYZ, 0, Angle.PI_OVER_TWO, 0);
1454         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.XYZ, 0, 0, Angle.PI_OVER_TWO);
1455         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.XYZ, Angle.PI_OVER_TWO, 0, Angle.PI_OVER_TWO);
1456 
1457         // XZY
1458         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.XZY, Angle.PI_OVER_TWO, 0, 0);
1459         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.XZY, 0, 0, Angle.PI_OVER_TWO);
1460         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.XZY, 0, Angle.PI_OVER_TWO, 0);
1461         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.XZY, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1462 
1463         // YXZ
1464         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YXZ, 0, Angle.PI_OVER_TWO, 0);
1465         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YXZ, Angle.PI_OVER_TWO, 0, 0);
1466         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YXZ, 0, 0, Angle.PI_OVER_TWO);
1467         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YXZ, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1468 
1469         // YZX
1470         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YZX, 0, 0, Angle.PI_OVER_TWO);
1471         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YZX, Angle.PI_OVER_TWO, 0, 0);
1472         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YZX, 0, Angle.PI_OVER_TWO, 0);
1473         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YZX, Angle.PI_OVER_TWO, 0, Angle.PI_OVER_TWO);
1474 
1475         // ZXY
1476         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YZX, 0, 0, Angle.PI_OVER_TWO);
1477         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YZX, Angle.PI_OVER_TWO, 0, 0);
1478         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YZX, 0, Angle.PI_OVER_TWO, 0);
1479         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YZX, Angle.PI_OVER_TWO, 0, Angle.PI_OVER_TWO);
1480 
1481         // ZYX
1482         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.ZYX, 0, 0, Angle.PI_OVER_TWO);
1483         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.ZYX, 0, Angle.PI_OVER_TWO, 0);
1484         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.ZYX, Angle.PI_OVER_TWO, 0, 0);
1485         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.ZYX, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1486 
1487         // XYX
1488         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.XYX, Angle.PI_OVER_TWO, 0, 0);
1489         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.XYX, 0, Angle.PI_OVER_TWO, 0);
1490         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.XYX, Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO);
1491         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.XYX, 0, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1492 
1493         // XZX
1494         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.XZX, Angle.PI_OVER_TWO, 0, 0);
1495         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.XZX, -Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1496         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.XZX, 0, Angle.PI_OVER_TWO, 0);
1497         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.XZX, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1498 
1499         // YXY
1500         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YXY, 0, Angle.PI_OVER_TWO, 0);
1501         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YXY, Angle.PI_OVER_TWO, 0, 0);
1502         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YXY, -Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1503         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YXY, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1504 
1505         // YZY
1506         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YZY, -Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1507         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YZY, Angle.PI_OVER_TWO, 0, 0);
1508         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YZY, 0, Angle.PI_OVER_TWO, 0);
1509         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YZY, 0, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1510 
1511         // ZXZ
1512         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.ZXZ, 0, Angle.PI_OVER_TWO, 0);
1513         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.ZXZ, -Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1514         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.ZXZ, Angle.PI_OVER_TWO, 0, 0);
1515         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.ZXZ, 0, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1516 
1517         // ZYZ
1518         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.ZYZ, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO);
1519         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.ZYZ, 0, Angle.PI_OVER_TWO, 0);
1520         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.ZYZ, Angle.PI_OVER_TWO, 0, 0);
1521         checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.ZYZ, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1522     }
1523 
1524     /** Helper method for verifying that an absolute euler angles instance constructed with the given arguments
1525      * is correctly converted to a QuaternionRotation that matches the given operator.
1526      * @param rotation
1527      * @param axes
1528      * @param angle1
1529      * @param angle2
1530      * @param angle3
1531      */
1532     private void checkFromAxisAngleSequenceAbsolute(final UnaryOperator<Vector3D> rotation, final AxisSequence axes, final double angle1, final double angle2, final double angle3) {
1533         final AxisAngleSequence angles = AxisAngleSequence.createAbsolute(axes, angle1, angle2, angle3);
1534 
1535         assertRotationEquals(rotation, QuaternionRotation.fromAxisAngleSequence(angles));
1536     }
1537 
1538     private static void checkQuaternion(final QuaternionRotation qrot, final double w, final double x, final double y, final double z) {
1539         final String msg = "Expected" +
1540                 " quaternion to equal " + SimpleTupleFormat.getDefault().format(w, x, y, z) + " but was " + qrot;
1541 
1542         Assertions.assertEquals(w, qrot.getQuaternion().getW(), EPS, msg);
1543         Assertions.assertEquals(x, qrot.getQuaternion().getX(), EPS, msg);
1544         Assertions.assertEquals(y, qrot.getQuaternion().getY(), EPS, msg);
1545         Assertions.assertEquals(z, qrot.getQuaternion().getZ(), EPS, msg);
1546 
1547         final Quaternion q = qrot.getQuaternion();
1548         Assertions.assertEquals(w, q.getW(), EPS, msg);
1549         Assertions.assertEquals(x, q.getX(), EPS, msg);
1550         Assertions.assertEquals(y, q.getY(), EPS, msg);
1551         Assertions.assertEquals(z, q.getZ(), EPS, msg);
1552 
1553         Assertions.assertTrue(qrot.preservesOrientation());
1554     }
1555 
1556     private static void checkVector(final Vector3D v, final double x, final double y, final double z) {
1557         final String msg = "Expected vector to equal " + SimpleTupleFormat.getDefault().format(x, y, z) + " but was " + v;
1558 
1559         Assertions.assertEquals(x, v.getX(), EPS, msg);
1560         Assertions.assertEquals(y, v.getY(), EPS, msg);
1561         Assertions.assertEquals(z, v.getZ(), EPS, msg);
1562     }
1563 
1564     /** Assert that the two given radian values are equivalent.
1565      * @param expected
1566      * @param actual
1567      */
1568     private static void assertRadiansEquals(final double expected, final double actual) {
1569         final double diff = Angle.Rad.WITHIN_MINUS_PI_AND_PI.applyAsDouble(expected - actual);
1570         final String msg = "Expected " + actual + " radians to be equivalent to " + expected + " radians; difference is " + diff;
1571 
1572         Assertions.assertTrue(Math.abs(diff) < 1e-6, msg);
1573     }
1574 
1575     /**
1576      * Assert that {@code rotation} returns the same outputs as {@code expected} for a range of vector inputs.
1577      * @param expected
1578      * @param rotation
1579      */
1580     private static void assertRotationEquals(final UnaryOperator<Vector3D> expected, final QuaternionRotation rotation) {
1581         assertFnEquals(expected, rotation);
1582     }
1583 
1584     /**
1585      * Assert that {@code transform} returns the same outputs as {@code expected} for a range of vector inputs.
1586      * @param expected
1587      * @param transform
1588      */
1589     private static void assertTransformEquals(final UnaryOperator<Vector3D> expected, final AffineTransformMatrix3D transform) {
1590         assertFnEquals(expected, transform);
1591     }
1592 
1593     /**
1594      * Assert that {@code actual} returns the same output as {@code expected} for a range of inputs.
1595      * @param expectedFn
1596      * @param actualFn
1597      */
1598     private static void assertFnEquals(final UnaryOperator<Vector3D> expectedFn, final UnaryOperator<Vector3D> actualFn) {
1599         EuclideanTestUtils.permute(-2, 2, 0.25, (x, y, z) -> {
1600             final Vector3D input = Vector3D.of(x, y, z);
1601 
1602             final Vector3D expected = expectedFn.apply(input);
1603             final Vector3D actual = actualFn.apply(input);
1604 
1605             final String msg = "Expected vector " + input + " to be transformed to " + expected + " but was " + actual;
1606 
1607             Assertions.assertEquals(expected.getX(), actual.getX(), EPS, msg);
1608             Assertions.assertEquals(expected.getY(), actual.getY(), EPS, msg);
1609             Assertions.assertEquals(expected.getZ(), actual.getZ(), EPS, msg);
1610         });
1611     }
1612 }