/*
    * 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.Matrix;
  import com.irurueta.algebra.WrongSizeException;
  
  /**
   * Utility methods related to rotations.
   */
  @SuppressWarnings("DuplicatedCode")
  public class RotationUtils {
  
      /**
       * Number of components of angular rates (angular speed).
       */
      public static final int N_ANGULAR_RATES = 3;
  
      /**
       * Size of skew symmetric matrix omega.
       */
      public static final int SKEW_MATRIX_SIZE = 4;
  
      /**
       * Constructor.
       */
      private RotationUtils() {
      }
  
      /**
       * Skew symmetric matrix omega from angular rates vector w.
       *
       * @param w1     angular rate from x-axis.
       * @param w2     angular rate from y-axis.
       * @param w3     angular rate from z-axis.
       * @param result instance where result will be stored.
       * @throws IllegalArgumentException if provided matrix is not 4x4.
       * @see <a href="https://github.com/joansola/slamtb">w2omega.m at https://github.com/joansola/slamtb</a>
       */
      public static void w2omega(final double w1, final double w2, final double w3, final Matrix result) {
          if (result.getRows() != SKEW_MATRIX_SIZE || result.getColumns() != SKEW_MATRIX_SIZE) {
              throw new IllegalArgumentException("result must be 4x4");
          }
  
          result.setElementAt(0, 0, 0.0);
          result.setElementAt(1, 0, w1);
          result.setElementAt(2, 0, w2);
          result.setElementAt(3, 0, w3);
  
          result.setElementAt(0, 1, -w1);
          result.setElementAt(1, 1, 0.0);
          result.setElementAt(2, 1, -w3);
          result.setElementAt(3, 1, w2);
  
          result.setElementAt(0, 2, -w2);
          result.setElementAt(1, 2, w3);
          result.setElementAt(2, 2, 0.0);
          result.setElementAt(3, 2, -w1);
  
          result.setElementAt(0, 3, -w3);
          result.setElementAt(1, 3, -w2);
          result.setElementAt(2, 3, w1);
          result.setElementAt(3, 3, 0.0);
      }
  
      /**
       * Skew symmetric matrix omega from angular rates vector w.
       *
       * @param w      array containing angular rates. Must have length 3.
       * @param result instance where result will be stored.
       * @throws IllegalArgumentException if provided matrix is not 4x4 or array w
       *                                  does not have length 3.
       * @see <a href="https://github.com/joansola/slamtb">w2omega.m at https://github.com/joansola/slamtb</a>
       */
      public static void w2omega(final double[] w, final Matrix result) {
          if (w.length != N_ANGULAR_RATES) {
              throw new IllegalArgumentException("w must have length 3");
          }
          w2omega(w[0], w[1], w[2], result);
      }
  
      /**
       * Skew symmetric matrix omega from angular rates vector w.
       *
       * @param w1 angular rate from x-axis.
       * @param w2 angular rate from y-axis.
      * @param w3 angular rate from z-axis.
      * @return a new skew symmetric matrix omega.
      * @see <a href="https://github.com/joansola/slamtb">w2omega.m at https://github.com/joansola/slamtb</a>
      */
     public static Matrix w2omega(final double w1, final double w2, final double w3) {
         Matrix result = null;
         try {
             result = new Matrix(SKEW_MATRIX_SIZE, SKEW_MATRIX_SIZE);
             w2omega(w1, w2, w3, result);
         } catch (final WrongSizeException ignore) {
             // never happens
         }
 
         return result;
     }
 
     /**
      * Skew symmetric matrix omega from angular rates vector w.
      *
      * @param w array containing angular rates. Must have length 3.
      * @return a new skew symmetric matrix omega.
      * @see <a href="https://github.com/joansola/slamtb">w2omega.m at https://github.com/joansola/slamtb</a>
      */
     public static Matrix w2omega(final double[] w) {
         if (w.length != N_ANGULAR_RATES) {
             throw new IllegalArgumentException("w must have length 3");
         }
         return w2omega(w[0], w[1], w[2]);
     }
 
     /**
      * Converts provided quaternion into Pi matrix.
      * Given a quaternion q = [a b c d]', and the angular rates vector
      * w = [p q r]' and omega = w2omega(w) a skew symmetric matrix, then the PI
      * matrix is the Jacobian respect to w, expressed as: PI = omega(w)*q.
      *
      * @param quaternion a quaternion.
      * @param result     matrix where resulting pi matrix is stored.
      * @throws IllegalArgumentException if provided result matrix is not 4x3.
      * @see <a href="https://github.com/joansola/slamtb">q2Pi.m at https://github.com/joansola/slamtb</a>
      */
     public static void quaternionToPiMatrix(final Quaternion quaternion, final Matrix result) {
         if (result.getRows() != Quaternion.N_PARAMS || result.getColumns() != Quaternion.N_ANGLES) {
             throw new IllegalArgumentException("result matrix must be 4x3");
         }
 
         final var a = quaternion.getA();
         final var b = quaternion.getB();
         final var c = quaternion.getC();
         final var d = quaternion.getD();
 
         result.setElementAt(0, 0, -b);
         result.setElementAt(1, 0, a);
         result.setElementAt(2, 0, d);
         result.setElementAt(3, 0, -c);
 
         result.setElementAt(0, 1, -c);
         result.setElementAt(1, 1, -d);
         result.setElementAt(2, 1, a);
         result.setElementAt(3, 1, b);
 
         result.setElementAt(0, 2, -d);
         result.setElementAt(1, 2, c);
         result.setElementAt(2, 2, -b);
         result.setElementAt(3, 2, a);
     }
 
     /**
      * Converts provided quaternion into Pi matrix.
      * Given a quaternion q = [a b c d]', and the angular rates vector
      * w = [p q r]' and omega = w2omega(w) a skew symmetric matrix, then the PI
      * matrix is the Jacobian respect to w, expressed as: PI = omega(w)*q.
      *
      * @param quaternion a quaternion.
      * @return pi matrix.
      * @see <a href="https://github.com/joansola/slamtb">q2Pi.m at https://github.com/joansola/slamtb</a>
      */
     public static Matrix quaternionToPiMatrix(final Quaternion quaternion) {
         Matrix m = null;
         try {
             m = new Matrix(Quaternion.N_PARAMS, Quaternion.N_ANGLES);
             quaternionToPiMatrix(quaternion, m);
         } catch (final WrongSizeException ignore) {
             // never thrown
         }
         return m;
     }
 
     /**
      * Converts provided quaternion into conjugated Pi matrix.
      * Given a quaternion q = [a b c d]', then the conjugated Pi matrix is the
      * Pi matrix of the conjugated quaternion [a -b -c -d]'.
      *
      * @param quaternion a quaternion.
      * @param result     matrix where resulting conjugated pi matrix is stored.
      * @throws IllegalArgumentException if provided result matrix is not 4x3.
      * @see <a href="https://github.com/joansola/slamtb">pi2pc.m at https://github.com/joansola/slamtb</a>
      */
     public static void quaternionToConjugatedPiMatrix(final Quaternion quaternion, final Matrix result) {
         quaternionToPiMatrix(quaternion.conjugateAndReturnNew(), result);
     }
 
     /**
      * Converts provided quaternion into conjugated Pi matrix.
      * Given a quaternion q = [a b c d]', then the conjugated Pi matrix is the
      * Pi matrix of the conjugated quaternion [a -b -c -d]'.
      *
      * @param quaternion a quaternion.
      * @return a matrix containing the conjugated pi matrix.
      * @see <a href="https://github.com/joansola/slamtb">pi2pc.m at https://github.com/joansola/slamtb</a>
      */
     public static Matrix quaternionToConjugatedPiMatrix(final Quaternion quaternion) {
         Matrix m = null;
         try {
             m = new Matrix(Quaternion.N_PARAMS, Quaternion.N_ANGLES);
             quaternionToConjugatedPiMatrix(quaternion, m);
         } catch (final WrongSizeException ignore) {
             // never thrown
         }
         return m;
     }
 
     /**
      * Computes conjugated pi matrix from pi matrix.
      *
      * @param pi     pi matrix used as input.
      * @param result instance where conjugated pi matrix is stored.
      * @throws IllegalArgumentException if provided matrices are not 4x3.
      * @see <a href="https://github.com/joansola/slamtb">pi2pc.m at https://github.com/joansola/slamtb</a>
      */
     public static void piMatrixToConjugatedPiMatrix(final Matrix pi, final Matrix result) {
         if (pi.getRows() != Quaternion.N_PARAMS || pi.getColumns() != Quaternion.N_ANGLES) {
             throw new IllegalArgumentException("pi must be 4x3");
         }
         if (result.getRows() != Quaternion.N_PARAMS || result.getColumns() != Quaternion.N_ANGLES) {
             throw new IllegalArgumentException("result must be 4x3");
         }
 
         result.copyFrom(pi);
 
         result.setElementAtIndex(0, -pi.getElementAtIndex(0));
         result.setElementAtIndex(2, -pi.getElementAtIndex(2));
         result.setElementAtIndex(3, -pi.getElementAtIndex(3));
         result.setElementAtIndex(4, -pi.getElementAtIndex(4));
         result.setElementAtIndex(5, -pi.getElementAtIndex(5));
         result.setElementAtIndex(7, -pi.getElementAtIndex(7));
         result.setElementAtIndex(8, -pi.getElementAtIndex(8));
         result.setElementAtIndex(9, -pi.getElementAtIndex(9));
         result.setElementAtIndex(10, -pi.getElementAtIndex(10));
     }
 
     /**
      * Computes conjugated pi matrix from pi matrix.
      *
      * @param pi pi matrix used as input.
      * @return conjugated pi matrix.
      * @throws IllegalArgumentException if provided input matrix is not 4x3.
      * @see <a href="https://github.com/joansola/slamtb">pi2pc.m at https://github.com/joansola/slamtb</a>
      */
     public static Matrix piMatrixToConjugatedPiMatrix(final Matrix pi) {
         Matrix m = null;
         try {
             m = new Matrix(Quaternion.N_PARAMS, Quaternion.N_ANGLES);
             piMatrixToConjugatedPiMatrix(pi, m);
         } catch (final WrongSizeException ignore) {
             // never thrown
         }
         return m;
     }
 
     /**
      * Rotates a point by using the rotation matrix obtained from a quaternion.
      *
      * @param q         quaternion containing rotation information.
      * @param point     array containing inhomogeneous 3D coordinates of a point to
      *                  be rotated.
      * @param result    array containing result of rotation.
      * @param jacobianQ jacobian wrt of quaternion. Must be 3x4.
      * @param jacobianP jacobian wrt of point. Must be 3x3.
      * @throws IllegalArgumentException if provided arrays of points or result
      *                                  don't have length 3, or if jacobian of quaternions is not 3x4 (if
      *                                  provided), or if jacobian of point is not 3x3 (if provided).
      * @see <a href="https://github.com/joansola/slamtb">Rp.m at https://github.com/joansola/slamtb</a>
      */
     public static void rotationMatrixTimesVector(
             final Quaternion q, final double[] point, final double[] result, final Matrix jacobianQ,
             final Matrix jacobianP) {
         if (point.length != Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {
             throw new IllegalArgumentException("point must have length 3");
         }
         if (result.length != Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {
             throw new IllegalArgumentException("result must have length 3");
         }
         if (jacobianQ != null && (jacobianQ.getRows() != Quaternion.N_ANGLES
                 || jacobianQ.getColumns() != Quaternion.N_PARAMS)) {
             throw new IllegalArgumentException("jacobian wrt of quaternion must be 3x4");
         }
         if (jacobianP != null && (jacobianP.getRows() != MatrixRotation3D.ROTATION3D_INHOM_MATRIX_ROWS
                 || jacobianP.getColumns() != MatrixRotation3D.ROTATION3D_INHOM_MATRIX_COLS)) {
             throw new IllegalArgumentException("jacobian wrt of point must be 3x3");
         }
 
         try {
             final var r = new Matrix(MatrixRotation3D.ROTATION3D_INHOM_MATRIX_ROWS,
                     MatrixRotation3D.ROTATION3D_INHOM_MATRIX_COLS);
             q.toMatrixRotation(r);
 
             if (jacobianP != null) {
                 jacobianP.copyFrom(r);
             }
             // multiply rotation matrix by point
             final var p = Matrix.newFromArray(point, true);
             r.multiply(p);
 
             // copy to result
             r.getSubmatrixAsArray(0, 0, r.getRows() - 1,
                     r.getColumns() - 1, result);
 
             if (jacobianQ != null) {
                 final var a = q.getA();
                 final var b = q.getB();
                 final var c = q.getC();
                 final var d = q.getD();
 
                 final var x = point[0];
                 final var y = point[1];
                 final var z = point[2];
 
                 final var axdycz = 2.0 * (a * x - d * y + c * z);
                 final var bxcydz = 2.0 * (b * x + c * y + d * z);
                 final var cxbyaz = 2.0 * (c * x - b * y - a * z);
                 final var dxaybz = 2.0 * (d * x + a * y - b * z);
 
                 jacobianQ.setElementAt(0, 0, axdycz);
                 jacobianQ.setElementAt(1, 0, dxaybz);
                 jacobianQ.setElementAt(2, 0, -cxbyaz);
 
                 jacobianQ.setElementAt(0, 1, bxcydz);
                 jacobianQ.setElementAt(1, 1, cxbyaz);
                 jacobianQ.setElementAt(2, 1, dxaybz);
 
                 jacobianQ.setElementAt(0, 2, -cxbyaz);
                 jacobianQ.setElementAt(1, 2, bxcydz);
                 jacobianQ.setElementAt(2, 2, -axdycz);
 
                 jacobianQ.setElementAt(0, 3, -dxaybz);
                 jacobianQ.setElementAt(1, 3, axdycz);
                 jacobianQ.setElementAt(2, 3, bxcydz);
             }
         } catch (final WrongSizeException ignore) {
             // never thrown
         }
     }
 
     /**
      * Rotates a point by using the rotation matrix obtained from a quaternion.
      *
      * @param q         quaternion containing rotation information.
      * @param point     array containing inhomogeneous 3D coordinates of a point to
      *                  be rotated.
      * @param jacobianQ jacobian wrt of quaternion. Must be 3x4.
      * @param jacobianP jacobian wrt of point. Must be 3x3.
      * @return array containing result of rotation as 3D inhomogeneous
      * coordinates.
      * @throws IllegalArgumentException if provided point array doesn't have
      *                                  length 3, or if jacobian of quaternions is not 3x4 (if provided), or if
      *                                  jacobian of point is not 3x3 (if provided).
      * @see <a href="https://github.com/joansola/slamtb">Rp.m at https://github.com/joansola/slamtb</a>
      */
     public static double[] rotationMatrixTimesVector(
             final Quaternion q, final double[] point, final Matrix jacobianQ, final Matrix jacobianP) {
         final var result = new double[Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH];
         rotationMatrixTimesVector(q, point, result, jacobianQ, jacobianP);
         return result;
     }
 
     /**
      * Rotates a point by using the rotation matrix obtained from a quaternion.
      *
      * @param q      quaternion containing rotation information.
      * @param point  array containing inhomogeneous 3D coordinates of a point to
      *               be rotated.
      * @param result array containing result of rotation.
      * @throws IllegalArgumentException if provided arrays of points or result
      *                                  don't have length 3.
      * @see <a href="https://github.com/joansola/slamtb">Rp.m at https://github.com/joansola/slamtb</a>
      */
     public static void rotationMatrixTimesVector(final Quaternion q, final double[] point, final double[] result) {
         rotationMatrixTimesVector(q, point, result, null, null);
     }
 
     /**
      * Rotates a point by using the rotation matrix obtained from a quaternion.
      *
      * @param q     quaternion containing rotation information.
      * @param point array containing inhomogeneous 3D coordinates of a point to
      *              be rotated.
      * @return array containing result of rotation as 3D inhomogeneous
      * coordinates.
      * @throws IllegalArgumentException if provided point array doesn't have
      *                                  length 3.
      * @see <a href="https://github.com/joansola/slamtb">Rp.m at https://github.com/joansola/slamtb</a>
      */
     public static double[] rotationMatrixTimesVector(final Quaternion q, final double[] point) {
         return rotationMatrixTimesVector(q, point, null, null);
     }
 
     /**
      * Rotates a 3D point by using the rotation matrix obtained from a
      * quaternion.
      *
      * @param q         quaternion containing rotation information.
      * @param point     3D point to be rotated.
      * @param result    result of rotation.
      * @param jacobianQ jacobian wrt of quaternion. Must be 3x4.
      * @param jacobianP jacobian wrt of point. Must be 3x3.
      * @throws IllegalArgumentException if jacobian of quaternions is not 3x4
      *                                  (if provided), or if jacobian of point is not 3x3 (if provided).
      * @see <a href="https://github.com/joansola/slamtb">Rp.m at https://github.com/joansola/slamtb</a>
      */
     public static void rotationMatrixTimesVector(
             final Quaternion q, final Point3D point, final Point3D result, final Matrix jacobianQ,
             final Matrix jacobianP) {
         final var coords = new double[]{point.getInhomX(), point.getInhomY(), point.getInhomZ()};
         final var rp = rotationMatrixTimesVector(q, coords, jacobianQ, jacobianP);
         result.setInhomogeneousCoordinates(rp[0], rp[1], rp[2]);
     }
 
     /**
      * Rotates a 3D point by using the rotation matrix obtained from a
      * quaternion.
      *
      * @param q         quaternion containing rotation information.
      * @param point     3D point to be rotated.
      * @param jacobianQ jacobian wrt of quaternion. Must be 3x4.
      * @param jacobianP jacobian wrt of point. Must be 3x3.
      * @return result of rotation.
      * @throws IllegalArgumentException if jacobian of quaternions is not 3x4.
      *                                  (if provided), of if jacobian of point is not 3x3 (if provided).
      * @see <a href="https://github.com/joansola/slamtb">Rp.m at https://github.com/joansola/slamtb</a>
      */
     public static Point3D rotationMatrixTimesVector(
             final Quaternion q, final Point3D point, final Matrix jacobianQ, final Matrix jacobianP) {
         final var result = Point3D.create();
         rotationMatrixTimesVector(q, point, result, jacobianQ, jacobianP);
         return result;
     }
 
     /**
      * Rotates a 3D point by using the rotation matrix obtained from a
      * quaternion.
      *
      * @param q      quaternion containing rotation information.
      * @param point  3D point to be rotated.
      * @param result result of rotation.
      * @see <a href="https://github.com/joansola/slamtb">Rp.m at https://github.com/joansola/slamtb</a>
      */
     public static void rotationMatrixTimesVector(final Quaternion q, final Point3D point, final Point3D result) {
         rotationMatrixTimesVector(q, point, result, null, null);
     }
 
     /**
      * Rotates a 3D point by using the rotation matrix obtained from a
      * quaternion.
      *
      * @param q     quaternion containing rotation information.
      * @param point 3D point to be rotated.
      * @return result of rotation.
      * @see <a href="https://github.com/joansola/slamtb">Rp.m at https://github.com/joansola/slamtb</a>
      */
     public static Point3D rotationMatrixTimesVector(final Quaternion q, final Point3D point) {
         return rotationMatrixTimesVector(q, point, null, null);
     }
 
     /**
      * Rotates a point by the inverse rotation obtained from a quaternion.
      *
      * @param q         quaternion containing rotation information.
      * @param point     array containing inhomogeneous 3D coordinates of a point to
      *                  be rotated.
      * @param result    array containing result of rotation.
      * @param jacobianQ jacobian wrt of quaternion. Must be 3x4.
      * @param jacobianP jacobian wrt of point. Must be 3x3.
      * @throws IllegalArgumentException if provided arrays of points or result
      *                                  don't have length 3, or if jacobian of quaternions is not 3x4 (if
      *                                  provided), or if jacobian of point is not 3x3 (if provided).
      * @see <a href="https://github.com/joansola/slamtb">Rtp.m at https://github.com/joansola/slamtb</a>
      */
     public static void transposedRotationMatrixTimesVector(
             final Quaternion q, final double[] point, final double[] result, final Matrix jacobianQ,
             final Matrix jacobianP) {
         if (point.length != Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {
             throw new IllegalArgumentException("point must have length 3");
         }
         if (result.length != Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH) {
             throw new IllegalArgumentException("result must have length 3");
         }
         if (jacobianQ != null && (jacobianQ.getRows() != Quaternion.N_ANGLES
                 || jacobianQ.getColumns() != Quaternion.N_PARAMS)) {
             throw new IllegalArgumentException("jacobian wrt of quaternion must be 3x4");
         }
         if (jacobianP != null && (jacobianP.getRows() != MatrixRotation3D.ROTATION3D_INHOM_MATRIX_ROWS
                 || jacobianP.getColumns() != MatrixRotation3D.ROTATION3D_INHOM_MATRIX_COLS)) {
             throw new IllegalArgumentException("jacobian wrt of point must be 3x3");
         }
 
         try {
             final var rt = new Matrix(MatrixRotation3D.ROTATION3D_INHOM_MATRIX_ROWS,
                     MatrixRotation3D.ROTATION3D_INHOM_MATRIX_COLS);
             q.toMatrixRotation(rt);
             // transpose rotation
             // the transpose of rotation is its inverse
             rt.transpose();
 
             if (jacobianP != null) {
                 jacobianP.copyFrom(rt);
             }
             // multiply rotation matrix by point
             final var p = Matrix.newFromArray(point, true);
             rt.multiply(p);
 
             // copy to result
             rt.getSubmatrixAsArray(0, 0, rt.getRows() - 1,
                     rt.getColumns() - 1, result);
 
             if (jacobianQ != null) {
                 final var a = q.getA();
                 final var b = q.getB();
                 final var c = q.getC();
                 final var d = q.getD();
 
                 final var x = p.getElementAtIndex(0);
                 final var y = p.getElementAtIndex(1);
                 final var z = p.getElementAtIndex(2);
 
                 final var axdycz = 2.0 * (a * x + d * y - c * z);
                 final var bxcydz = 2.0 * (b * x + c * y + d * z);
                 final var cxbyaz = 2.0 * (c * x - b * y + a * z);
                 final var dxaybz = 2.0 * (d * x - a * y - b * z);
 
                 jacobianQ.setElementAt(0, 0, axdycz);
                 jacobianQ.setElementAt(1, 0, -dxaybz);
                 jacobianQ.setElementAt(2, 0, cxbyaz);
 
                 jacobianQ.setElementAt(0, 1, bxcydz);
                 jacobianQ.setElementAt(1, 1, cxbyaz);
                 jacobianQ.setElementAt(2, 1, dxaybz);
 
                 jacobianQ.setElementAt(0, 2, -cxbyaz);
                 jacobianQ.setElementAt(1, 2, bxcydz);
                 jacobianQ.setElementAt(2, 2, axdycz);
 
                 jacobianQ.setElementAt(0, 3, -dxaybz);
                 jacobianQ.setElementAt(1, 3, -axdycz);
                 jacobianQ.setElementAt(2, 3, bxcydz);
             }
         } catch (final WrongSizeException ignore) {
             // never thrown
         }
     }
 
     /**
      * Rotates a point by the inverse rotation obtained from a quaternion.
      *
      * @param q         quaternion containing rotation information.
      * @param point     array containing inhomogeneous 3D coordinates of a point to
      *                  be rotated.
      * @param jacobianQ jacobian wrt of quaternion. Must be 3x4.
      * @param jacobianP jacobian wrt of point. Must be 3x3.
      * @return result of rotation.
      * @throws IllegalArgumentException if provided arrays of points doesn't
      *                                  have length 3, or if jacobian of quaternions is not 3x4 (if provided), or
      *                                  if jacobian of point is not 3x3 (if provided).
      * @see <a href="https://github.com/joansola/slamtb">Rtp.m at https://github.com/joansola/slamtb</a>
      */
     public static double[] transposedRotationMatrixTimesVector(
             final Quaternion q, final double[] point, final Matrix jacobianQ, final Matrix jacobianP) {
         final var result = new double[Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH];
         transposedRotationMatrixTimesVector(q, point, result, jacobianQ, jacobianP);
         return result;
     }
 
     /**
      * Rotates a point by the inverse rotation obtained from a quaternion.
      *
      * @param q      quaternion containing rotation information.
      * @param point  array containing inhomogeneous 3D coordinates of a point to
      *               be rotated.
      * @param result result of rotation.
      * @throws IllegalArgumentException if provided arrays of points doesn't
      *                                  have length 3.
      * @see <a href="https://github.com/joansola/slamtb">Rtp.m at https://github.com/joansola/slamtb</a>
      */
     public static void transposedRotationMatrixTimesVector(
             final Quaternion q, final double[] point, final double[] result) {
         transposedRotationMatrixTimesVector(q, point, result, null, null);
     }
 
     /**
      * Rotates a point by the inverse rotation obtained from a quaternion.
      *
      * @param q     quaternion containing rotation information.
      * @param point array containing inhomogeneous 3D coordinates of a point to
      *              be rotated.
      * @return array containing result of rotation as 3D inhomogeneous
      * coordinates.
      * @throws IllegalArgumentException if provided point array doesn't have
      *                                  length 3.
      * @see <a href="https://github.com/joansola/slamtb">Rtp.m at https://github.com/joansola/slamtb</a>
      */
     public static double[] transposedRotationMatrixTimesVector(final Quaternion q, final double[] point) {
         return transposedRotationMatrixTimesVector(q, point, null, null);
     }
 
     /**
      * Rotates a 3D point by the inverse rotation obtained from a quaternion.
      *
      * @param q         quaternion containing rotation information.
      * @param point     array containing inhomogeneous 3D coordinates of a point to
      *                  be rotated.
      * @param result    result of rotation.
      * @param jacobianQ jacobian wrt of quaternion. Must be 3x4.
      * @param jacobianP jacobian wrt of point. Must be 3x3.
      * @throws IllegalArgumentException if jacobian of quaternions is not 3x4
      *                                  (if provided), or if jacobian of point is not 3x3 (if provided).
      * @see <a href="https://github.com/joansola/slamtb">Rtp.m at https://github.com/joansola/slamtb</a>
      */
     public static void transposedRotationMatrixTimesVector(
             final Quaternion q, final Point3D point, final Point3D result, final Matrix jacobianQ,
             final Matrix jacobianP) {
         final var coords = new double[]{point.getInhomX(), point.getInhomY(), point.getInhomZ()};
         final var rp = transposedRotationMatrixTimesVector(q, coords, jacobianQ, jacobianP);
         result.setInhomogeneousCoordinates(rp[0], rp[1], rp[2]);
     }
 
     /**
      * Rotates a 3D point by the inverse rotation obtained from a quaternion.
      *
      * @param q         quaternion containing rotation information.
      * @param point     array containing inhomogeneous 3D coordinates of a point to
      *                  be rotated.
      * @param jacobianQ jacobian wrt of quaternion. Must be 3x4.
      * @param jacobianP jacobian wrt of point. Must be 3x3.
      * @return result of rotation.
      * @throws IllegalArgumentException if jacobian of quaternions is not 3x4
      *                                  (if provided), or if jacobian of point is not 3x3 (if provided).
      * @see <a href="https://github.com/joansola/slamtb">Rtp.m at https://github.com/joansola/slamtb</a>
      */
     public static Point3D transposedRotationMatrixTimesVector(
             final Quaternion q, final Point3D point, final Matrix jacobianQ, final Matrix jacobianP) {
         final var result = Point3D.create();
         transposedRotationMatrixTimesVector(q, point, result, jacobianQ, jacobianP);
         return result;
     }
 
     /**
      * Rotates a 3D point by the inverse rotation obtained from a quaternion.
      *
      * @param q      quaternion containing rotation information.
      * @param point  3D point to be rotated.
      * @param result result of rotation.
      * @see <a href="https://github.com/joansola/slamtb">Rtp.m at https://github.com/joansola/slamtb</a>
      */
     public static void transposedRotationMatrixTimesVector(
             final Quaternion q, final Point3D point, final Point3D result) {
         transposedRotationMatrixTimesVector(q, point, result, null, null);
     }
 
     /**
      * Rotates a 3D point by the inverse rotation obtained from a quaternion.
      *
      * @param q     quaternion containing rotation information.
      * @param point 3D point to be rotated.
      * @return result of rotation.
      * @see <a href="https://github.com/joansola/slamtb">Rtp.m at https://github.com/joansola/slamtb</a>
      */
     public static Point3D transposedRotationMatrixTimesVector(final Quaternion q, final Point3D point) {
         return transposedRotationMatrixTimesVector(q, point, null, null);
     }
 
     /**
      * Rotation to convert camera body to camera sensor.
      *
      * @param result instance where resulting rotation will be stored.
      */
     public static void cameraBodyToCameraSensorRotation(final MatrixRotation3D result) {
         Quaternion.eulerToMatrixRotation(-Math.PI / 2.0, 0, -Math.PI / 2.0, result);
     }
 
     /**
      * Rotation to convert camera body to camera sensor.
      *
      * @return a new instance containing rotation.
      */
     public static MatrixRotation3D cameraBodyToCameraSensorRotation() {
         final var result = new MatrixRotation3D();
         cameraBodyToCameraSensorRotation(result);
         return result;
     }
 
     /**
      * Transform quaternion Gaussian to Euler Gaussian.
      * The Gaussian quaternion is provided as N(q, Q) where q is the quaternion
      * mean and Q is the quaternion covariance, and returns an Euler angles
      * Gaussian as N(e,E) where e is the Euler angles mean and E is the Euler
      * angles covariance.
      *
      * @param q                    mean quaternion to be transformed.
      * @param quaternionCovariance quaternion covariance to be transformed.
      * @param angles               obtained mean Euler angles.
      * @param anglesCovariance     obtained Euler covariance.
      * @throws IllegalArgumentException if provided quaternion covariance is
      *                                  not 4x4
      * @see <a href="https://github.com/joansola/slamtb">q2eG.m at https://github.com/joansola/slamtb</a>
      */
     public static void quaternionToEulerGaussian(
             final Quaternion q, final Matrix quaternionCovariance, final double[] angles,
             final Matrix anglesCovariance) {
         if (quaternionCovariance.getRows() != Quaternion.N_PARAMS
                 || quaternionCovariance.getColumns() != Quaternion.N_PARAMS) {
             throw new IllegalArgumentException("quaternion covariance must be 4x4");
         }
 
         try {
             final var eq = new Matrix(Quaternion.N_ANGLES, Quaternion.N_PARAMS);
             q.toEulerAngles(angles, eq);
 
             // jacobian = eq * Q * eq', where is provided covariance
             anglesCovariance.copyFrom(eq);
             anglesCovariance.multiply(quaternionCovariance);
             anglesCovariance.multiply(eq.transposeAndReturnNew());
         } catch (final WrongSizeException ignore) {
             // never thrown
         }
     }
 
     /**
      * Obtains the skew symmetric matrix from angular rates vector.
      *
      * @param angularRates a vector containing the 3 components (x,y,z) of
      *                     angular rates (rad/s).
      * @param result       a skew symmetric matrix.
      * @throws IllegalArgumentException if provided array of angular rates does
      *                                  not have length 3 or if provided result matrix is not 4x4.
      */
     public static void angularRatesToSkew(final double[] angularRates, final Matrix result) {
         if (angularRates.length != Quaternion.N_ANGLES) {
             throw new IllegalArgumentException("angular rates must have length 3");
         }
         if (result.getRows() != Quaternion.N_PARAMS || result.getColumns() != Quaternion.N_PARAMS) {
             throw new IllegalArgumentException("result matrix must be 4x4");
         }
 
         result.setElementAt(0, 0, 0.0);
         result.setElementAt(1, 0, angularRates[0]);
         result.setElementAt(2, 0, angularRates[1]);
         result.setElementAt(3, 0, angularRates[2]);
 
         result.setElementAt(0, 1, -angularRates[0]);
         result.setElementAt(1, 1, 0.0);
         result.setElementAt(2, 1, -angularRates[2]);
         result.setElementAt(3, 1, angularRates[1]);
 
         result.setElementAt(0, 2, -angularRates[1]);
         result.setElementAt(1, 2, angularRates[2]);
         result.setElementAt(2, 2, 0.0);
         result.setElementAt(3, 2, -angularRates[0]);
 
         result.setElementAt(0, 3, -angularRates[2]);
         result.setElementAt(1, 3, -angularRates[1]);
         result.setElementAt(2, 3, angularRates[0]);
         result.setElementAt(3, 3, 0.0);
     }
 
     /**
      * Obtains the skew symmetric matrix from angular rates vector.
      *
      * @param angularRates a vector containing the 3 components (x,y,z) of
      *                     angular rates (rad/s).
      * @return a new instance containing the skew symmetric matrix
      * @throws IllegalArgumentException if provided array of angular rates does
      *                                  not have length 3.
      */
     public static Matrix angularRatesToSkew(final double[] angularRates) {
         Matrix result = null;
         try {
             result = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
             angularRatesToSkew(angularRates, result);
         } catch (final WrongSizeException ignore) {
             // never thrown
         }
         return result;
     }
 
 }