View Javadoc

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