View Javadoc

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