void solve( const fullMatrix & A,
fullMatrix& X,
const fullMatrix& Y )
{ // find X s.t. A X = Y using Gausian Elimination with
// row pivoting -- no scaling this version
if( A.row != A.col ){
std::cerr << "solve only works for square matrices\n";
return;
}
int i, j, k, n = A.col, m=Y.col;
double s, pa;
// initialize X to Y and copy A
X = Y;
fullMatrix B(A);
// could scale here
// next do forward elimination and forward solution
for( i=0; i<n; i++ ){ // clean up the i-th column
// find the pivot
k = i; // row with the pivot
pa = std::abs(B(i,i));
for( j=i+1; j<n; j++ )
if( std::abs(B(j,i)) > pa ){
pa = std::abs(B(j,i));
k = j;
}
if( pa == 0.0 )
error( "solve: singular matrix");
B.swapRows(i,k);
X.swapRows(i,k); // the pivot is now in B(i,i)
for( j=i+1; j<n; j++ ){ // for each row below row i
s = B(j,i)/B(i,i);
for( k=i+1; k<n; k++ ) // modifications that make B(j,i) zero
B(j,k) -= s*B(i,k);
for( k=0; k<m; k++ ) // change the rhs too
X(j,k) -= s*X(i,k);
}
}
// now B is upper triangular
for( i=n-1; i>=0; i-- ){ // do back solution
for(j = i+1; j<n; j++ ){
for( k=0; k<m; k++ )
X(i,k) -= B(i,j)*X(j,k);
}
for( k=0; k<m; k++ )
X(i,k) /= B(i,i);
}
}
