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.Objects;
    20  import java.util.function.DoubleFunction;
    21
    22  import org.apache.commons.geometry.core.internal.GeometryInternalError;
    23  import org.apache.commons.geometry.euclidean.internal.Vectors;
    24  import org.apache.commons.geometry.euclidean.threed.AffineTransformMatrix3D;
    25  import org.apache.commons.geometry.euclidean.threed.Vector3D;
    26  import org.apache.commons.numbers.angle.Angle;
    27  import org.apache.commons.numbers.quaternion.Quaternion;
    28  import org.apache.commons.numbers.quaternion.Slerp;
    29
    30  /**
    31   * Class using a unit-length quaternion to represent
    32   * <a href="https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation">rotations</a>
    33   * in 3-dimensional Euclidean space.
    34   * The underlying quaternion is in <em>positive polar form</em>: It is normalized and has a
    35   * non-negative scalar component ({@code w}).
    36   *
    37   * @see Quaternion
    38   */
    39  public final class QuaternionRotation implements Rotation3D {
    40      /** Threshold value for the dot product of antiparallel vectors. If the dot product of two vectors is
    41       * less than this value, (adjusted for the lengths of the vectors), then the vectors are considered to be
    42       * antiparallel (ie, negations of each other).
    43       */
    44      private static final double ANTIPARALLEL_DOT_THRESHOLD = 2.0e-15 - 1.0;
    45
    46      /** Threshold value used to identify singularities when converting from quaternions to
    47       * axis angle sequences.
    48       */
    49      private static final double AXIS_ANGLE_SINGULARITY_THRESHOLD = 0.9999999999;
    50
    51      /** Instance used to represent the identity rotation, ie a rotation with
    52       * an angle of zero.
    53       */
    54      private static final QuaternionRotation IDENTITY_INSTANCE = of(Quaternion.ONE);
    55
    56      /** Unit-length quaternion instance in positive polar form. */
    57      private final Quaternion quat;
    58
    59      /** Simple constructor. The given quaternion is converted to positive polar form.
    60       * @param quat quaternion instance
    61       * @throws IllegalStateException if the the norm of the given components is zero,
    62       *                              NaN, or infinite
    63       */
    64      private QuaternionRotation(final Quaternion quat) {
    65          this.quat = quat.positivePolarForm();
    66      }
    67
    68      /** Get the underlying quaternion instance.
    69       * @return the quaternion instance
    70       */
    71      public Quaternion getQuaternion() {
    72          return quat;
    73      }
    74
    75      /**
    76       * Get the axis of rotation as a normalized {@link Vector3D}. The rotation axis
    77       * is not well defined when the rotation is the identity rotation, ie it has a
    78       * rotation angle of zero. In this case, the vector representing the positive
    79       * x-axis is returned.
    80       *
    81       * @return the axis of rotation
    82       */
    83      @Override
    84      public Vector3D getAxis() {
    85          final Vector3D axis = Vector3D.of(quat.getX(), quat.getY(), quat.getZ())
    86                  .normalizeOrNull();
    87          return axis != null ?
    88                  axis :
    89                  Vector3D.Unit.PLUS_X;
    90      }
    91
    92      /**
    93       * Get the angle of rotation in radians. The returned value is in the range 0
    94       * through {@code pi}.
    95       *
    96       * @return The rotation angle in the range {@code [0, pi]}.
    97       */
    98      @Override
    99      public double getAngle() {
    100         return 2 * Math.acos(quat.getW());
    101     }
    102
    103     /**
    104      * Get the inverse of this rotation. The returned rotation has the same
    105      * rotation angle but the opposite rotation axis. If {@code r.apply(u)}
    106      * is equal to {@code v}, then {@code r.negate().apply(v)} is equal
    107      * to {@code u}.
    108      *
    109      * @return the negation (inverse) of the rotation
    110      */
    111     @Override
    112     public QuaternionRotation inverse() {
    113         return new QuaternionRotation(quat.conjugate());
    114     }
    115
    116     /**
    117      * Apply this rotation to the given vector.
    118      *
    119      * @param v vector to rotate
    120      * @return the rotated vector
    121      */
    122     @Override
    123     public Vector3D apply(final Vector3D v) {
    124         final double qw = quat.getW();
    125         final double qx = quat.getX();
    126         final double qy = quat.getY();
    127         final double qz = quat.getZ();
    128
    129         final double x = v.getX();
    130         final double y = v.getY();
    131         final double z = v.getZ();
    132
    133         // calculate the Hamilton product of the quaternion and vector
    134         final double iw = -(qx * x) - (qy * y) - (qz * z);
    135         final double ix = (qw * x) + (qy * z) - (qz * y);
    136         final double iy = (qw * y) + (qz * x) - (qx * z);
    137         final double iz = (qw * z) + (qx * y) - (qy * x);
    138
    139         // calculate the Hamilton product of the intermediate vector and
    140         // the inverse quaternion
    141
    142         return Vector3D.of(
    143                     (iw * -qx) + (ix * qw) + (iy * -qz) - (iz * -qy),
    144                     (iw * -qy) - (ix * -qz) + (iy * qw) + (iz * -qx),
    145                     (iw * -qz) + (ix * -qy) - (iy * -qx) + (iz * qw)
    146                 );
    147     }
    148
    149     /** {@inheritDoc}
    150      *
    151      * <p>This method simply calls {@code apply(vec)} since rotations treat
    152      * points and vectors similarly.</p>
    153      */
    154     @Override
    155     public Vector3D applyVector(final Vector3D vec) {
    156         return apply(vec);
    157     }
    158
    159     /** {@inheritDoc}
    160      *
    161      * <p>This method simply returns true since rotations always preserve the orientation
    162      * of the space.</p>
    163      */
    164     @Override
    165     public boolean preservesOrientation() {
    166         return true;
    167     }
    168
    169     /** Return an {@link AffineTransformMatrix3D} representing the same rotation as this
    170      * instance.
    171      * @return a transform matrix representing the same rotation as this instance
    172      */
    173     public AffineTransformMatrix3D toMatrix() {
    174
    175         final double qw = quat.getW();
    176         final double qx = quat.getX();
    177         final double qy = quat.getY();
    178         final double qz = quat.getZ();
    179
    180         // pre-calculate products that we'll need
    181         final double xx = qx * qx;
    182         final double xy = qx * qy;
    183         final double xz = qx * qz;
    184         final double xw = qx * qw;
    185
    186         final double yy = qy * qy;
    187         final double yz = qy * qz;
    188         final double yw = qy * qw;
    189
    190         final double zz = qz * qz;
    191         final double zw = qz * qw;
    192
    193         final double m00 = 1.0 - (2.0 * (yy + zz));
    194         final double m01 = 2.0 * (xy - zw);
    195         final double m02 = 2.0 * (xz + yw);
    196         final double m03 = 0.0;
    197
    198         final double m10 = 2.0 * (xy + zw);
    199         final double m11 = 1.0 - (2.0 * (xx + zz));
    200         final double m12 = 2.0 * (yz - xw);
    201         final double m13 = 0.0;
    202
    203         final double m20 = 2.0 * (xz - yw);
    204         final double m21 = 2.0 * (yz + xw);
    205         final double m22 = 1.0 - (2.0 * (xx + yy));
    206         final double m23 = 0.0;
    207
    208         return AffineTransformMatrix3D.of(
    209                     m00, m01, m02, m03,
    210                     m10, m11, m12, m13,
    211                     m20, m21, m22, m23
    212                 );
    213     }
    214
    215     /**
    216      * Multiply this instance by the given argument, returning the result as
    217      * a new instance. This is equivalent to the expression {@code t * q} where
    218      * {@code q} is the argument and {@code t} is this instance.
    219      *
    220      * <p>
    221      * Multiplication of quaternions behaves similarly to transformation
    222      * matrices in regard to the order that operations are performed.
    223      * For example, if <code>q<sub>1</sub></code> and <code>q<sub>2</sub></code> are unit
    224      * quaternions, then the quaternion <code>q<sub>r</sub> = q<sub>1</sub>*q<sub>2</sub></code>
    225      * will give the effect of applying the rotation in <code>q<sub>2</sub></code> followed
    226      * by the rotation in <code>q<sub>1</sub></code>. In other words, the rightmost element
    227      * in the multiplication is applied first.
    228      * </p>
    229      *
    230      * @param q quaternion to multiply with the current instance
    231      * @return the result of multiplying this quaternion by the argument
    232      */
    233     public QuaternionRotation multiply(final QuaternionRotation q) {
    234         final Quaternion product = quat.multiply(q.quat);
    235         return new QuaternionRotation(product);
    236     }
    237
    238     /** Multiply the argument by this instance, returning the result as
    239      * a new instance. This is equivalent to the expression {@code q * t} where
    240      * {@code q} is the argument and {@code t} is this instance.
    241      *
    242      * <p>
    243      * Multiplication of quaternions behaves similarly to transformation
    244      * matrices in regard to the order that operations are performed.
    245      * For example, if <code>q<sub>1</sub></code> and <code>q<sub>2</sub></code> are unit
    246      * quaternions, then the quaternion <code>q<sub>r</sub> = q<sub>1</sub>*q<sub>2</sub></code>
    247      * will give the effect of applying the rotation in <code>q<sub>2</sub></code> followed
    248      * by the rotation in <code>q<sub>1</sub></code>. In other words, the rightmost element
    249      * in the multiplication is applied first.
    250      * </p>
    251      *
    252      * @param q quaternion to multiply by the current instance
    253      * @return the result of multiplying the argument by the current instance
    254      */
    255     public QuaternionRotation premultiply(final QuaternionRotation q) {
    256         return q.multiply(this);
    257     }
    258
    259     /**
    260      * Creates a function that performs a
    261      * <a href="https://en.wikipedia.org/wiki/Slerp">spherical
    262      * linear interpolation</a> between this instance and the argument.
    263      * <p>
    264      * The argument to the function returned by this method is the
    265      * interpolation parameter {@code t}.
    266      * If {@code t = 0}, the rotation is equal to this instance.
    267      * If {@code t = 1}, the rotation is equal to the {@code end} instance.
    268      * All other values are interpolated (or extrapolated if {@code t} is
    269      * outside of the {@code [0, 1]} range).
    270      *
    271      * @param end end value of the interpolation
    272      * @return a function that interpolates between this instance and the
    273      * argument.
    274      *
    275      * @see org.apache.commons.numbers.quaternion.Slerp
    276      */
    277     public DoubleFunction<QuaternionRotation> slerp(final QuaternionRotation end) {
    278         final Slerp s = new Slerp(getQuaternion(), end.getQuaternion());
    279         return t -> QuaternionRotation.of(s.apply(t));
    280     }
    281
    282     /** Get a sequence of axis-angle rotations that produce an overall rotation equivalent to this instance.
    283      *
    284      * <p>
    285      * In most cases, the returned rotation sequence will be unique. However, at points of singularity
    286      * (second angle equal to {@code 0} or {@code -pi} for Euler angles and {@code +pi/2} or {@code -pi/2}
    287      * for Tait-Bryan angles), there are an infinite number of possible sequences that produce the same result.
    288      * In these cases, the result is returned that leaves the last rotation equal to 0 (in the case of a relative
    289      * reference frame) or the first rotation equal to 0 (in the case of an absolute reference frame).
    290      * </p>
    291      *
    292      * @param frame the reference frame used to interpret the positions of the rotation axes
    293      * @param axes the sequence of rotation axes
    294      * @return a sequence of axis-angle rotations equivalent to this rotation
    295      */
    296     public AxisAngleSequence toAxisAngleSequence(final AxisReferenceFrame frame, final AxisSequence axes) {
    297         if (frame == null) {
    298             throw new IllegalArgumentException("Axis reference frame cannot be null");
    299         }
    300         if (axes == null) {
    301             throw new IllegalArgumentException("Axis sequence cannot be null");
    302         }
    303
    304         final double[] angles = getAngles(frame, axes);
    305
    306         return new AxisAngleSequence(frame, axes, angles[0], angles[1], angles[2]);
    307     }
    308
    309     /** Get a sequence of axis-angle rotations that produce an overall rotation equivalent to this instance.
    310      * Each rotation axis is interpreted relative to the rotated coordinate frame (ie, intrinsic rotation).
    311      * @param axes the sequence of rotation axes
    312      * @return a sequence of relative axis-angle rotations equivalent to this rotation
    313      * @see #toAxisAngleSequence(AxisReferenceFrame, AxisSequence)
    314      */
    315     public AxisAngleSequence toRelativeAxisAngleSequence(final AxisSequence axes) {
    316         return toAxisAngleSequence(AxisReferenceFrame.RELATIVE, axes);
    317     }
    318
    319     /** Get a sequence of axis-angle rotations that produce an overall rotation equivalent to this instance.
    320      * Each rotation axis is interpreted as part of an absolute, unmoving coordinate frame (ie, extrinsic rotation).
    321      * @param axes the sequence of rotation axes
    322      * @return a sequence of absolute axis-angle rotations equivalent to this rotation
    323      * @see #toAxisAngleSequence(AxisReferenceFrame, AxisSequence)
    324      */
    325     public AxisAngleSequence toAbsoluteAxisAngleSequence(final AxisSequence axes) {
    326         return toAxisAngleSequence(AxisReferenceFrame.ABSOLUTE, axes);
    327     }
    328
    329     /** {@inheritDoc} */
    330     @Override
    331     public int hashCode() {
    332         return quat.hashCode();
    333     }
    334
    335     /** {@inheritDoc} */
    336     @Override
    337     public boolean equals(final Object obj) {
    338         if (this == obj) {
    339             return true;
    340         }
    341         if (!(obj instanceof QuaternionRotation)) {
    342             return false;
    343         }
    344
    345         final QuaternionRotation other = (QuaternionRotation) obj;
    346         return Objects.equals(this.quat, other.quat);
    347     }
    348
    349     /** {@inheritDoc} */
    350     @Override
    351     public String toString() {
    352         return quat.toString();
    353     }
    354
    355     /** Get a sequence of angles around the given axes that produce a rotation equivalent
    356      * to this instance.
    357      * @param frame the reference frame used to define the positions of the axes
    358      * @param axes the axis sequence
    359      * @return a sequence of angles around the given axes that produce a rotation equivalent
    360      *      to this instance
    361      */
    362     private double[] getAngles(final AxisReferenceFrame frame, final AxisSequence axes) {
    363
    364         final AxisSequenceType sequenceType = axes.getType();
    365
    366         final Vector3D axis1 = axes.getAxis1();
    367         final Vector3D axis2 = axes.getAxis2();
    368         final Vector3D axis3 = axes.getAxis3();
    369
    370         if (frame == AxisReferenceFrame.RELATIVE) {
    371             if (sequenceType == AxisSequenceType.TAIT_BRYAN) {
    372                 return getRelativeTaitBryanAngles(axis1, axis2, axis3);
    373             } else if (sequenceType == AxisSequenceType.EULER) {
    374                 return getRelativeEulerAngles(axis1, axis2);
    375             }
    376         } else if (frame == AxisReferenceFrame.ABSOLUTE) {
    377             if (sequenceType == AxisSequenceType.TAIT_BRYAN) {
    378                 return getAbsoluteTaitBryanAngles(axis1, axis2, axis3);
    379             } else if (sequenceType == AxisSequenceType.EULER) {
    380                 return getAbsoluteEulerAngles(axis1, axis2);
    381             }
    382         }
    383
    384         // all possibilities should have been covered above
    385         throw new GeometryInternalError();
    386     }
    387
    388     /** Get a sequence of angles around the given Tait-Bryan axes that produce a rotation equivalent
    389      * to this instance. The axes are interpreted as being relative to the rotated coordinate frame.
    390      * @param axis1 first Tait-Bryan axis
    391      * @param axis2 second Tait-Bryan axis
    392      * @param axis3 third Tait-Bryan axis
    393      * @return a sequence of rotation angles around the relative input axes that produce a rotation equivalent
    394      *      to this instance
    395      */
    396     private double[] getRelativeTaitBryanAngles(final Vector3D axis1, final Vector3D axis2, final Vector3D axis3) {
    397
    398         // We can use geometry to get the first and second angles pretty easily here by analyzing the positions
    399         // of the transformed rotation axes. The third angle is trickier but we can get it by treating it as
    400         // if it were the first rotation in the inverse (which it would be).
    401
    402         final Vector3D vec3 = apply(axis3);
    403         final Vector3D invVec1 = inverse().apply(axis1);
    404
    405         final double angle2Sin = vec3.dot(axis2.cross(axis3));
    406
    407         if (angle2Sin < -AXIS_ANGLE_SINGULARITY_THRESHOLD ||
    408                 angle2Sin > AXIS_ANGLE_SINGULARITY_THRESHOLD) {
    409
    410             final Vector3D vec2 = apply(axis2);
    411
    412             final double angle1TanY = vec2.dot(axis1.cross(axis2));
    413             final double angle1TanX = vec2.dot(axis2);
    414
    415             final double angle2 = angle2Sin > AXIS_ANGLE_SINGULARITY_THRESHOLD ?
    416                     Angle.PI_OVER_TWO :
    417                     -Angle.PI_OVER_TWO;
    418
    419             return new double[] {
    420                 Math.atan2(angle1TanY, angle1TanX),
    421                 angle2,
    422                 0.0
    423             };
    424         }
    425
    426         final Vector3D  crossAxis13 = axis1.cross(axis3);
    427
    428         final double angle1TanY = vec3.dot(crossAxis13);
    429         final double angle1TanX = vec3.dot(axis3);
    430
    431         final double angle3TanY = invVec1.dot(crossAxis13);
    432         final double angle3TanX = invVec1.dot(axis1);
    433
    434         return new double[] {
    435             Math.atan2(angle1TanY, angle1TanX),
    436             Math.asin(angle2Sin),
    437             Math.atan2(angle3TanY, angle3TanX)
    438         };
    439     }
    440
    441     /** Get a sequence of angles around the given Tait-Bryan axes that produce a rotation equivalent
    442      * to this instance. The axes are interpreted as being part of an absolute (unmoving) coordinate frame.
    443      * @param axis1 first Tait-Bryan axis
    444      * @param axis2 second Tait-Bryan axis
    445      * @param axis3 third Tait-Bryan axis
    446      * @return a sequence of rotation angles around the absolute input axes that produce a rotation equivalent
    447      *      to this instance
    448      */
    449     private double[] getAbsoluteTaitBryanAngles(final Vector3D axis1, final Vector3D axis2, final Vector3D axis3) {
    450         // A relative axis-angle rotation sequence is equivalent to an absolute one with the rotation
    451         // sequence reversed, meaning we can reuse our relative logic here.
    452         return reverseArray(getRelativeTaitBryanAngles(axis3, axis2, axis1));
    453     }
    454
    455     /** Get a sequence of angles around the given Euler axes that produce a rotation equivalent
    456      * to this instance. The axes are interpreted as being relative to the rotated coordinate frame. Only
    457      * the first two axes are needed since, by definition, the first Euler angle axis is repeated as the
    458      * third axis.
    459      * @param axis1 first Euler axis
    460      * @param axis2 second Euler axis
    461      * @return a sequence of rotation angles around the relative input axes that produce a rotation equivalent
    462      *      to this instance
    463      */
    464     private double[] getRelativeEulerAngles(final Vector3D axis1, final Vector3D axis2) {
    465
    466         // Use the same overall approach as with the Tait-Bryan angles: get the first two angles by looking
    467         // at the transformed rotation axes and the third by using the inverse.
    468
    469         final Vector3D crossAxis = axis1.cross(axis2);
    470
    471         final Vector3D vec1 = apply(axis1);
    472         final Vector3D invVec1 = inverse().apply(axis1);
    473
    474         final double angle2Cos = vec1.dot(axis1);
    475
    476         if (angle2Cos < -AXIS_ANGLE_SINGULARITY_THRESHOLD ||
    477                 angle2Cos > AXIS_ANGLE_SINGULARITY_THRESHOLD) {
    478
    479             final Vector3D vec2 = apply(axis2);
    480
    481             final double angle1TanY = vec2.dot(crossAxis);
    482             final double angle1TanX = vec2.dot(axis2);
    483
    484             final double angle2 = angle2Cos > AXIS_ANGLE_SINGULARITY_THRESHOLD ? 0.0 : Math.PI;
    485
    486             return new double[] {
    487                 Math.atan2(angle1TanY, angle1TanX),
    488                 angle2,
    489                 0.0
    490             };
    491         }
    492
    493         final double angle1TanY = vec1.dot(axis2);
    494         final double angle1TanX = -vec1.dot(crossAxis);
    495
    496         final double angle3TanY = invVec1.dot(axis2);
    497         final double angle3TanX = invVec1.dot(crossAxis);
    498
    499         return new double[] {
    500             Math.atan2(angle1TanY, angle1TanX),
    501             Math.acos(angle2Cos),
    502             Math.atan2(angle3TanY, angle3TanX)
    503         };
    504     }
    505
    506     /** Get a sequence of angles around the given Euler axes that produce a rotation equivalent
    507      * to this instance. The axes are interpreted as being part of an absolute (unmoving) coordinate frame.
    508      * Only the first two axes are needed since, by definition, the first Euler angle axis is repeated as
    509      * the third axis.
    510      * @param axis1 first Euler axis
    511      * @param axis2 second Euler axis
    512      * @return a sequence of rotation angles around the absolute input axes that produce a rotation equivalent
    513      *      to this instance
    514      */
    515     private double[] getAbsoluteEulerAngles(final Vector3D axis1, final Vector3D axis2) {
    516         // A relative axis-angle rotation sequence is equivalent to an absolute one with the rotation
    517         // sequence reversed, meaning we can reuse our relative logic here.
    518         return reverseArray(getRelativeEulerAngles(axis1, axis2));
    519     }
    520
    521     /** Create a new instance from the given quaternion. The quaternion is normalized and
    522      * converted to positive polar form (ie, with w &gt;= 0).
    523      *
    524      * @param quat the quaternion to use for the rotation
    525      * @return a new instance built from the given quaternion.
    526      * @throws IllegalStateException if the the norm of the given components is zero,
    527      *                              NaN, or infinite
    528      * @see Quaternion#normalize()
    529      * @see Quaternion#positivePolarForm()
    530      */
    531     public static QuaternionRotation of(final Quaternion quat) {
    532         return new QuaternionRotation(quat);
    533     }
    534
    535     /**
    536      * Create a new instance from the given quaternion values. The inputs are
    537      * normalized and converted to positive polar form (ie, with w &gt;= 0).
    538      *
    539      * @param w quaternion scalar component
    540      * @param x first quaternion vectorial component
    541      * @param y second quaternion vectorial component
    542      * @param z third quaternion vectorial component
    543      * @return a new instance containing the normalized quaterion components
    544      * @throws IllegalStateException if the the norm of the given components is zero,
    545      *                              NaN, or infinite
    546      * @see Quaternion#normalize()
    547      * @see Quaternion#positivePolarForm()
    548      */
    549     public static QuaternionRotation of(final double w,
    550                                         final double x,
    551                                         final double y,
    552                                         final double z) {
    553         return of(Quaternion.of(w, x, y, z));
    554     }
    555
    556     /** Return an instance representing a rotation of zero.
    557      * @return instance representing a rotation of zero.
    558      */
    559     public static QuaternionRotation identity() {
    560         return IDENTITY_INSTANCE;
    561     }
    562
    563     /** Create a new instance representing a rotation of {@code angle} radians around
    564      * {@code axis}.
    565      *
    566      * <p>
    567      * Rotation direction follows the right-hand rule, meaning that if one
    568      * places their right hand such that the thumb points in the direction of the vector,
    569      * the curl of the fingers indicates the direction of rotation.
    570      * </p>
    571      *
    572      * <p>
    573      * Note that the returned quaternion will represent the defined rotation but the values
    574      * returned by {@link #getAxis()} and {@link #getAngle()} may not match the ones given here.
    575      * This is because the axis and angle are normalized such that the axis has unit length,
    576      * and the angle lies in the range {@code [0, pi]}. Depending on the inputs, the axis may
    577      * need to be inverted in order for the angle to lie in this range.
    578      * </p>
    579      *
    580      * @param axis the axis of rotation
    581      * @param angle angle of rotation in radians
    582      * @return a new instance representing the defined rotation
    583      *
    584      * @throws IllegalArgumentException if the given axis cannot be normalized or the angle is NaN or infinite
    585      */
    586     public static QuaternionRotation fromAxisAngle(final Vector3D axis, final double angle) {
    587         // reference formula:
    588         // http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/index.htm
    589         final Vector3D normAxis = axis.normalize();
    590
    591         if (!Double.isFinite(angle)) {
    592             throw new IllegalArgumentException("Invalid angle: " + angle);
    593         }
    594
    595         final double halfAngle = 0.5 * angle;
    596         final double sinHalfAngle = Math.sin(halfAngle);
    597
    598         final double w = Math.cos(halfAngle);
    599         final double x = sinHalfAngle * normAxis.getX();
    600         final double y = sinHalfAngle * normAxis.getY();
    601         final double z = sinHalfAngle * normAxis.getZ();
    602
    603         return of(w, x, y, z);
    604     }
    605
    606     /** Return an instance that rotates the first vector to the second.
    607      *
    608      * <p>Except for a possible scale factor, if the returned instance is
    609      * applied to vector {@code u}, it will produce the vector {@code v}. There are an
    610      * infinite number of such rotations; this method chooses the one with the smallest
    611      * associated angle, meaning the one whose axis is orthogonal to the {@code (u, v)}
    612      * plane. If {@code u} and {@code v} are collinear, an arbitrary rotation axis is
    613      * chosen.</p>
    614      *
    615      * @param u origin vector
    616      * @param v target vector
    617      * @return a new instance that rotates {@code u} to point in the direction of {@code v}
    618      * @throws IllegalArgumentException if either vector has a norm of zero, NaN, or infinity
    619      */
    620     public static QuaternionRotation createVectorRotation(final Vector3D u, final Vector3D v) {
    621
    622         final double normProduct  = Vectors.checkedNorm(u) * Vectors.checkedNorm(v);
    623         final double dot = u.dot(v);
    624
    625         if (dot < ANTIPARALLEL_DOT_THRESHOLD * normProduct) {
    626             // Special case where u1 = -u2:
    627             // create a pi angle rotation around
    628             // an arbitrary unit vector orthogonal to u1
    629             final Vector3D axis = u.orthogonal();
    630
    631             return of(0,
    632                       axis.getX(),
    633                       axis.getY(),
    634                       axis.getZ());
    635         }
    636
    637         // General case:
    638         // (u1, u2) defines a plane so rotate around the normal of the plane
    639
    640         // w must equal cos(theta/2); we can calculate this directly using values
    641         // we already have with the identity cos(theta/2) = sqrt((1 + cos(theta)) / 2)
    642         // and the fact that dot = norm(u1) * norm(u2) * cos(theta).
    643         final double w = Math.sqrt(0.5 * (1.0 + (dot / normProduct)));
    644
    645         // The cross product u1 x u2 must be normalized and then multiplied by
    646         // sin(theta/2) in order to set the vectorial part of the quaternion. To
    647         // accomplish this, we'll use the following:
    648         //
    649         // 1) norm(a x b) = norm(a) * norm(b) * sin(theta)
    650         // 2) sin(theta/2) = sqrt((1 - cos(theta)) / 2)
    651         //
    652         // Our full, combined normalization and sine half angle term factor then becomes:
    653         //
    654         // sqrt((1 - cos(theta)) / 2) / (norm(u1) * norm(u2) * sin(theta))
    655         //
    656         // This can be simplified to the expression below.
    657         final double vectorialScaleFactor = 1.0 / (2.0 * w * normProduct);
    658         final Vector3D axis = u.cross(v);
    659
    660         return of(w,
    661                   vectorialScaleFactor * axis.getX(),
    662                   vectorialScaleFactor * axis.getY(),
    663                   vectorialScaleFactor * axis.getZ());
    664     }
    665
    666     /** Return an instance that rotates the basis defined by the first two vectors into the basis
    667      * defined by the second two.
    668      *
    669      * <p>
    670      * The given basis vectors do not have to be directly orthogonal. A right-handed orthonormal
    671      * basis is created from each pair by normalizing the first vector, making the second vector
    672      * orthogonal to the first, and then taking the cross product. A rotation is then calculated
    673      * that rotates the first to the second.
    674      * </p>
    675      *
    676      * @param u1 first vector of the source basis
    677      * @param u2 second vector of the source basis
    678      * @param v1 first vector of the target basis
    679      * @param v2 second vector of the target basis
    680      * @return an instance that rotates the source basis to the target basis
    681      * @throws IllegalArgumentException if any of the input vectors cannot be normalized
    682      *      or the vectors defining either basis are collinear
    683      */
    684     public static QuaternionRotation createBasisRotation(final Vector3D u1, final Vector3D u2,
    685             final Vector3D v1, final Vector3D v2) {
    686
    687         // calculate orthonormalized bases
    688         final Vector3D a = u1.normalize();
    689         final Vector3D b = a.orthogonal(u2);
    690         final Vector3D c = a.cross(b);
    691
    692         final Vector3D d = v1.normalize();
    693         final Vector3D e = d.orthogonal(v2);
    694         final Vector3D f = d.cross(e);
    695
    696         // create an orthogonal rotation matrix representing the change of basis; this matrix will
    697         // be the multiplication of the matrix composed of the column vectors d, e, f and the
    698         // inverse of the matrix composed of the column vectors a, b, c (which is simply the transpose since
    699         // it's orthogonal).
    700         final double m00 = Vectors.linearCombination(d.getX(), a.getX(), e.getX(), b.getX(), f.getX(), c.getX());
    701         final double m01 = Vectors.linearCombination(d.getX(), a.getY(), e.getX(), b.getY(), f.getX(), c.getY());
    702         final double m02 = Vectors.linearCombination(d.getX(), a.getZ(), e.getX(), b.getZ(), f.getX(), c.getZ());
    703
    704         final double m10 = Vectors.linearCombination(d.getY(), a.getX(), e.getY(), b.getX(), f.getY(), c.getX());
    705         final double m11 = Vectors.linearCombination(d.getY(), a.getY(), e.getY(), b.getY(), f.getY(), c.getY());
    706         final double m12 = Vectors.linearCombination(d.getY(), a.getZ(), e.getY(), b.getZ(), f.getY(), c.getZ());
    707
    708         final double m20 = Vectors.linearCombination(d.getZ(), a.getX(), e.getZ(), b.getX(), f.getZ(), c.getX());
    709         final double m21 = Vectors.linearCombination(d.getZ(), a.getY(), e.getZ(), b.getY(), f.getZ(), c.getY());
    710         final double m22 = Vectors.linearCombination(d.getZ(), a.getZ(), e.getZ(), b.getZ(), f.getZ(), c.getZ());
    711
    712
    713         return orthogonalRotationMatrixToQuaternion(
    714                     m00, m01, m02,
    715                     m10, m11, m12,
    716                     m20, m21, m22
    717                 );
    718     }
    719
    720     /** Create a new instance equivalent to the given sequence of axis-angle rotations.
    721      * @param sequence the axis-angle rotation sequence to convert to a quaternion rotation
    722      * @return instance representing a rotation equivalent to the given axis-angle sequence
    723      */
    724     public static QuaternionRotation fromAxisAngleSequence(final AxisAngleSequence sequence) {
    725         final AxisSequence axes = sequence.getAxisSequence();
    726
    727         final QuaternionRotation q1 = fromAxisAngle(axes.getAxis1(), sequence.getAngle1());
    728         final QuaternionRotation q2 = fromAxisAngle(axes.getAxis2(), sequence.getAngle2());
    729         final QuaternionRotation q3 = fromAxisAngle(axes.getAxis3(), sequence.getAngle3());
    730
    731         if (sequence.getReferenceFrame() == AxisReferenceFrame.ABSOLUTE) {
    732             return q3.multiply(q2).multiply(q1);
    733         }
    734
    735         return q1.multiply(q2).multiply(q3);
    736     }
    737
    738     /** Create an instance from an orthogonal rotation matrix.
    739      *
    740      * @param m00 matrix entry <code>m<sub>0,0</sub></code>
    741      * @param m01 matrix entry <code>m<sub>0,1</sub></code>
    742      * @param m02 matrix entry <code>m<sub>0,2</sub></code>
    743      * @param m10 matrix entry <code>m<sub>1,0</sub></code>
    744      * @param m11 matrix entry <code>m<sub>1,1</sub></code>
    745      * @param m12 matrix entry <code>m<sub>1,2</sub></code>
    746      * @param m20 matrix entry <code>m<sub>2,0</sub></code>
    747      * @param m21 matrix entry <code>m<sub>2,1</sub></code>
    748      * @param m22 matrix entry <code>m<sub>2,2</sub></code>
    749      * @return an instance representing the same 3D rotation as the given matrix
    750      */
    751     private static QuaternionRotation orthogonalRotationMatrixToQuaternion(
    752             final double m00, final double m01, final double m02,
    753             final double m10, final double m11, final double m12,
    754             final double m20, final double m21, final double m22) {
    755
    756         // reference formula:
    757         // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
    758
    759         // The overall approach here is to take the equations for converting a quaternion to
    760         // a matrix along with the fact that 1 = x^2 + y^2 + z^2 + w^2 for a normalized quaternion
    761         // and solve for the various terms. This can theoretically be done using just the diagonal
    762         // terms from the matrix. However, there are a few issues with this:
    763         // 1) The term that we end up taking the square root of may be negative.
    764         // 2) It's ambiguous as to whether we should use a plus or minus for the value of the
    765         //    square root.
    766         // We'll address these concerns by only calculating a single term from one of the diagonal
    767         // elements and then calculate the rest from the non-diagonals, which do not involve
    768         // a square root. This solves the first issue since we can make sure to choose a diagonal
    769         // element that will not cause us to take a square root of a negative number. The second
    770         // issue is solved since only the relative signs between the quaternion terms are important
    771         // (q and -q represent the same 3D rotation). It therefore doesn't matter whether we choose
    772         // a plus or minus for our initial square root solution.
    773
    774         final double trace = m00 + m11 + m22;
    775
    776         final double w;
    777         final double x;
    778         final double y;
    779         final double z;
    780
    781         if (trace > 0) {
    782             // let s = 4*w
    783             final double s = 2.0 * Math.sqrt(1.0 + trace);
    784             final double sinv = 1.0 / s;
    785
    786             x = (m21 - m12) * sinv;
    787             y = (m02 - m20) * sinv;
    788             z = (m10 - m01) * sinv;
    789             w = 0.25 * s;
    790         } else if ((m00 > m11) && (m00 > m22)) {
    791             // let s = 4*x
    792             final double s = 2.0 * Math.sqrt(1.0 + m00 - m11 - m22);
    793             final double sinv = 1.0 / s;
    794
    795             x = 0.25 * s;
    796             y = (m01 + m10) * sinv;
    797             z = (m02 + m20) * sinv;
    798             w = (m21 - m12) * sinv;
    799         } else if (m11 > m22) {
    800             // let s = 4*y
    801             final double s = 2.0 * Math.sqrt(1.0 + m11 - m00 - m22);
    802             final double sinv = 1.0 / s;
    803
    804             x = (m01 + m10) * sinv;
    805             y = 0.25 * s;
    806             z = (m21 + m12) * sinv;
    807             w = (m02 - m20) * sinv;
    808         } else {
    809             // let s = 4*z
    810             final double s = 2.0 * Math.sqrt(1.0 + m22 - m00 - m11);
    811             final double sinv = 1.0 / s;
    812
    813             x = (m02 + m20) * sinv;
    814             y = (m21 + m12) * sinv;
    815             z = 0.25 * s;
    816             w = (m10 - m01) * sinv;
    817         }
    818
    819         return of(w, x, y, z);
    820     }
    821
    822     /** Reverse the elements in {@code arr}. The array is returned.
    823      * @param arr the array to reverse
    824      * @return the input array with the elements reversed
    825      */
    826     private static double[] reverseArray(final double[] arr) {
    827
    828         final int len = arr.length;
    829         double temp;
    830
    831         int i;
    832         int j;
    833
    834         for (i = 0, j = len - 1; i < len / 2; ++i, --j) {
    835             temp = arr[i];
    836             arr[i] = arr[j];
    837             arr[j] = temp;
    838         }
    839
    840         return arr;
    841     }
    842 }