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