Java LocalizedFormats.CLOSEST_ORTHOGONAL_MATRIX_HAS_NEGATIVE_DETERMINANT属性代码示例

/** Build a rotation from a 3X3 matrix.

     * <p>Rotation matrices are orthogonal matrices, i.e. unit matrices
     * (which are matrices for which m.m<sup>T</sup> = I) with real
     * coefficients. The module of the determinant of unit matrices is
     * 1, among the orthogonal 3X3 matrices, only the ones having a
     * positive determinant (+1) are rotation matrices.</p>

     * <p>When a rotation is defined by a matrix with truncated values
     * (typically when it is extracted from a technical sheet where only
     * four to five significant digits are available), the matrix is not
     * orthogonal anymore. This constructor handles this case
     * transparently by using a copy of the given matrix and applying a
     * correction to the copy in order to perfect its orthogonality. If
     * the Frobenius norm of the correction needed is above the given
     * threshold, then the matrix is considered to be too far from a
     * true rotation matrix and an exception is thrown.<p>

     * @param m rotation matrix
     * @param threshold convergence threshold for the iterative
     * orthogonality correction (convergence is reached when the
     * difference between two steps of the Frobenius norm of the
     * correction is below this threshold)

     * @exception NotARotationMatrixException if the matrix is not a 3X3
     * matrix, or if it cannot be transformed into an orthogonal matrix
     * with the given threshold, or if the determinant of the resulting
     * orthogonal matrix is negative

     */
    public FieldRotation(final T[][] m, final double threshold)
        throws NotARotationMatrixException {

        // dimension check
        if ((m.length != 3) || (m[0].length != 3) ||
                (m[1].length != 3) || (m[2].length != 3)) {
            throw new NotARotationMatrixException(
                                                  LocalizedFormats.ROTATION_MATRIX_DIMENSIONS,
                                                  m.length, m[0].length);
        }

        // compute a "close" orthogonal matrix
        final T[][] ort = orthogonalizeMatrix(m, threshold);

        // check the sign of the determinant
        final T d0 = ort[1][1].multiply(ort[2][2]).subtract(ort[2][1].multiply(ort[1][2]));
        final T d1 = ort[0][1].multiply(ort[2][2]).subtract(ort[2][1].multiply(ort[0][2]));
        final T d2 = ort[0][1].multiply(ort[1][2]).subtract(ort[1][1].multiply(ort[0][2]));
        final T det =
                ort[0][0].multiply(d0).subtract(ort[1][0].multiply(d1)).add(ort[2][0].multiply(d2));
        if (det.getReal() < 0.0) {
            throw new NotARotationMatrixException(
                                                  LocalizedFormats.CLOSEST_ORTHOGONAL_MATRIX_HAS_NEGATIVE_DETERMINANT,
                                                  det);
        }

        final T[] quat = mat2quat(ort);
        q0 = quat[0];
        q1 = quat[1];
        q2 = quat[2];
        q3 = quat[3];

    }
 

/** Build a rotation from a 3X3 matrix.

   * <p>Rotation matrices are orthogonal matrices, i.e. unit matrices
   * (which are matrices for which m.m<sup>T</sup> = I) with real
   * coefficients. The module of the determinant of unit matrices is
   * 1, among the orthogonal 3X3 matrices, only the ones having a
   * positive determinant (+1) are rotation matrices.</p>

   * <p>When a rotation is defined by a matrix with truncated values
   * (typically when it is extracted from a technical sheet where only
   * four to five significant digits are available), the matrix is not
   * orthogonal anymore. This constructor handles this case
   * transparently by using a copy of the given matrix and applying a
   * correction to the copy in order to perfect its orthogonality. If
   * the Frobenius norm of the correction needed is above the given
   * threshold, then the matrix is considered to be too far from a
   * true rotation matrix and an exception is thrown.<p>

   * @param m rotation matrix
   * @param threshold convergence threshold for the iterative
   * orthogonality correction (convergence is reached when the
   * difference between two steps of the Frobenius norm of the
   * correction is below this threshold)

   * @exception NotARotationMatrixException if the matrix is not a 3X3
   * matrix, or if it cannot be transformed into an orthogonal matrix
   * with the given threshold, or if the determinant of the resulting
   * orthogonal matrix is negative

   */
  public Rotation(double[][] m, double threshold)
    throws NotARotationMatrixException {

    // dimension check
    if ((m.length != 3) || (m[0].length != 3) ||
        (m[1].length != 3) || (m[2].length != 3)) {
      throw new NotARotationMatrixException(
              LocalizedFormats.ROTATION_MATRIX_DIMENSIONS,
              m.length, m[0].length);
    }

    // compute a "close" orthogonal matrix
    double[][] ort = orthogonalizeMatrix(m, threshold);

    // check the sign of the determinant
    double det = ort[0][0] * (ort[1][1] * ort[2][2] - ort[2][1] * ort[1][2]) -
                 ort[1][0] * (ort[0][1] * ort[2][2] - ort[2][1] * ort[0][2]) +
                 ort[2][0] * (ort[0][1] * ort[1][2] - ort[1][1] * ort[0][2]);
    if (det < 0.0) {
      throw new NotARotationMatrixException(
              LocalizedFormats.CLOSEST_ORTHOGONAL_MATRIX_HAS_NEGATIVE_DETERMINANT,
              det);
    }

    double[] quat = mat2quat(ort);
    q0 = quat[0];
    q1 = quat[1];
    q2 = quat[2];
    q3 = quat[3];

  }