// The linear system is U*X = L^{-1}*B. Reduce this to
// X = U^{-1}*L^{-1}*B. The return value is 'true' iff the operation
// is successful. See the comments for SolveSystem(double*,int) about
// the storage for dataMatrix.
private bool SolveUpper(ref double[] dataMatrix, int numColumns)
{
LexicoArray2 data = new LexicoArray2(this.size, numColumns, dataMatrix);
for (int r = this.size - 1; r >= 0; r--)
{
double upperRR = this[r, r];
if (upperRR > 0.0)
{
for (int c = r + 1; c < this.size; c++)
{
double upperRC = this[r, c];
for (int bCol = 0; bCol < numColumns; bCol++)
{
data[r, bCol] -= upperRC * data[c, bCol];
}
}
double inverse = 1.0 / upperRR;
for (int bCol = 0; bCol < numColumns; bCol++)
{
data[r, bCol] *= inverse;
}
}
else
{
return(false);
}
}
return(true);
}
// The linear system is L*U*X = B, where A = L*U and U = L^T, Reduce
// this to U*X = L^{-1}*B. The return value is 'true' iff the
// operation is successful. See the comments for
// SolveSystem(double*,int) about the storage for dataMatrix.
private bool SolveLower(ref double[] dataMatrix, int numColumns)
{
LexicoArray2 data = new LexicoArray2(this.size, numColumns, dataMatrix);
for (int r = 0; r < this.size; r++)
{
double lowerRR = this[r, r];
if (lowerRR > 0.0)
{
for (int c = 0; c < r; c++)
{
double lowerRC = this[r, c];
for (int bCol = 0; bCol < numColumns; bCol++)
{
data[r, bCol] -= lowerRC * data[c, bCol];
}
}
double inverse = 1.0 / lowerRR;
for (int bCol = 0; bCol < numColumns; bCol++)
{
data[r, bCol] *= inverse;
}
}
else
{
return(false);
}
}
return(true);
}
// Compute the inverse of the banded matrix. The return value is
// 'true' when the matrix is invertible, in which case the 'inverse'
// output is valid. The return value is 'false' when the matrix is
// not invertible, in which case 'inverse' is invalid and should not
// be used. The input matrix 'inverse' must be the same size as
// 'this'.
//
// 'bMatrix' must have the storage order specified by the template
// parameter.
public bool ComputeInverse(double[] inverse)
{
LexicoArray2 invA = new LexicoArray2(this.size, this.size, inverse);
BandedMatrix tmpA = this;
for (int row = 0; row < this.size; row++)
{
for (int col = 0; col < this.size; col++)
{
if (row != col)
{
invA[row, col] = 0.0;
}
else
{
invA[row, row] = 1.0;
}
}
}
// Forward elimination.
for (int row = 0; row < this.size; row++)
{
// The pivot must be nonzero in order to proceed.
double diag = tmpA[row, row];
if (Math.Abs(diag) < double.Epsilon)
{
return(false);
}
double invDiag = 1.0 / diag;
tmpA[row, row] = 1.0;
// Multiply the row to be consistent with diagonal term of 1.
int colMin = row + 1;
int colMax = colMin + this.uBands.Length;
if (colMax > this.size)
{
colMax = this.size;
}
int c;
for (c = colMin; c < colMax; c++)
{
tmpA[row, c] *= invDiag;
}
for (c = 0; c <= row; c++)
{
invA[row, c] *= invDiag;
}
// Reduce the remaining rows.
int rowMin = row + 1;
int rowMax = rowMin + this.lBands.Length;
if (rowMax > this.size)
{
rowMax = this.size;
}
for (int r = rowMin; r < rowMax; r++)
{
double mult = tmpA[r, row];
tmpA[r, row] = 0.0;
for (c = colMin; c < colMax; c++)
{
tmpA[r, c] -= mult * tmpA[row, c];
}
for (c = 0; c <= row; c++)
{
invA[r, c] -= mult * invA[row, c];
}
}
}
// Backward elimination.
for (int row = this.size - 1; row >= 1; row--)
{
int rowMax = row - 1;
int rowMin = row - this.uBands.Length;
if (rowMin < 0)
{
rowMin = 0;
}
for (int r = rowMax; r >= rowMin; r--)
{
double mult = tmpA[r, row];
tmpA[r, row] = 0.0;
for (int c = 0; c < this.size; c++)
{
invA[r, c] -= mult * invA[row, c];
}
}
}
return(true);
}
public static bool Solve(int numRows, double[] M, double[] inverseM, out double determinant, double[] B, double[] X, double[] C, int numCols, double[] Y)
{
if (numRows <= 0 || M == null || ((B != null) != (X != null)) || ((C != null) != (Y != null)) || (C != null && numCols < 1))
{
Debug.Assert(false, "Invalid input.");
}
int numElements = numRows * numRows;
bool wantInverse = inverseM != null;
//if (!wantInverse)
//{
// inverseM = new double[numElements];
//}
Set(numElements, M, out inverseM);
if (B != null)
{
Set(numRows, B, out X);
}
if (C != null)
{
Set(numRows * numCols, C, out Y);
}
LexicoArray2 matInvM = new LexicoArray2(numRows, numRows, inverseM);
LexicoArray2 matY = new LexicoArray2(numRows, numCols, Y);
int[] colIndex = new int[numRows];
int[] rowIndex = new int[numRows];
bool[] pivoted = new bool[numRows];
const double zero = 0.0;
const double one = 1.0;
bool odd = false;
determinant = one;
// Elimination by full pivoting.
int i1, i2, row = 0, col = 0;
for (int i0 = 0; i0 < numRows; i0++)
{
// Search matrix (excluding pivoted rows) for maximum absolute entry.
double maxValue = zero;
for (i1 = 0; i1 < numRows; i1++)
{
if (!pivoted[i1])
{
for (i2 = 0; i2 < numRows; i2++)
{
if (!pivoted[i2])
{
double value = matInvM[i1, i2];
double absValue = (value >= zero ? value : -value);
if (absValue > maxValue)
{
maxValue = absValue;
row = i1;
col = i2;
}
}
}
}
}
if (Math.Abs(maxValue - zero) < double.Epsilon)
{
// The matrix is not invertible.
if (wantInverse)
{
Set(numElements, null, out inverseM);
}
determinant = zero;
if (B != null)
{
Set(numRows, null, out X);
}
if (C != null)
{
Set(numRows * numCols, null, out Y);
}
return(false);
}
pivoted[col] = true;
// Swap rows so that the pivot entry is in row 'col'.
if (row != col)
{
odd = !odd;
for (int i = 0; i < numRows; i++)
{
double tmp = matInvM[row, i];
matInvM[row, i] = matInvM[col, i];
matInvM[col, i] = tmp;
}
if (B != null)
{
double tmp = X[row];
X[row] = X[col];
X[col] = tmp;
}
if (C != null)
{
for (int i = 0; i < numCols; i++)
{
double tmp = matY[row, i];
matY[row, i] = matY[col, i];
matY[col, i] = tmp;
}
}
}
// Keep track of the permutations of the rows.
rowIndex[i0] = row;
colIndex[i0] = col;
// Scale the row so that the pivot entry is 1.
double diagonal = matInvM[col, col];
determinant *= diagonal;
double inv = one / diagonal;
matInvM[col, col] = one;
for (i2 = 0; i2 < numRows; i2++)
{
matInvM[col, i2] *= inv;
}
if (B != null)
{
X[col] *= inv;
}
if (C != null)
{
for (i2 = 0; i2 < numCols; i2++)
{
matY[col, i2] *= inv;
}
}
// Zero out the pivot column locations in the other rows.
for (i1 = 0; i1 < numRows; i1++)
{
if (i1 != col)
{
double save = matInvM[i1, col];
matInvM[i1, col] = zero;
for (i2 = 0; i2 < numRows; i2++)
{
matInvM[i1, i2] -= matInvM[col, i2] * save;
}
if (B != null)
{
X[i1] -= X[col] * save;
}
if (C != null)
{
for (i2 = 0; i2 < numCols; i2++)
{
matY[i1, i2] -= matY[col, i2] * save;
}
}
}
}
}
if (wantInverse)
{
// Reorder rows to undo any permutations in Gaussian elimination.
for (i1 = numRows - 1; i1 >= 0; i1--)
{
if (rowIndex[i1] != colIndex[i1])
{
for (i2 = 0; i2 < numRows; i2++)
{
double tmp = matInvM[i2, rowIndex[i1]];
matInvM[i2, rowIndex[i1]] = matInvM[i2, colIndex[i1]];
matInvM[i2, colIndex[i1]] = tmp;
}
}
}
}
if (odd)
{
determinant = -determinant;
}
return(true);
}
