| /* |
| * Licensed to the Apache Software Foundation (ASF) under one or more |
| * contributor license agreements. See the NOTICE file distributed with |
| * this work for additional information regarding copyright ownership. |
| * The ASF licenses this file to You 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 org.apache.sis.referencing.operation.matrix; |
| |
| import org.opengis.referencing.operation.Matrix; |
| import org.apache.sis.internal.referencing.Resources; |
| import org.apache.sis.internal.util.DoubleDouble; |
| import org.apache.sis.util.ArraysExt; |
| |
| |
| /** |
| * Computes the value of <var>U</var> which solves {@code X} × <var>U</var> = {@code Y}. |
| * The {@link #solve(double[], Matrix, double[], int, int)} method in this class is adapted from the |
| * {@code LUDecomposition} class of the <a href="http://math.nist.gov/javanumerics/jama">JAMA matrix package</a>. |
| * JAMA is provided in the public domain. |
| * |
| * <p>This class implements the {@link Matrix} interface as an implementation convenience. |
| * This implementation details can be ignored.</p> |
| * |
| * @author JAMA team |
| * @author Martin Desruisseaux (Geomatys) |
| * @version 0.4 |
| * @since 0.4 |
| * @module |
| */ |
| @SuppressWarnings("CloneInNonCloneableClass") |
| final class Solver implements Matrix { // Not Cloneable, despite the clone() method. |
| /** |
| * The size of the (i, j, s) tuples used internally by {@link #solve(Matrix, Matrix, double[], int, int, boolean)} |
| * for storing information about the NaN values. |
| */ |
| private static final int TUPLE_SIZE = 3; |
| |
| /** |
| * A immutable identity matrix without defined size. |
| * This is used only for computing the inverse. |
| */ |
| private static final Matrix IDENTITY = new Solver(); |
| |
| /** |
| * For the {@link #IDENTITY} constant only. |
| */ |
| private Solver() { |
| } |
| |
| /** |
| * Returns {@code true} since this matrix is the identity matrix. |
| */ |
| @Override |
| public boolean isIdentity() { |
| return true; |
| } |
| |
| /** |
| * Returns 1 for elements on the diagonal, 0 otherwise. |
| * This method never thrown exception. |
| */ |
| @Override |
| public double getElement(final int j, final int i) { |
| return (j == i) ? 1 : 0; |
| } |
| |
| /** |
| * Unsupported operation since this matrix is immutable. |
| */ |
| @Override |
| public void setElement(int j, int i, double d) { |
| throw new UnsupportedOperationException(); |
| } |
| |
| /** |
| * Returns {@code this} since this matrix is immutable. |
| */ |
| @Override |
| @SuppressWarnings("CloneDoesntCallSuperClone") |
| public Matrix clone() { |
| return this; |
| } |
| |
| /** |
| * Arbitrarily returns 0. The actual value does not matter for the purpose of {@code Solver}. |
| */ |
| @Override |
| public int getNumRow() { |
| return 0; |
| } |
| |
| /** |
| * Arbitrarily returns 0. The actual value does not matter for the purpose of {@code Solver}. |
| */ |
| @Override |
| public int getNumCol() { |
| return 0; |
| } |
| |
| /** |
| * Computes the inverse of the given matrix. This method shall be invoked only for square matrices. |
| * |
| * @param X the matrix to invert, which must be square. |
| * @param noChange if {@code true}, do not allow modifications to the {@code X} matrix. |
| * @throws NoninvertibleMatrixException if the {@code X} matrix is not square or singular. |
| */ |
| static MatrixSIS inverse(final Matrix X, final boolean noChange) throws NoninvertibleMatrixException { |
| final int size = X.getNumRow(); |
| final int numCol = X.getNumCol(); |
| if (numCol != size) { |
| throw new NoninvertibleMatrixException(Resources.format(Resources.Keys.NonInvertibleMatrix_2, size, numCol)); |
| } |
| return solve(X, IDENTITY, null, size, size, noChange); |
| } |
| |
| /** |
| * Solves {@code X} × <var>U</var> = {@code Y}. |
| * This method is an adaptation of the {@code LUDecomposition} class of the JAMA matrix package. |
| * |
| * @param X the matrix to invert. |
| * @param Y the desired result of {@code X} × <var>U</var>. |
| * @throws NoninvertibleMatrixException if the {@code X} matrix is not square or singular. |
| */ |
| static MatrixSIS solve(final Matrix X, final Matrix Y) throws NoninvertibleMatrixException { |
| final int size = X.getNumRow(); |
| final int numCol = X.getNumCol(); |
| if (numCol != size) { |
| throw new NoninvertibleMatrixException(Resources.format(Resources.Keys.NonInvertibleMatrix_2, size, numCol)); |
| } |
| final int innerSize = Y.getNumCol(); |
| GeneralMatrix.ensureNumRowMatch(size, Y.getNumRow(), innerSize); |
| double[] eltY = null; |
| if (Y instanceof GeneralMatrix) { |
| eltY = ((GeneralMatrix) Y).elements; |
| if (eltY.length == size * innerSize) { |
| eltY = null; // Matrix does not contains error terms. |
| } |
| } |
| return solve(X, Y, eltY, size, innerSize, true); |
| } |
| |
| /** |
| * Implementation of {@code solve} and {@code inverse} methods, with filtering of NaN values. |
| * This method searches for NaN values before to attempt solving or inverting the matrix. |
| * If some NaN values are found but the matrix is written in such a way that each NaN value |
| * is used for exactly one coordinate value (i.e. a matrix row is used for a one-dimensional |
| * conversion which is independent of all other dimensions), then we will edit the matrix in |
| * such a way that this NaN value does not prevent the inverse matrix to be computed. |
| * |
| * <p>This method does <strong>not</strong> checks the matrix size. |
| * Check for matrix size shall be performed by the caller like below:</p> |
| * |
| * {@preformat java |
| * final int size = X.getNumRow(); |
| * if (X.getNumCol() != size) { |
| * throw new NoninvertibleMatrixException("Matrix must be square."); |
| * } |
| * if (Y.getNumRow() != size) { |
| * throw new MismatchedMatrixSizeException("Matrix row dimensions must agree."); |
| * } |
| * } |
| * |
| * @param X the matrix to invert, which must be square. |
| * @param Y the desired result of {@code X} × <var>U</var>. |
| * @param eltY elements and error terms of the {@code Y} matrix, or {@code null} if not available. |
| * @param size the value of {@code X.getNumRow()}, {@code X.getNumCol()} and {@code Y.getNumRow()}. |
| * @param innerSize the value of {@code Y.getNumCol()}. |
| * @param noChange if {@code true}, do not allow modifications to the {@code X} matrix. |
| * @throws NoninvertibleMatrixException if the {@code X} matrix is not square or singular. |
| */ |
| private static MatrixSIS solve(final Matrix X, final Matrix Y, final double[] eltY, |
| final int size, final int innerSize, final boolean noChange) throws NoninvertibleMatrixException |
| { |
| assert (X.getNumRow() == size && X.getNumCol() == size) : size; |
| assert (Y.getNumRow() == size && Y.getNumCol() == innerSize) || (Y instanceof Solver); |
| final double[] LU = GeneralMatrix.getExtendedElements(X, size, size, noChange); |
| final int lastRowOrColumn = size - 1; |
| /* |
| * indexOfNaN array will be created only if at least one NaN value is found, and those NaN met |
| * the conditions documented in the code below. In such case, the array will contain a sequence |
| * of (i,j,s) where (i,j) are the indices where the NaN value has been found and s is the column |
| * of the scale factor. |
| */ |
| int[] indexOfNaN = null; |
| int indexCount = 0; |
| if ((X instanceof MatrixSIS) ? ((MatrixSIS) X).isAffine() : MatrixSIS.isAffine(X)) { // Avoid dependency to Matrices class. |
| /* |
| * Conservatively search for NaN values only if the matrix looks like an affine transform. |
| * If the matrix is affine, then we will assume that we can interpret the last column as |
| * translation terms and other columns as scale factors. |
| * |
| * Note: the iteration below skips the last row, since it is (0, 0, ..., 1). |
| */ |
| searchNaN: for (int flatIndex = (size - 1) * size; --flatIndex >= 0;) { |
| if (Double.isNaN(LU[flatIndex])) { |
| final int j = flatIndex / size; |
| final int i = flatIndex % size; |
| /* |
| * Found a NaN value. First, if we are not in the translation column, ensure |
| * that the column contains only zero values except on the current line. |
| */ |
| int columnOfScale = -1; |
| if (i != lastRowOrColumn) { // Enter only if this column is not for translations. |
| columnOfScale = i; // The non-translation element is the scale factor. |
| for (int k=lastRowOrColumn; --k>=0;) { // Scan all other rows in the current column. |
| if (k != j && LU[k*size + i] != 0) { |
| /* |
| * Found a non-zero element in the current column. |
| * We can not proceed - cancel everything. |
| */ |
| indexOfNaN = null; |
| indexCount = 0; |
| break searchNaN; |
| } |
| } |
| } |
| /* |
| * Next, ensure that the row contains only zero elements except for |
| * the scale factor and the offset (the element in the translation |
| * column, which is not checked by the loop below). |
| */ |
| for (int k=lastRowOrColumn; --k>=0;) { |
| if (k != i && LU[j*size + k] != 0) { |
| if (columnOfScale >= 0) { |
| /* |
| * If there is more than 1 non-zero element, |
| * abandon the attempt to handle NaN values. |
| */ |
| indexOfNaN = null; |
| indexCount = 0; |
| break searchNaN; |
| } |
| columnOfScale = k; |
| } |
| } |
| /* |
| * At this point, the NaN element has been determined as replaceable. |
| * Remember its index; the replacement will be performed later. |
| */ |
| if (indexOfNaN == null) { |
| indexOfNaN = new int[lastRowOrColumn * (2*TUPLE_SIZE)]; // At most one scale and one offset per row. |
| } |
| indexOfNaN[indexCount++] = i; |
| indexOfNaN[indexCount++] = j; |
| indexOfNaN[indexCount++] = columnOfScale; // May be -1 (while uncommon) |
| assert (indexCount % TUPLE_SIZE) == 0; |
| } |
| } |
| /* |
| * If there is any NaN value to edit, replace them by 0 if they appear in the translation column |
| * or by 1 otherwise (scale or shear). We perform this replacement after the loop searching for |
| * NaN, not inside the loop, in order to not change anything if the search has been canceled. |
| */ |
| for (int k=0; k<indexCount; k += TUPLE_SIZE) { |
| final int i = indexOfNaN[k ]; |
| final int j = indexOfNaN[k+1]; |
| final int flatIndex = j*size + i; |
| LU[flatIndex] = (i == lastRowOrColumn) ? 0 : 1; |
| LU[flatIndex + size*size] = 0; // Error term (see 'errorLU') in next method. |
| } |
| } |
| /* |
| * Now apply the inversion. |
| */ |
| final MatrixSIS matrix = solve(LU, Y, eltY, size, innerSize); |
| /* |
| * At this point, the matrix has been inverted. If they were any NaN value in the original |
| * matrix, set the corresponding scale factor and offset to NaN in the resulting matrix. |
| */ |
| for (int k=0; k<indexCount;) { |
| assert (k % TUPLE_SIZE) == 0; |
| final int i = indexOfNaN[k++]; |
| final int j = indexOfNaN[k++]; |
| final int s = indexOfNaN[k++]; |
| if (i != lastRowOrColumn) { |
| // Found a scale factor to set to NaN. |
| matrix.setElement(i, j, Double.NaN); // Note that i,j indices are interchanged. |
| if (matrix.getElement(i, lastRowOrColumn) != 0) { |
| matrix.setElement(i, lastRowOrColumn, Double.NaN); // = -offset/scale, so 0 stay 0. |
| } |
| } else if (s >= 0) { |
| // Found a translation factory to set to NaN. |
| matrix.setElement(s, lastRowOrColumn, Double.NaN); |
| } |
| } |
| return matrix; |
| } |
| |
| /** |
| * Implementation of {@code solve} and {@code inverse} methods. |
| * This method contains the code ported from the JAMA package. |
| * Use a "left-looking", dot-product, Crout/Doolittle algorithm. |
| * |
| * <p>This method does <strong>not</strong> checks the matrix size. |
| * It is caller's responsibility to ensure that the following hold:</p> |
| * |
| * {@preformat java |
| * X.getNumRow() == size; |
| * X.getNumCol() == size; |
| * Y.getNumRow() == size; |
| * Y.getNumCol() == innerSize; |
| * } |
| * |
| * @param LU elements of the {@code X} matrix to invert, including error terms. |
| * @param Y the desired result of {@code X} × <var>U</var>. |
| * @param eltY elements and error terms of the {@code Y} matrix, or {@code null} if not available. |
| * @param size the value of {@code X.getNumRow()}, {@code X.getNumCol()} and {@code Y.getNumRow()}. |
| * @param innerSize the value of {@code Y.getNumCol()}. |
| * @throws NoninvertibleMatrixException if the {@code X} matrix is not square or singular. |
| */ |
| private static MatrixSIS solve(final double[] LU, final Matrix Y, final double[] eltY, |
| final int size, final int innerSize) throws NoninvertibleMatrixException |
| { |
| final int errorLU = size * size; |
| assert errorLU == GeneralMatrix.indexOfErrors(size, size, LU); |
| final int[] pivot = ArraysExt.range(0, size); |
| final double[] column = new double[size * 2]; // [0 … size-1] : column values; [size … 2*size-1] : error terms. |
| final DoubleDouble acc = new DoubleDouble(); // Temporary variable for sum ("accumulator") and subtraction. |
| final DoubleDouble rat = new DoubleDouble(); // Temporary variable for products and ratios. |
| for (int i=0; i<size; i++) { |
| /* |
| * Make a copy of the i-th column. |
| */ |
| for (int j=0; j<size; j++) { |
| final int k = j*size + i; |
| column[j] = LU[k]; // Value |
| column[j + size] = LU[k + errorLU]; // Error |
| } |
| /* |
| * Apply previous transformations. This part is equivalent to the following code, |
| * but using double-double arithmetic instead than the primitive 'double' type: |
| * |
| * double sum = 0; |
| * for (int k=0; k<kmax; k++) { |
| * sum += LU[rowOffset + k] * column[k]; |
| * } |
| * LU[rowOffset + i] = (column[j] -= sum); |
| */ |
| for (int j=0; j<size; j++) { |
| final int rowOffset = j*size; |
| final int kmax = Math.min(j,i); |
| acc.clear(); |
| for (int k=0; k<kmax; k++) { |
| rat.setFrom(LU, rowOffset + k, errorLU); |
| rat.multiply(column, k, size); |
| acc.add(rat); |
| } |
| acc.subtract(column, j, size); |
| acc.negate(); |
| acc.storeTo(column, j, size); |
| acc.storeTo(LU, rowOffset + i, errorLU); |
| } |
| /* |
| * Find pivot and exchange if necessary. There is no floating-point arithmetic here |
| * (ignoring the comparison for magnitude order), only work on index values. |
| */ |
| int p = i; |
| for (int j=i; ++j < size;) { |
| if (Math.abs(column[j]) > Math.abs(column[p])) { |
| p = j; |
| } |
| } |
| if (p != i) { |
| final int pRow = p*size; |
| final int iRow = i*size; |
| for (int k=0; k<size; k++) { // Swap two full rows. |
| DoubleDouble.swap(LU, pRow + k, iRow + k, errorLU); |
| } |
| ArraysExt.swap(pivot, p, i); |
| } |
| /* |
| * Compute multipliers. This part is equivalent to the following code, but |
| * using double-double arithmetic instead than the primitive 'double' type: |
| * |
| * final double sum = LU[i*size + i]; |
| * if (sum != 0.0) { |
| * for (int j=i; ++j < size;) { |
| * LU[j*size + i] /= sum; |
| * } |
| * } |
| */ |
| acc.setFrom(LU, i*size + i, errorLU); |
| if (!acc.isZero()) { |
| for (int j=i; ++j < size;) { |
| final int t = j*size + i; |
| rat.setFrom(acc); |
| rat.inverseDivide(LU, t, errorLU); |
| rat.storeTo (LU, t, errorLU); |
| } |
| } |
| } |
| /* |
| * At this point, we are done computing LU. |
| * Ensure that the matrix is not singular. |
| */ |
| for (int j=0; j<size; j++) { |
| rat.setFrom(LU, j*size + j, errorLU); |
| if (rat.isZero()) { |
| throw new NoninvertibleMatrixException(Resources.format(Resources.Keys.SingularMatrix)); |
| } |
| } |
| /* |
| * Copy right hand side with pivoting. Write the result directly in the elements array |
| * of the result matrix. This block does not perform floating-point arithmetic operations. |
| */ |
| final GeneralMatrix result = GeneralMatrix.createExtendedPrecision(size, innerSize, false); |
| final double[] elements = result.elements; |
| final int errorOffset = size * innerSize; |
| for (int k=0,j=0; j<size; j++) { |
| final int p = pivot[j]; |
| for (int i=0; i<innerSize; i++) { |
| if (eltY != null) { |
| final int t = p*innerSize + i; |
| elements[k] = eltY[t]; |
| elements[k + errorOffset] = eltY[t + errorOffset]; |
| } else { |
| elements[k] = Y.getElement(p, i); |
| } |
| k++; |
| } |
| } |
| /* |
| * Solve L*Y = B(pivot, :). The inner block is equivalent to the following line, |
| * but using double-double arithmetic instead of 'double' primitive type: |
| * |
| * elements[loRowOffset + i] -= (elements[rowOffset + i] * LU[luRowOffset + k]); |
| */ |
| for (int k=0; k<size; k++) { |
| final int rowOffset = k*innerSize; // Offset of row computed by current iteration. |
| for (int j=k; ++j < size;) { |
| final int loRowOffset = j*innerSize; // Offset of some row after the current row. |
| final int luRowOffset = j*size; // Offset of the corresponding row in the LU matrix. |
| for (int i=0; i<innerSize; i++) { |
| acc.setFrom (elements, loRowOffset + i, errorOffset); |
| rat.setFrom (elements, rowOffset + i, errorOffset); |
| rat.multiply(LU, luRowOffset + k, errorLU); |
| acc.subtract(rat); |
| acc.storeTo (elements, loRowOffset + i, errorOffset); |
| } |
| } |
| } |
| /* |
| * Solve U*X = Y. The content of the loop is equivalent to the following line, |
| * but using double-double arithmetic instead of 'double' primitive type: |
| * |
| * double sum = LU[k*size + k]; |
| * for (int i=0; i<innerSize; i++) { |
| * elements[rowOffset + i] /= sum; |
| * } |
| * for (int j=0; j<k; j++) { |
| * sum = LU[j*size + k]; |
| * for (int i=0; i<innerSize; i++) { |
| * elements[upRowOffset + i] -= (elements[rowOffset + i] * sum); |
| * } |
| * } |
| */ |
| for (int k=size; --k >= 0;) { |
| final int rowOffset = k*innerSize; // Offset of row computed by current iteration. |
| acc.setFrom(LU, k*size + k, errorLU); // A diagonal element on the current row. |
| for (int i=0; i<innerSize; i++) { // Apply to all columns in the current row. |
| rat.setFrom(acc); |
| rat.inverseDivide(elements, rowOffset + i, errorOffset); |
| rat.storeTo (elements, rowOffset + i, errorOffset); |
| } |
| for (int j=0; j<k; j++) { |
| final int upRowOffset = j*innerSize; // Offset of a row before (locate upper) the current row. |
| acc.setFrom(LU, j*size + k, errorLU); // Same column than the diagonal element, but in the upper row. |
| for (int i=0; i<innerSize; i++) { // Apply to all columns in the upper row. |
| rat.setFrom(elements, rowOffset + i, errorOffset); |
| rat.multiply(acc); |
| rat.subtract(elements, upRowOffset + i, errorOffset); |
| rat.negate(); |
| rat.storeTo(elements, upRowOffset + i, errorOffset); |
| } |
| } |
| } |
| return result; |
| } |
| } |
