/*
    * Copyright (C) 2012 Alberto Irurueta Carro (alberto@irurueta.com)
    *
    * Licensed under the Apache License, Version 2.0 (the "License");
    * you may not use this file except in compliance with the License.
    * You may obtain a copy of the License at
    *
    *         http://www.apache.org/licenses/LICENSE-2.0
    *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   */
  package com.irurueta.geometry;
  
  import com.irurueta.algebra.AlgebraException;
  import com.irurueta.algebra.Matrix;
  import com.irurueta.algebra.SingularValueDecomposer;
  import com.irurueta.algebra.Utils;
  import com.irurueta.algebra.WrongSizeException;
  
  import java.io.Serializable;
  
  /**
   * This class defines the amount of rotation for 3D points or planes.
   * Rotation is defined internally as a matrix.
   */
  @SuppressWarnings("DuplicatedCode")
  public class MatrixRotation3D extends Rotation3D implements Serializable {
  
      /**
       * Constant defining the number of rows on a 3D rotation matrix expressed
       * in inhomogeneous coordinates.
       */
      public static final int ROTATION3D_INHOM_MATRIX_ROWS = 3;
  
      /**
       * Constant defining the number of columns on a 3D rotation matrix expressed
       * in inhomogeneous coordinates.
       */
      public static final int ROTATION3D_INHOM_MATRIX_COLS = 3;
  
      /**
       * Constant defining the number of rows on a 3D rotation matrix expressed
       * in homogeneous coordinates.
       */
      public static final int ROTATION3D_HOM_MATRIX_ROWS = 4;
  
      /**
       * Constant defining the number of columns on a 3D rotation matrix expressed
       * in homogeneous coordinates.
       */
      public static final int ROTATION3D_HOM_MATRIX_COLS = 4;
  
      /**
       * Threshold to determine that a gimbal locked might have been achieved
       * when trying to find roll, pitch and yaw angles.
       */
      public static final double GIMBAL_THRESHOLD = 1e-6;
  
      /**
       * Internal matrix containing rotation using inhomogeneous coordinates.
       * This matrix will be square, 3x3, orthogonal and will have determinant
       * equal to one.
       */
      protected Matrix internalMatrix;
  
  
      /**
       * Empty Constructor.
       * Initializes rotation so that no rotation exists (i.e. internal matrix is
       * the identity).
       */
      public MatrixRotation3D() {
          try {
              internalMatrix = Matrix.identity(ROTATION3D_INHOM_MATRIX_ROWS, ROTATION3D_INHOM_MATRIX_ROWS);
          } catch (final WrongSizeException ignore) {
              // never happens
          }
      }
  
      /**
       * Copy constructor.
       * Copies provided rotation into this instance.
       *
       * @param rotation Instance to be copied.
       */
      public MatrixRotation3D(final MatrixRotation3D rotation) {
          internalMatrix = new Matrix(rotation.internalMatrix);
      }
  
      /**
       * Copy constructor.
       * Copies and converts provided rotation into this instance.
       *
       * @param rotation Instance to be copied.
       */
     public MatrixRotation3D(final Rotation3D rotation) {
         internalMatrix = rotation.asInhomogeneousMatrix();
     }
 
     /**
      * Constructor.
      * Creates a 3D rotation using provided matrix.
      * Provided matrix can be expressed in either homogeneous or inhomogeneous
      * coordinates, and it must also be orthogonal and having determinant equal
      * to 1.
      * The threshold to determine whether provided matrix is orthonormal will
      * be DEFAULT_VALID_THRESHOLD.
      *
      * @param m Matrix to create rotation from.
      * @throws InvalidRotationMatrixException Raised if provided matrix is not
      *                                        valid (its size is wrong, or it is not orthonormal).
      *                                        {@link #isValidRotationMatrix(Matrix)}.
      */
     public MatrixRotation3D(final Matrix m) throws InvalidRotationMatrixException {
         fromMatrix(m);
     }
 
     /**
      * Constructor.
      * Creates a 3D rotation using provided matrix.
      * Provided matrix can be expressed in either homogeneous or inhomogeneous
      * coordinates, and it must also be orthogonal up to provided threshold, and
      * must have determinant equal to 1.
      *
      * @param m         Matrix to create rotation from.
      * @param threshold Threshold to determine whether matrix is orthonormal.
      * @throws InvalidRotationMatrixException Raised if provided matrix is not
      *                                        valid (its size is wrong, or it is not orthonormal).
      * @throws IllegalArgumentException       Raised if provided threshold is
      *                                        negative.
      *                                        {@link #isValidRotationMatrix(Matrix)}.
      */
     public MatrixRotation3D(final Matrix m, final double threshold) throws InvalidRotationMatrixException {
         fromMatrix(m, threshold);
     }
 
     /**
      * Constructor.
      * Creates a 3D rotation using provided Euler angles expressed in radians.
      *
      * @param alphaEuler Alpha Euler angle expressed in radians.
      * @param betaEuler  Beta Euler angle expressed in radians.
      * @param gammaEuler Gamma Euler angle expressed in radians.
      */
     public MatrixRotation3D(final double alphaEuler, final double betaEuler, final double gammaEuler) {
         setEulerAngles(alphaEuler, betaEuler, gammaEuler);
     }
 
     /**
      * Constructor.
      * Creates a 3D reconstruction using provided rotation axis and rotation
      * angle expressed in radians.
      *
      * @param axis  Axis of rotation. Axis must be a length-3 array containing
      *              the axis vector. For better accuracy axis coordinates should be
      *              normalized (norm equal to 1).
      * @param theta Angle of rotation respect the axis expressed in radians.
      * @throws IllegalArgumentException Raised if provided axis does not have
      *                                  length 3.
      */
     public MatrixRotation3D(final double[] axis, final double theta) {
         setAxisAndRotation(axis, theta);
     }
 
     /**
      * Constructor.
      * Creates a 3D reconstruction using provided rotation axis coordinates and
      * rotation angle expressed in radians.
      * Note: for better accuracy axis coordinates should be normalized (norm
      * equal to 1).
      *
      * @param axisX X coordinate of axis.
      * @param axisY Y coordinate of axis.
      * @param axisZ Z coordinate of axis.
      * @param theta Angle of rotation respect the axis expressed in radians.
      */
     public MatrixRotation3D(final double axisX, final double axisY, final double axisZ, final double theta) {
         setAxisAndRotation(axisX, axisY, axisZ, theta);
     }
 
     /**
      * Returns type of this rotation.
      *
      * @return Type of this rotation.
      */
     @Override
     public Rotation3DType getType() {
         return Rotation3DType.MATRIX_ROTATION3D;
     }
 
     /**
      * Returns a copy of the internal matrix so that the internal matrix cannot
      * be modified accidentally.
      * Returned matrix will be 3x3, orthogonal and will have determinant equal
      * to one.
      *
      * @return Internal matrix containing rotation of this instance.
      */
     public Matrix getInternalMatrix() {
         return new Matrix(internalMatrix);
     }
 
     /**
      * Sets the internal matrix of this rotation. Provided matrix must be 3x3
      * and orthonormal (orthogonal with determinant equal to 1).
      *
      * @param internalMatrix Internal matrix to be set.
      * @throws InvalidRotationMatrixException Raised if provided matrix is not
      *                                        3x3, or it is not orthonormal.
      */
     public final void setInternalMatrix(final Matrix internalMatrix) throws InvalidRotationMatrixException {
         setInternalMatrix(internalMatrix, DEFAULT_VALID_THRESHOLD);
     }
 
     /**
      * Sets the internal matrix of this rotation. Provided matrix must be 3x3
      * and orthonormal (orthogonal with determinant equal to 1) up to an error
      * equal to provided threshold.
      *
      * @param m         Internal matrix to be set.
      * @param threshold Threshold to determine whether matrix is orthonormal or
      *                  not.
      * @throws InvalidRotationMatrixException Raised if provided matrix is not
      *                                        3x3, or it is not orthonormal.
      * @throws IllegalArgumentException       Raised if provided threshold is
      *                                        negative.
      */
     public final void setInternalMatrix(final Matrix m, final double threshold) throws InvalidRotationMatrixException {
         if (m.getRows() != ROTATION3D_INHOM_MATRIX_ROWS || m.getColumns() != ROTATION3D_INHOM_MATRIX_COLS) {
             throw new InvalidRotationMatrixException();
         }
         if (!isValidRotationMatrix(m, threshold)) {
             throw new InvalidRotationMatrixException();
         }
 
         internalMatrix = m;
     }
 
     /**
      * Returns alpha euler angle within the range -pi and pi.
      *
      * @return Alpha euler angle expressed in radians.
      */
     public double getAlphaEulerAngle() {
         return Math.atan2(-internalMatrix.getElementAt(2, 0),
                 internalMatrix.getElementAt(2, 2));
     }
 
     /**
      * Returns beta euler angle within the range -pi/2 and pi/2.
      *
      * @return Beta euler angle expressed in radians.
      */
     public double getBetaEulerAngle() {
         return Math.asin(internalMatrix.getElementAt(2, 1));
     }
 
     /**
      * Returns gamma euler angle within the range -pi and pi.
      *
      * @return Gamma euler angle expressed in radians.
      */
     public double getGammaEulerAngle() {
         return Math.atan2(-internalMatrix.getElementAt(0, 1),
                 internalMatrix.getElementAt(1, 1));
     }
 
     /**
      * Sets euler angles of this rotation, expressed in radians.
      *
      * @param alphaEuler Alpha euler angle in radians.
      * @param betaEuler  Beta euler angle in radians.
      * @param gammaEuler Gamma euler angle in radians.
      */
     public final void setEulerAngles(final double alphaEuler, final double betaEuler, final double gammaEuler) {
         final var sinAlpha = Math.sin(alphaEuler);
         final var cosAlpha = Math.cos(alphaEuler);
 
         final var sinBeta = Math.sin(betaEuler);
         final var cosBeta = Math.cos(betaEuler);
 
         final var sinGamma = Math.sin(gammaEuler);
         final var cosGamma = Math.cos(gammaEuler);
 
         // reuse internal matrix if possible
         try {
             if (internalMatrix == null) {
                 internalMatrix = new Matrix(ROTATION3D_INHOM_MATRIX_ROWS, ROTATION3D_INHOM_MATRIX_COLS);
             }
 
             internalMatrix.setElementAt(0, 0, cosAlpha * cosGamma - sinAlpha * sinBeta * sinGamma);
             internalMatrix.setElementAt(1, 0, cosAlpha * sinGamma + sinAlpha * sinBeta * cosGamma);
             internalMatrix.setElementAt(2, 0, -sinAlpha * cosBeta);
 
             internalMatrix.setElementAt(0, 1, -cosBeta * sinGamma);
             internalMatrix.setElementAt(1, 1, cosBeta * cosGamma);
             internalMatrix.setElementAt(2, 1, sinBeta);
 
             internalMatrix.setElementAt(0, 2, sinAlpha * cosGamma + cosAlpha * sinBeta * sinGamma);
             internalMatrix.setElementAt(1, 2, sinAlpha * sinGamma - cosAlpha * sinBeta * cosGamma);
             internalMatrix.setElementAt(2, 2, cosAlpha * cosBeta);
         } catch (final WrongSizeException ignore) {
             // never happens
         }
     }
 
     /**
      * Returns roll angle around x-axis expressed in radians for the 1st
      * possible set of solutions.
      * When obtaining roll, pitch and yaw angles from a rotation matrix, there
      * might be two possible sets of solutions (#getRollAngle(),
      * #getPitchAngle(), #getYawAngle()) or (#getRollAngle2(),
      * #getPitchAngle2(), #getYawAngle2()).
      *
      * @return roll angle around x-axis.
      * @see <a href="http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf">http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf</a>
      * @see <a href="https://github.com/joansola/slamtb">R2e.m at https://github.com/joansola/slamtb</a>
      */
     public double getRollAngle() {
         return getRollAngle(getPitchAngle());
     }
 
     /**
      * Returns roll angle around x-axis expressed in radians for the 2nd
      * possible set of solutions.
      * When obtaining roll, pitch and yaw angles from a rotation matrix, there
      * might be two possible sets of solutions (#getRollAngle(),
      * #getPitchAngle(), #getYawAngle()) or (#getRollAngle2(),
      * #getPitchAngle2(), #getYawAngle2()).
      *
      * @return roll angle around x-axis.
      * @see <a href="http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf">http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf</a>
      * @see <a href="https://github.com/joansola/slamtb">R2e.m at https://github.com/joansola/slamtb</a>
      */
     public double getRollAngle2() {
         return getRollAngle(getPitchAngle2());
     }
 
     /**
      * Returns roll angle around x-axis expressed in radians corresponding to
      * provided pitch value.
      * This method is used internally.
      *
      * @param pitch pitch angle expressed in radians.
      * @return roll angle around x-axis.
      */
     private double getRollAngle(final double pitch) {
         if (!hasGimbalLock()) {
             final var cosPitch = Math.cos(pitch);
             return Math.atan2(internalMatrix.getElementAt(2, 1) / cosPitch,
                     internalMatrix.getElementAt(2, 2) / cosPitch);
         } else {
             // gimbal lock (pitch is close to +-90 degrees)
             if (internalMatrix.getElementAt(2, 0) < 0.0) {
                 // pitch is +90 degrees
                 return Math.atan2(internalMatrix.getElementAt(0, 1),
                         internalMatrix.getElementAt(0, 2));
             } else {
                 // pitch is -90 degrees
                 return Math.atan2(-internalMatrix.getElementAt(0, 1),
                         -internalMatrix.getElementAt(0, 2));
             }
         }
     }
 
     /**
      * Returns pitch angle around y-axis expressed in radians for the 1st
      * possible set of solutions.
      * When obtaining roll, pitch and yaw angles from a rotation matrix, there
      * might be two possible sets of solutions (#getRollAngle(),
      * #getPitchAngle(), #getYawAngle()) or (#getRollAngle2(),
      * #getPitchAngle2(), #getYawAngle2()).
      *
      * @return pitch angle around y-axis.
      * @see <a href="http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf">http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf</a>
      * @see <a href="https://github.com/joansola/slamtb">R2e.m at https://github.com/joansola/slamtb</a>
      */
     public double getPitchAngle() {
         return -Math.asin(internalMatrix.getElementAt(2, 0));
     }
 
     /**
      * Returns pitch angle around y-axis expressed in radians for the 2nd
      * possible set of solutions.
      * When obtaining roll, pitch and yaw angles from a rotation matrix, there
      * might be two possible sets of solutions (#getRollAngle(),
      * #getPitchAngle(), #getYawAngle()) or (#getRollAngle2(),
      * #getPitchAngle2(), #getYawAngle2()).
      * When a gimbal lock occurs, both pitch angles are equal because only
      * yaw is undefined, but pitch and roll are unique.
      *
      * @return pitch angle around y-axis.
      * @see <a href="http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf">http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf</a>
      * @see <a href="https://github.com/joansola/slamtb">R2e.m at https://github.com/joansola/slamtb</a>
      */
     public double getPitchAngle2() {
         if (!hasGimbalLock()) {
             return Math.PI - getPitchAngle();
         } else {
             return getPitchAngle();
         }
     }
 
     /**
      * Returns yaw angle around z axis expressed in radians for the 1st possible
      * set of solutions.
      * When a gimbal lock occurs (pitch angle is close to +- 90 degrees), then
      * yaw angle is undefined, and can be any value, although this method will
      * return 0.0.
      * When obtaining roll, pitch and yaw angles from a rotation matrix, there
      * might be two possible sets of solutions (#getRollAngle(),
      * #getPitchAngle(), #getYawAngle()) or (#getRollAngle2(),
      * #getPitchAngle2(), #getYawAngle2()).
      *
      * @return yaw angle around z axis.
      * @see <a href="http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf">http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf</a>
      * @see <a href="https://github.com/joansola/slamtb">R2e.m at https://github.com/joansola/slamtb</a>
      */
     public double getYawAngle() {
         return getYawAngle(getPitchAngle());
     }
 
     /**
      * Returns yaw angle around z axis expressed in radians for the 2nd possible
      * set of solutions.
      * When a gimbal lock occurs (pitch angle is close to +- 90 degrees), then
      * yaw angle is undefined, and can be any value, although this method will
      * return 0.0.
      * When obtaining roll, pitch and yaw angles from a rotation matrix, there
      * might be two possible sets of solutions (#getRollAngle(),
      * #getPitchAngle(), #getYawAngle()) or (#getRollAngle2(),
      * #getPitchAngle2(), #getYawAngle2()).
      *
      * @return yaw angle around z axis.
      * @see <a href="http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf">http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf</a>
      * @see <a href="https://github.com/joansola/slamtb">R2e.m at https://github.com/joansola/slamtb</a>
      */
     public double getYawAngle2() {
         return getYawAngle(getPitchAngle2());
     }
 
     /**
      * Returns yaw angle around x-axis expressed in radians corresponding to
      * provided pitch value.
      * This method is used internally.
      *
      * @param pitch pitch angle expressed in radians.
      * @return yaw angle around x-axis.
      */
     private double getYawAngle(final double pitch) {
         if (!hasGimbalLock()) {
             final var cosPitch = Math.cos(pitch);
             return Math.atan2(internalMatrix.getElementAt(1, 0) / cosPitch,
                     internalMatrix.getElementAt(0, 0) / cosPitch);
         } else {
             // gimbal lock (pitch is close to +-90 degrees)
             // can be anything.
             return 0.0;
         }
     }
 
     /**
      * Indicates whether current rotation contains ambiguities (a.k.a. gimbal
      * lock). This situation happens when pitch angle is close to +-90 degrees.
      *
      * @return true if current rotation contains a gimbal lock, false otherwise.
      * @see <a href="http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf">http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf</a>
      * @see <a href="https://github.com/joansola/slamtb">R2e.m at https://github.com/joansola/slamtb</a>
      */
     public boolean hasGimbalLock() {
         return Math.abs(Math.abs(internalMatrix.getElementAt(2, 0)) - 1.0) <
                 GIMBAL_THRESHOLD;
     }
 
     /**
      * Sets rotation angles, expressed in radians.
      *
      * @param roll  roll angle in radians around x-axis.
      * @param pitch pitch angle in radians around y-axis.
      * @param yaw   yaw angle in radians around z-axis.
      * @see <a href="http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf">http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf</a>
      * @see <a href="https://github.com/joansola/slamtb">e2R.m at https://github.com/joansola/slamtb</a>
      */
     public void setRollPitchYaw(final double roll, final double pitch, final double yaw) {
         final var sr = Math.sin(roll);
         final var cr = Math.cos(roll);
 
         final var sp = Math.sin(pitch);
         final var cp = Math.cos(pitch);
 
         final var sy = Math.sin(yaw);
         final var cy = Math.cos(yaw);
 
         try {
             // reuse internal matrix if possible
             if (internalMatrix == null) {
                 internalMatrix = new Matrix(ROTATION3D_INHOM_MATRIX_ROWS, ROTATION3D_INHOM_MATRIX_COLS);
             }
 
             internalMatrix.setElementAt(0, 0, cp * cy);
             internalMatrix.setElementAt(1, 0, cp * sy);
             internalMatrix.setElementAt(2, 0, -sp);
 
             internalMatrix.setElementAt(0, 1, -cr * sy + sr * sp * cy);
             internalMatrix.setElementAt(1, 1, cr * cy + sr * sp * sy);
             internalMatrix.setElementAt(2, 1, sr * cp);
 
             internalMatrix.setElementAt(0, 2, sr * sy + cr * sp * cy);
             internalMatrix.setElementAt(1, 2, -sr * cy + cr * sp * sy);
             internalMatrix.setElementAt(2, 2, cr * cp);
         } catch (final WrongSizeException ignore) {
             // never happens
         }
     }
 
     /**
      * Sets the axis and rotation of this instance.
      * Once set, points will rotate around provided axis an amount equal to
      * provided rotation angle in radians.
      * Note: to avoid numerical instabilities and improve accuracy, axis
      * coordinates should be normalized (e.g. norm equal to 1).
      *
      * @param axisX X coordinate of rotation axis.
      * @param axisY Y coordinate of rotation axis.
      * @param axisZ Z coordinate of rotation axis.
      * @param theta Amount of rotation in radians.
      */
     @Override
     public final void setAxisAndRotation(
             final double axisX, final double axisY, final double axisZ, final double theta) {
         final var axisX2 = axisX * axisX;
         final var axisY2 = axisY * axisY;
         final var axisZ2 = axisZ * axisZ;
 
         final var axisXY = axisX * axisY;
         final var axisXZ = axisX * axisZ;
         final var axisYZ = axisY * axisZ;
 
         final var sinTheta = Math.sin(theta);
         final var cosTheta = Math.cos(theta);
 
         try {
             if (internalMatrix == null) {
                 internalMatrix = new Matrix(ROTATION3D_INHOM_MATRIX_ROWS, ROTATION3D_INHOM_MATRIX_COLS);
             }
 
             internalMatrix.setElementAt(0, 0, axisX2 + (1.0 - axisX2) * cosTheta);
             internalMatrix.setElementAt(1, 0, axisXY * (1.0 - cosTheta) + axisZ * sinTheta);
             internalMatrix.setElementAt(2, 0, axisXZ * (1.0 - cosTheta) - axisY * sinTheta);
 
             internalMatrix.setElementAt(0, 1, axisXY * (1.0 - cosTheta) - axisZ * sinTheta);
             internalMatrix.setElementAt(1, 1, axisY2 + (1.0 - axisY2) * cosTheta);
             internalMatrix.setElementAt(2, 1, axisYZ * (1.0 - cosTheta) + axisX * sinTheta);
 
             internalMatrix.setElementAt(0, 2, axisXZ * (1.0 - cosTheta) + axisY * sinTheta);
             internalMatrix.setElementAt(1, 2, axisYZ * (1.0 - cosTheta) - axisX * sinTheta);
             internalMatrix.setElementAt(2, 2, axisZ2 + (1.0 - axisZ2) * cosTheta);
         } catch (final WrongSizeException ignore) {
             // never happens
         }
     }
 
     /**
      * Returns rotation axis corresponding to this instance.
      * Result is stored in provided axis array, which must have length 3.
      *
      * @param axis Array where axis coordinates will be stored.
      * @throws IllegalArgumentException Raised if provided array does not have
      *                                  length 3.
      * @throws RotationException        Raised if numerical instabilities happen.
      *                                  Because internal matrix will always be well-defined (orthogonal and
      *                                  determinant equal to 1), this exception will rarely happen.
      */
     @Override
     public void rotationAxis(final double[] axis) throws RotationException {
         if (axis.length != ROTATION3D_INHOM_MATRIX_ROWS) {
             throw new IllegalArgumentException();
         }
 
         try {
             // Rotation axis follows: R*v = v, which means that rotation axis is
             // left unchanged after rotation. Hence:
             // R*v = I*v --> (R - I)*v = 0, consequently rotation axis is the
             // null-space of R-I
             final Matrix identity = Matrix.identity(ROTATION3D_INHOM_MATRIX_ROWS, ROTATION3D_INHOM_MATRIX_COLS);
             // line below: identity = internalMatrix - identity
             internalMatrix.subtract(identity, identity);
             // internalMatrix - identity
 
             final SingularValueDecomposer decomposer = new SingularValueDecomposer(identity);
 
             decomposer.decompose();
 
             final var v = decomposer.getV();
 
             // last column of V contains axis values
             axis[0] = v.getElementAt(0, 2);
             axis[1] = v.getElementAt(1, 2);
             axis[2] = v.getElementAt(2, 2);
         } catch (final AlgebraException e) {
             throw new RotationException(e);
         }
     }
 
     /**
      * Returns rotation amount or angle in radians around the rotation axis
      * associated to this instance.
      *
      * @return Rotation angle in radians.
      * @throws RotationException Raised if numerical instabilities happen.
      *                           Because internal matrix will always be well-defined (orthogonal and
      *                           determinant equal to 1), this exception will rarely happen.
      */
     @Override
     public double getRotationAngle() throws RotationException {
 
         // obtain rotation axis
         final var axis = getRotationAxis();
 
         try {
             final var axisMatrix = new Matrix(1, ROTATION3D_INHOM_MATRIX_COLS);
             axisMatrix.setSubmatrix(0, 0, 0,
                     ROTATION3D_INHOM_MATRIX_COLS - 1, axis);
 
             final var decomposer = new SingularValueDecomposer(axisMatrix);
             decomposer.decompose();
 
             final var v = decomposer.getV();
 
             // because axisMatrix has rank 1, its null-space will contain a
             // two-dimensional space (two vectors) perpendicular to axisMatrix
             final var perpendicular = v.getSubmatrix(0, 2, 2, 2);
             final var normPerpendicular = Utils.normF(perpendicular);
 
             // use internal matrix to rotate perpendicular vector
             final var rotated = internalMatrix.multiplyAndReturnNew(perpendicular);
             final var normRotated = Utils.normF(rotated);
 
             // normalize vectors
             perpendicular.multiplyByScalar(1.0 / normPerpendicular);
             rotated.multiplyByScalar(1.0 / normRotated);
 
             // their dot product is the cosine of their angle
             // (we transpose rotated matrix to compute dot product)
             rotated.transpose();
             rotated.multiply(perpendicular);
             final var dotProduct = rotated.getElementAtIndex(0);
 
             final var theta = Math.acos(dotProduct);
 
             // we need to determine sign of theta, for that reason we instantiate
             // two camera rotations with theta and -theta using the same rotation
             // axis and check for the rotation matrix that produces less error
             final var rotation1 = new MatrixRotation3D(axis, theta);
             final var rotation2 = new MatrixRotation3D(axis, -theta);
 
             final var rotationMatrix1 = rotation1.internalMatrix;
             final var rotationMatrix2 = rotation2.internalMatrix;
 
             // normalize rotation matrices for their comparison
             final var norm1 = Utils.normF(rotationMatrix1);
             final var norm2 = Utils.normF(rotationMatrix2);
             rotationMatrix1.multiplyByScalar(1.0 / norm1);
             rotationMatrix2.multiplyByScalar(1.0 / norm2);
 
             // normalize current internal matrix for its comparison
             final var internalNorm = Utils.normF(internalMatrix);
             final var internal = internalMatrix.multiplyByScalarAndReturnNew(1.0 / internalNorm);
 
             // compare matrices
             rotationMatrix1.subtract(internal);
             rotationMatrix2.subtract(internal);
 
             // now rotation matrices 1 and 2 contain the difference
             final var normDiff1 = Utils.normF(rotationMatrix1);
             final var normDiff2 = Utils.normF(rotationMatrix2);
 
             // pick the rotation matrix that produces less error
             if (normDiff1 < normDiff2) {
                 // positive theta
                 return theta;
             } else {
                 // negative theta
                 return -theta;
             }
         } catch (final AlgebraException e) {
             throw new RotationException(e);
         }
     }
 
     /**
      * Returns a 3D rotation which is inverse to this instance.
      * In other words, the combination of this rotation with its inverse
      * produces no change.
      *
      * @return Inverse 3D rotation.
      */
     @Override
     public MatrixRotation3D inverseRotationAndReturnNew() {
         final var result = new MatrixRotation3D();
         inverseRotation(result);
         return result;
     }
 
     /**
      * Sets into provided MatrixRotation3D instance a rotation inverse to this
      * instance.
      * The combination of this rotation with its inverse produces no change.
      *
      * @param result Instance where inverse rotation will be set.
      */
     public void inverseRotation(final MatrixRotation3D result) {
         try {
             result.internalMatrix = Utils.inverse(internalMatrix);
         } catch (final AlgebraException ignore) {
             // matrix should always be invertible
         }
     }
 
     /**
      * Sets into provided MatrixRotation3D instance a rotation inverse to this
      * instance.
      * The combination of this rotation with its inverse produces no change.
      *
      * @param result Instance where inverse rotation will be set.
      */
     @Override
     public void inverseRotation(final Rotation3D result) {
         if (result instanceof MatrixRotation3D matrixResult) {
             inverseRotation(matrixResult);
         } else if (result instanceof AxisRotation3D) {
             MatrixRotation3D rot = new MatrixRotation3D();
             inverseRotation(rot);
             try {
                 result.fromMatrix(rot.asInhomogeneousMatrix());
             } catch (final InvalidRotationMatrixException ignore) {
                 // never happens
             }
         }
     }
 
     /**
      * Reverses the rotation of this instance.
      */
     @Override
     public void inverseRotation() {
         inverseRotation(this);
     }
 
 
     /**
      * Returns this 3D rotation instance expressed as a 3x3 inhomogeneous
      * matrix.
      * This is equivalent to call getInternalMatrix().
      *
      * @return Rotation matrix expressed in inhomogeneous coordinates.
      */
     @Override
     public Matrix asInhomogeneousMatrix() {
         return getInternalMatrix();
     }
 
     /**
      * Sets into provided Matrix instance this 3D rotation expressed as a
      * 3x3 inhomogeneous matrix.
      *
      * @param result Matrix where rotation will be set.
      * @throws IllegalArgumentException Raised if provided instance does not
      *                                  have size 3x3.
      */
     @Override
     public void asInhomogeneousMatrix(final Matrix result) {
         if (result.getRows() != ROTATION3D_INHOM_MATRIX_ROWS || result.getColumns() != ROTATION3D_INHOM_MATRIX_COLS) {
             throw new IllegalArgumentException();
         }
 
         result.copyFrom(internalMatrix);
     }
 
     /**
      * Returns this 3D rotation instance expressed as a 4x4 homogeneous matrix.
      *
      * @return Rotation matrix expressed in homogeneous coordinates.
      */
     @Override
     public Matrix asHomogeneousMatrix() {
         Matrix result = null;
         try {
             result = Matrix.identity(ROTATION3D_HOM_MATRIX_ROWS, ROTATION3D_HOM_MATRIX_COLS);
             // sets into 3x3 top-left sub-matrix the internal matrix of this
             // instance, the remaining part will continue to be the identity
             result.setSubmatrix(0, 0, ROTATION3D_INHOM_MATRIX_ROWS - 1,
                     ROTATION3D_INHOM_MATRIX_COLS - 1, internalMatrix);
         } catch (final WrongSizeException ignore) {
             // never happens
         }
         return result;
     }
 
     /**
      * Sets into provided Matrix instance this 3D rotation expressed as a
      * 4x4 homogeneous matrix.
      *
      * @param result Matrix where rotation will be set.
      * @throws IllegalArgumentException Raised if provided instance does not
      *                                  have size 4x4.
      */
     @Override
     public void asHomogeneousMatrix(final Matrix result) {
         if (result.getRows() != ROTATION3D_HOM_MATRIX_ROWS || result.getColumns() != ROTATION3D_HOM_MATRIX_COLS) {
             throw new IllegalArgumentException();
         }
 
         result.initialize(0.0);
         // sets into 3x3 top-left sub-matrix the internal matrix of this instance
         result.setSubmatrix(0, 0, ROTATION3D_INHOM_MATRIX_ROWS - 1,
                 ROTATION3D_INHOM_MATRIX_COLS - 1, internalMatrix);
         // set las element to 1.0 (to be like the identity
         result.setElementAt(ROTATION3D_HOM_MATRIX_ROWS - 1, ROTATION3D_HOM_MATRIX_COLS - 1, 1.0);
     }
 
     /**
      * Sets amount of rotation from provided inhomogeneous rotation matrix.
      * Provided matrix must be orthogonal (i.e. squared, non-singular, it's
      * transpose must be its inverse) and must have determinant equal to 1.
      * Provided matrix must also have size 3x3.
      *
      * @param m         Provided rotation matrix.
      * @param threshold Threshold to determine whether matrix is orthonormal.
      * @throws InvalidRotationMatrixException Raised if provided matrix is not
      *                                        valid (has wrong size, or it is not orthonormal).
      * @throws IllegalArgumentException       Raised if provided threshold is
      *                                        negative.
      *                                        {@link #isValidRotationMatrix(Matrix)}.
      */
     @Override
     public void fromInhomogeneousMatrix(final Matrix m, final double threshold) throws InvalidRotationMatrixException {
         setInternalMatrix(m, threshold);
     }
 
     /**
      * Sets amount of rotation from provided homogeneous rotation matrix.
      * Provided matrix must be orthogonal (i.e. squared, non-singular, it's
      * transpose must be its inverse) and must have determinant equal to 1.
      * Provided matrix must also have size 4x4, and its last row and column must
      * be zero, except for element in last row and column which must be 1.
      *
      * @param m         Provided rotation matrix.
      * @param threshold Threshold to determine whether matrix is orthonormal.
      * @throws InvalidRotationMatrixException Raised if provided matrix is not
      *                                        valid (has wrong size, or it is not orthonormal).
      * @throws IllegalArgumentException       Raised if provided threshold is
      *                                        negative.
      *                                        {@link #isValidRotationMatrix(Matrix)}.
      */
     @Override
     public void fromHomogeneousMatrix(final Matrix m, final double threshold) throws InvalidRotationMatrixException {
         if (m.getRows() != ROTATION3D_HOM_MATRIX_ROWS || m.getColumns() != ROTATION3D_HOM_MATRIX_COLS) {
             throw new InvalidRotationMatrixException();
         }
         if (!isValidRotationMatrix(m, threshold)) {
             throw new InvalidRotationMatrixException();
         }
         if (Math.abs(m.getElementAt(3, 0)) > threshold
                 || Math.abs(m.getElementAt(3, 1)) > threshold
                 || Math.abs(m.getElementAt(3, 2)) > threshold
                 || Math.abs(m.getElementAt(0, 3)) > threshold
                 || Math.abs(m.getElementAt(1, 3)) > threshold
                 || Math.abs(m.getElementAt(2, 3)) > threshold
                 || Math.abs(m.getElementAt(3, 3) - 1.0) > threshold) {
             throw new InvalidRotationMatrixException();
         }
 
         internalMatrix = m.getSubmatrix(0, 0, ROTATION3D_INHOM_MATRIX_ROWS - 1,
                 ROTATION3D_INHOM_MATRIX_COLS - 1);
     }
 
     /**
      * Rotates a 3D point using the origin of coordinates as the axis of
      * rotation.
      * Point will be rotated by the amount of rotation contained in this
      * instance.
      *
      * @param inputPoint  Input point to be rotated.
      * @param resultPoint Rotated point.
      */
     @Override
     public void rotate(final Point3D inputPoint, final Point3D resultPoint) {
         try {
             final var r = asHomogeneousMatrix();
             final var p = new Matrix(Point3D.POINT3D_HOMOGENEOUS_COORDINATES_LENGTH, 1);
 
             // to increase accuracy
             inputPoint.normalize();
             p.setElementAt(0, 0, inputPoint.getHomX());
             p.setElementAt(1, 0, inputPoint.getHomY());
             p.setElementAt(2, 0, inputPoint.getHomZ());
             p.setElementAt(3, 0, inputPoint.getHomW());
 
             // Rotated point below is R * p
             r.multiply(p);
 
             resultPoint.setHomogeneousCoordinates(r.getElementAt(0, 0), r.getElementAt(1, 0),
                     r.getElementAt(2, 0), r.getElementAt(3, 0));
         } catch (final WrongSizeException ignore) {
             // never happens
         }
     }
 
     /**
      * Combines provided rotation with this rotation and returns the result as
      * a new MatrixRotation3D instance.
      *
      * @param rotation Input rotation to be combined.
      * @return Combined rotation, which is equal to the multiplication of the
      * internal matrix of provided rotation with the internal matrix of this
      * instance.
      */
     public MatrixRotation3D combineAndReturnNew(final MatrixRotation3D rotation) {
         final var result = new MatrixRotation3D();
         combine(this, rotation, result);
         return result;
     }
 
     /**
      * Combines provided rotation with this rotation and returns the result as
      * a new MatrixRotation3D instance.
      *
      * @param rotation Input rotation to be combined.
      * @return Combined rotation, which is equal to the multiplication of the
      * internal matrix of provided rotation with the internal matrix of this
      * instance.
      */
     @Override
     public Rotation3D combineAndReturnNew(final Rotation3D rotation) {
         if (rotation instanceof MatrixRotation3D matrixRotation) {
             return combineAndReturnNew(matrixRotation);
         } else {
             return combineAndReturnNew(new MatrixRotation3D(rotation));
         }
     }
 
     /**
      * Combines provided rotation into this rotation resulting in the
      * multiplication of the internal matrices of both rotations.
      *
      * @param rotation Input rotation to be combined.
      */
     public void combine(final MatrixRotation3D rotation) {
         combine(this, rotation, this);
     }
 
     /**
      * Combines provided rotation into this rotation resulting in the
      * multiplication of the internal matrices of both rotations.
      *
      * @param rotation Input rotation to be combined.
      */
     @Override
     public void combine(final Rotation3D rotation) {
         if (rotation instanceof MatrixRotation3D matrixRotation) {
             combine(matrixRotation);
         } else {
             combine(new MatrixRotation3D(rotation));
         }
     }
 
     /**
      * Combines the rotation of instances rot1 and rot1 into provided result
      * instance.
      *
      * @param rot1   1st input rotation.
      * @param rot2   2nd input rotation.
      * @param result Combined rotation, which is equal to the multiplication of
      *               the internal matrix of provided rotation with the internal matrix of this
      *               instance.
      */
     public static void combine(
             final MatrixRotation3D rot1, final MatrixRotation3D rot2, final MatrixRotation3D result) {
         try {
             result.internalMatrix = rot1.internalMatrix.multiplyAndReturnNew(rot2.internalMatrix);
         } catch (final WrongSizeException ignore) {
             // never happens
         }
     }
 
     /**
      * Sets values of this rotation from a 3D matrix rotation.
      *
      * @param rot 3D matrix rotation to set values from.
      */
     @Override
     public void fromRotation(final MatrixRotation3D rot) {
         rot.internalMatrix.copyTo(internalMatrix);
     }
 
     /**
      * Sets values of this rotation from a quaternion.
      *
      * @param q a quaternion to set values from.
      */
     @Override
     public void fromRotation(final Quaternion q) {
         q.toMatrixRotation(internalMatrix);
     }
 
     /**
      * Converts this 3D rotation into a matrix rotation storing the result
      * into provided instance.
      *
      * @param result instance where result wil be stored.
      */
     @Override
     public void toMatrixRotation(final MatrixRotation3D result) {
         internalMatrix.copyTo(result.internalMatrix);
     }
 
     /**
      * Converts this 3D rotation into an axis rotation storing the result into
      * provided instance.
      *
      * @param result instance where result will be stored.
      */
     @Override
     public void toAxisRotation(final AxisRotation3D result) {
         try {
             result.fromInhomogeneousMatrix(internalMatrix);
         } catch (final InvalidRotationMatrixException ignore) {
             // never thrown
         }
     }
 
     /**
      * Converts this 3D rotation into a quaternion storing the result into
      * provided instance.
      *
      * @param result instance where result will be stored.
      */
     @Override
     public void toQuaternion(final Quaternion result) {
         Quaternion.matrixRotationToQuaternion(internalMatrix, result);
     }
 }