// performs backwards substitution on the linear system U*x = b, filling in the input Mat x
int BackSub(Mat &Umat, Mat &xvec, Mat &bvec) {
// check that matrix sizes match
if (Umat.Rows() != bvec.Rows() || Umat.Rows() != Umat.Cols() || bvec.Cols() != 1 ||
xvec.Rows() != Umat.Rows() || xvec.Cols() != 1) {
cerr << "BackSub error, illegal matrix/vector dimensions\n";
cerr << " Mat is " << Umat.Rows() << " x " << Umat.Cols()
<< ", rhs is " << bvec.Rows() << " x " << bvec.Cols()
<< ", solution is " << xvec.Rows() << " x " << xvec.Cols() << endl;
return 1;
}
// get the matrix size
long int n = Umat.Rows();
// access the data arrays
double *U = Umat.get_data();
double *x = xvec.get_data();
double *b = bvec.get_data();
// copy b into x
xvec = bvec;
// analyze matrix for typical nonzero magnitude
double Umax = Umat.MaxNorm();
// perform column-oriented Backwards Substitution algorithm
for (long int j=n-1; j>=0; j--) {
// check for nonzero matrix diagonal
if (fabs(U[IDX(j,j,n)]) < STOL*Umax) {
cerr << "BackSub error: numerically singular matrix!\n";
return 1;
}
// solve for this row of solution
x[j] /= U[IDX(j,j,n)];
// update all remaining rhs
for (long int i=0; i<j; i++)
x[i] -= U[IDX(i,j,n)]*x[j];
}
// return success
return 0;
}
// performs forwards substitution on the linear system L*x = b, filling in the input Mat x
int FwdSub(Mat &Lmat, Mat &xvec, Mat &bvec) {
// check that matrix sizes match
if (Lmat.Rows() != bvec.Rows() || Lmat.Rows() != Lmat.Cols() || bvec.Cols() != 1 ||
xvec.Rows() != Lmat.Rows() || xvec.Cols() != 1) {
cerr << "FwdSub error, illegal matrix/vector dimensions\n";
cerr << " Mat is " << Lmat.Rows() << " x " << Lmat.Cols()
<< ", rhs is " << bvec.Rows() << " x " << bvec.Cols()
<< ", solution is " << xvec.Rows() << " x " << xvec.Cols() << endl;
return 1;
}
// get the matrix size
long int n = Lmat.Rows();
// access the data arrays
double *L = Lmat.get_data();
double *x = xvec.get_data();
double *b = bvec.get_data();
// copy b into x
xvec = bvec;
// analyze matrix for typical nonzero magnitude
double Lmax = Lmat.MaxNorm();
// perform column-oriented Forwards Substitution algorithm
for (long int j=0; j<n; j++) {
// check for nonzero matrix diagonal
if (fabs(L[IDX(j,j,n)]) < STOL*Lmax) {
cerr << "FwdSub error: singular matrix!\n";
return 1;
}
// solve for this row of solution
x[j] /= L[IDX(j,j,n)];
// update all remaining rhs
for (long int i=j+1; i<n; i++)
x[i] -= L[IDX(i,j,n)]*x[j];
}
// return success
return 0;
}
// solves a linear system A*x = b, filling in the input Mat x
int Solve(Mat &Amat, Mat &xvec, Mat &bvec) {
// check that matrix sizes match
if (Amat.Rows() != bvec.Rows() || Amat.Rows() != Amat.Cols() || bvec.Cols() != 1 ||
xvec.Rows() != Amat.Rows() || xvec.Cols() != 1) {
cerr << "Solve error, illegal matrix/vector dimensions\n";
cerr << " Mat is " << Amat.Rows() << " x " << Amat.Cols()
<< ", rhs is " << bvec.Rows() << " x " << bvec.Cols()
<< ", solution is " << xvec.Rows() << " x " << xvec.Cols() << endl;
return 1;
}
// create temporary variables
long int i, j, k, p, n;
double m, tmp, Amax;
// access the data arrays
double *A = Amat.get_data();
double *b = bvec.get_data();
// determine maximum absolute entry in A (for singularity check later)
Amax = Amat.MaxNorm();
// perform Gaussian elimination to convert A,b to an upper-triangular system
n = Amat.Rows();
for (k=0; k<n-1; k++) { // loop over diagonals
// find the pivot row p
p=k;
for (i=k; i<n; i++)
if (fabs(A[IDX(i,k,n)]) > fabs(A[IDX(p,k,n)]))
p=i;
// swap rows in A
for (j=k; j<n; j++) {
tmp = A[IDX(p,j,n)];
A[IDX(p,j,n)] = A[IDX(k,j,n)];
A[IDX(k,j,n)] = tmp;
}
// swap rows in b
tmp = b[p];
b[p] = b[k];
b[k] = tmp;
// check for nonzero matrix diagonal
if (fabs(A[IDX(k,k,n)]) < STOL*Amax) {
cerr << "Solve error: numerically singular matrix!\n";
return 1;
}
// perform elimination using row k
for (i=k+1; i<n; i++) // store multipliers in column below pivot
A[IDX(i,k,n)] /= A[IDX(k,k,n)];
for (j=k+1; j<n; j++) // loop over columns of A, to right of pivot
for (i=k+1; i<n; i++) // update rows in column
A[IDX(i,j,n)] -= A[IDX(i,k,n)]*A[IDX(k,j,n)];
for (i=k+1; i<n; i++) // update entries in b
b[i] -= A[IDX(i,k,n)]*b[k];
}
// check for singularity at end (only need to check final diagonal entry)
if (fabs(A[IDX(n-1,n-1,n)]) < STOL*Amax) {
cerr << "Solve error: numerically singular matrix!\n";
return 1;
}
// check for singularity at end (only need to check final diagonal entry)
if (fabs(A[IDX(n-1,n-1,n)]) < STOL*Amax) {
cerr << "Solve error: numerically singular matrix!\n";
return 1;
}
// perform Backwards Substitution on result
if (BackSub(Amat, xvec, bvec) != 0) {
cerr << "Solve: error in BackSub call\n";
return 1;
}
// return success
return 0;
}
// solves a linear system A*x = b, filling in the input Mat x
int Solve(Mat &A, Mat &x, Mat &b) {
// create temporary variables
long int i, j, k, p, n;
double tmp, Amax;
// check that matrix sizes match
if (A.Rows() != b.Rows() || A.Rows() != A.Cols() ||
b.Cols() != 1 || x.Rows() != A.Rows() || x.Cols() != 1) {
fprintf(stderr,"Solve error, illegal matrix/vector dimensions\n");
fprintf(stderr," Mat is %li x %li, sol is %li x %li, rhs is %li x %li\n",
A.Rows(), A.Cols(), x.Rows(), x.Cols(), b.Rows(), b.Cols());
return 1;
}
// determine maximum absolute entry in A (for singularity check later)
Amax = A.MaxNorm();
// perform Gaussian elimination to convert A,b to an upper-triangular system
n = A.Rows();
for (k=0; k<n-1; k++) { // loop over diagonals
// find the pivot row p
p=k;
for (i=k; i<n; i++)
if (fabs(A(i,k)) > fabs(A(p,k)))
p=i;
// swap rows in A
for (j=k; j<n; j++) {
tmp = A(p,j);
A(p,j) = A(k,j);
A(k,j) = tmp;
}
// swap rows in b
tmp = b(p);
b(p) = b(k);
b(k) = tmp;
// check for singular matrix
if (fabs(A(k,k)) < 1.e-13*Amax) {
fprintf(stderr,"Solve error: numerically singular matrix!\n");
return 1;
}
// perform elimination on remaining submatrix of A using row k
for (j=k+1; j<n; j++)
for (i=k+1; i<n; i++)
A(i,j) = A(i,j) - A(i,k)/A(k,k)*A(k,j);
// perform elimination on remainder of b using row k
for (i=k+1; i<n; i++) b(i) -= A(i,k)/A(k,k)*b(k);
}
// check for singularity at end (only need to check final diagonal entry)
if (fabs(A(n-1,n-1)) < 1.e-13*Amax) {
fprintf(stderr,"Solve error: numerically singular matrix!\n");
return 1;
}
// perform Backwards Substitution on result
if (BackSub(A, x, b) != 0) {
fprintf(stderr,"Solve error in BackSub call\n");
return 1;
}
// return success
return 0;
}
