final int[] pivot = new int[size];
for (int j=0; j<size; j++) {
pivot[j] = j;
}
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(Errors.format(Errors.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);
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;
}