bool LinearSystem<Real>::SolveSymmetricCG (const GMatrix<Real>& A,
const Real* B, Real* X)
{
// Based on the algorithm in "Matrix Computations" by Golum and Van Loan.
assertion(A.GetNumRows() == A.GetNumColumns(), "Matrix must be square\n");
int size = A.GetNumRows();
Real* R = new1<Real>(size);
Real* P = new1<Real>(size);
Real* W = new1<Real>(size);
// The first iteration.
size_t numBytes = size*sizeof(Real);
memset(X, 0, numBytes);
memcpy(R, B, numBytes);
Real rho0 = Dot(size, R, R);
memcpy(P, R, numBytes);
Multiply(A, P, W);
Real alpha = rho0/Dot(size, P, W);
UpdateX(size, X, alpha, P);
UpdateR(size, R, alpha, W);
Real rho1 = Dot(size, R, R);
// The remaining iterations.
const int imax = 1024;
int i;
for (i = 1; i < imax; ++i)
{
Real root0 = Math<Real>::Sqrt(rho1);
Real norm = Dot(size, B, B);
Real root1 = Math<Real>::Sqrt(norm);
if (root0 <= ZeroTolerance*root1)
{
break;
}
Real beta = rho1/rho0;
UpdateP(size, P, beta, R);
Multiply(A, P, W);
alpha = rho1/Dot(size, P, W);
UpdateX(size, X, alpha, P);
UpdateR(size, R, alpha, W);
rho0 = rho1;
rho1 = Dot(size, R, R);
}
delete1(W);
delete1(P);
delete1(R);
return i < imax;
}
void LinearSystem<Real>::Multiply (const GMatrix<Real>& A, const Real* X,
Real* Prod)
{
int size = A.GetNumRows();
memset(Prod, 0, size*sizeof(Real));
for (int row = 0; row < size; ++row)
{
for (int col = 0; col < size; ++col)
{
Prod[row] += A[row][col]*X[col];
}
}
}
void SingularValueDecomposition<Real>::HouseholderPostmultiply (
const GVector<Real>& V, GMatrix<Real>& A)
{
GVector<Real> W = (((Real)-2)/V.SquaredLength())*(A*V);
int numRows = A.GetNumRows();
int numCols = A.GetNumColumns();
for (int row = 0; row < numRows; ++row)
{
for (int col = 0; col < numCols; ++col)
{
A[row][col] += W[row]*V[col];
}
}
}
SingularValueDecomposition<Real>::SingularValueDecomposition (
const GMatrix<Real>& M, GMatrix<Real>& L, GMatrix<Real>& D,
GMatrix<Real>& RTranspose)
{
// TODO. Implement other QR factorizations and SVD code from "Matrix
// Computations", and then give the user the ability to specify which
// methods are used here.
int numRows = M.GetNumRows();
int numCols = M.GetNumColumns();
L.SetSize(numRows, numRows);
D.SetSize(numRows, numCols);
RTranspose.SetSize(numCols, numCols);
GMatrix<Real> kMTM = M.TransposeTimes(M);
EigenDecomposition<Real> es(kMTM);
es.Solve(false);
GMatrix<Real> V = es.GetEigenvectors();
GMatrix<Real> MV = M*V;
HouseholderQR(MV, L, D);
RTranspose = V.Transpose();
}
bool LinearSystem<Real>::Inverse (const GMatrix<Real>& A,
GMatrix<Real>& invA)
{
// Computations are performed in-place.
assertion(A.GetNumRows() == A.GetNumColumns(), "Matrix must be square\n");
int size = invA.GetNumRows();
invA = A;
int* colIndex = new1<int>(size);
int* rowIndex = new1<int>(size);
bool* pivoted = new1<bool>(size);
memset(pivoted, 0, size*sizeof(bool));
int i1, i2, row = 0, col = 0;
Real save;
// Elimination by full pivoting.
for (int i0 = 0; i0 < size; ++i0)
{
// Search matrix (excluding pivoted rows) for maximum absolute entry.
Real maxValue = (Real)0;
for (i1 = 0; i1 < size; ++i1)
{
if (!pivoted[i1])
{
for (i2 = 0; i2 < size; ++i2)
{
if (!pivoted[i2])
{
Real absValue = Math<Real>::FAbs(invA[i1][i2]);
if (absValue > maxValue)
{
maxValue = absValue;
row = i1;
col = i2;
}
}
}
}
}
if (maxValue == (Real)0)
{
// The matrix is not invertible.
delete1(colIndex);
delete1(rowIndex);
delete1(pivoted);
return false;
}
pivoted[col] = true;
// Swap rows so that A[col][col] contains the pivot entry.
if (row != col)
{
invA.SwapRows(row,col);
}
// Keep track of the permutations of the rows.
rowIndex[i0] = row;
colIndex[i0] = col;
// Scale the row so that the pivot entry is 1.
Real inv = ((Real)1)/invA[col][col];
invA[col][col] = (Real)1;
for (i2 = 0; i2 < size; i2++)
{
invA[col][i2] *= inv;
}
// Zero out the pivot column locations in the other rows.
for (i1 = 0; i1 < size; ++i1)
{
if (i1 != col)
{
save = invA[i1][col];
invA[i1][col] = (Real)0;
for (i2 = 0; i2 < size; ++i2)
{
invA[i1][i2] -= invA[col][i2]*save;
}
}
}
}
// Reorder rows so that A[][] stores the inverse of the original matrix.
for (i1 = size-1; i1 >= 0; --i1)
{
if (rowIndex[i1] != colIndex[i1])
{
for (i2 = 0; i2 < size; ++i2)
{
save = invA[i2][rowIndex[i1]];
invA[i2][rowIndex[i1]] = invA[i2][colIndex[i1]];
invA[i2][colIndex[i1]] = save;
}
}
}
delete1(colIndex);
delete1(rowIndex);
delete1(pivoted);
return true;
}
void SingularValueDecomposition<Real>::HouseholderQR (
const GMatrix<Real>& A, GMatrix<Real>& Q, GMatrix<Real>& R)
{
// The matrix R gets a copy of A, and is then overwritten during the
// algorithm with the correct entries to be upper triangular.
R = A;
int numRows = R.GetNumRows();
int numCols = R.GetNumColumns();
assertion(numRows >= numCols, "Too many columns (use transpose)\n");
int row, col;
GVector<Real> V(numRows);
std::vector<GVector<Real> > VSave;
for (col = 0; col < numCols; ++col)
{
// Create the Householder vector for the partial column of A.
for (row = 0; row < col; ++row)
{
V[row] = (Real)0;
}
Real length = (Real)0;
for (row = col; row < numRows; ++row)
{
V[row] = R[row][col];
length += V[row]*V[row];
}
length = Math<Real>::Sqrt(length);
Real beta = V[col] + Math<Real>::Sign(V[col])*length;
if (beta != (Real)0)
{
Real invBeta = ((Real)1)/beta;
for (int i = col + 1; i < numRows; ++i)
{
V[i] *= invBeta;
}
}
V[col] = (Real)1;
// Premultiply A by the V-reflection matrix.
HouseholderPremultiply(V, R);
// Save the Householder vectors.
VSave.push_back(V);
}
// First, make Q the identity. Second, extract the Householder vectors
// and premultiply by the V-reflections to build Q.
memset(Q.GetElements(), 0, Q.GetNumElements()*sizeof(Real));
for (row = 0; row < numRows; ++row)
{
Q[row][row] = (Real)1;
}
for (col = numCols - 1; col >= 0; --col)
{
// Get the Householder vector.
V = VSave[col];
// Premultiply Q by the V-reflection matrix.
HouseholderPremultiply(V, Q);
}
}
