public void Decompose(IROMatrix <double> A)
{
// Initialize.
if (m == A.RowCount && n == A.ColumnCount)
{
MatrixMath.Copy(A, new JaggedArrayMatrix(QR, m, n));
//JaggedArrayMath.Copy(A, QR);
}
else
{
QR = JaggedArrayMath.GetMatrixCopy(A);
m = A.RowCount;
n = A.ColumnCount;
Rdiag = new double[n];
}
// Main loop.
for (int k = 0; k < n; k++)
{
// Compute 2-norm of k-th column without under/overflow.
double nrm = 0;
for (int i = k; i < m; i++)
{
nrm = RMath.Hypot(nrm, QR[i][k]);
}
if (nrm != 0.0)
{
// Form k-th Householder vector.
if (QR[k][k] < 0)
{
nrm = -nrm;
}
for (int i = k; i < m; i++)
{
QR[i][k] /= nrm;
}
QR[k][k] += 1.0;
// Apply transformation to remaining columns.
for (int j = k + 1; j < n; j++)
{
double s = 0.0;
for (int i = k; i < m; i++)
{
s += QR[i][k] * QR[i][j];
}
s = -s / QR[k][k];
for (int i = k; i < m; i++)
{
QR[i][j] += s * QR[i][k];
}
}
}
Rdiag[k] = -nrm;
}
}
/** Least squares solution of A*X = B
* @param B A Matrix with as many rows as A and any number of columns.
* @return X that minimizes the two norm of Q*R*X-B.
* @exception IllegalArgumentException Matrix row dimensions must agree.
* @exception RuntimeException Matrix is rank deficient.
*/
public void Solve(IROMatrix <double> B, IMatrix <double> result)
{
if (B.RowCount != m)
{
throw new ArgumentException("Matrix row dimensions must agree.");
}
if (!IsFullRank())
{
throw new Exception("Matrix is rank deficient.");
}
// Copy right hand side
int nx = B.ColumnCount;
double[][] X;
if (_solveMatrixWorkspace != null && _solveMatrixWorkspace.RowCount == B.RowCount && _solveMatrixWorkspace.ColumnCount == B.ColumnCount)
{
X = _solveMatrixWorkspace.Array;
MatrixMath.Copy(B, _solveMatrixWorkspace);
}
else
{
X = JaggedArrayMath.GetMatrixCopy(B);
_solveMatrixWorkspace = new JaggedArrayMatrix(X, B.RowCount, B.ColumnCount);
}
// Compute Y = transpose(Q)*B
for (int k = 0; k < n; k++)
{
for (int j = 0; j < nx; j++)
{
double s = 0.0;
for (int i = k; i < m; i++)
{
s += QR[i][k] * X[i][j];
}
s = -s / QR[k][k];
for (int i = k; i < m; i++)
{
X[i][j] += s * QR[i][k];
}
}
}
// Solve R*X = Y;
for (int k = n - 1; k >= 0; k--)
{
for (int j = 0; j < nx; j++)
{
X[k][j] /= Rdiag[k];
}
for (int i = 0; i < k; i++)
{
for (int j = 0; j < nx; j++)
{
X[i][j] -= X[k][j] * QR[i][k];
}
}
}
MatrixMath.Submatrix(_solveMatrixWorkspace, result, 0, 0);
}