diff --git a/common/fmatrix.hh b/common/fmatrix.hh
index ff6041596370b431f3c6e1189738f550b640193c..0df0daf17b1e9e496d14c52cfa8f1a43779ed29e 100644
--- a/common/fmatrix.hh
+++ b/common/fmatrix.hh
@@ -505,8 +505,116 @@ namespace Dune {
private:
// the data, very simply a built in array with row-wise ordering
row_type p[(n > 0) ? n : 1];
+
+ struct ElimPivot
+ {
+ ElimPivot(size_type pivot[n]);
+
+ void swap(int i, int j);
+
+ template<typename T>
+ void operator()(const T&, int k, int i)
+ {}
+
+ size_type* pivot_;
+ };
+
+ template<typename V>
+ struct Elim
+ {
+ Elim(V& rhs);
+
+ void swap(int i, int j);
+
+ void operator()(const typename V::field_type& factor, int k, int i);
+
+ V* rhs_;
+ };
+
+ template<class Func>
+ void luDecomposition(FieldMatrix<K,n,n>& A, Func func) const;
};
+ template<typename K, int n, int m>
+ FieldMatrix<K,n,m>::ElimPivot::ElimPivot(size_type pivot[n])
+ : pivot_(pivot)
+ {
+ for(int i=0; i < n; ++i) pivot[i]=i;
+ }
+
+ template<typename K, int n, int m>
+ void FieldMatrix<K,n,m>::ElimPivot::swap(int i, int j)
+ {
+ std::swap(pivot_[i], pivot_[j]);
+ }
+
+ template<typename K, int n, int m>
+ template<typename V>
+ FieldMatrix<K,n,m>::Elim<V>::Elim(V& rhs)
+ : rhs_(&rhs)
+ {}
+
+ template<typename K, int n, int m>
+ template<typename V>
+ void FieldMatrix<K,n,m>::Elim<V>::swap(int i, int j)
+ {
+ std::swap((*rhs_)[i], (*rhs_)[j]);
+ }
+
+ template<typename K, int n, int m>
+ template<typename V>
+ void FieldMatrix<K,n,m>::
+ Elim<V>::operator()(const typename V::field_type& factor, int k, int i)
+ {
+ (*rhs_)[k] -= factor*(*rhs_)[i];
+ }
+ template<typename K, int n, int m>
+ template<typename Func>
+ inline void FieldMatrix<K,n,m>::luDecomposition(FieldMatrix<K,n,n>& A, Func func) const
+ {
+ double norm=A.infinity_norm_real(); // for relative thresholds
+ double pivthres = std::max(FMatrixPrecision<>::absolute_limit(),norm*FMatrixPrecision<>::pivoting_limit());
+ double singthres = std::max(FMatrixPrecision<>::absolute_limit(),norm*FMatrixPrecision<>::singular_limit());
+
+ // LU decomposition of A in A
+ for (int i=0; i<n; i++) // loop over all rows
+ {
+ double pivmax=fvmeta_absreal(A[i][i]);
+
+ // pivoting ?
+ if (pivmax<pivthres)
+ {
+ // compute maximum of column
+ int imax=i; double abs;
+ for (int k=i+1; k<n; k++)
+ if ((abs=fvmeta_absreal(A[k][i]))>pivmax)
+ {
+ pivmax = abs; imax = k;
+ }
+ // swap rows
+ if (imax!=i) {
+ for (int j=i; j<n; j++)
+ std::swap(A[i][j],A[imax][j]);
+ func.swap(i, imax); // swap the pivot or rhs
+ }
+ }
+
+ // singular ?
+ if (pivmax<singthres)
+ DUNE_THROW(FMatrixError,"matrix is singular");
+
+ // eliminate
+ for (int k=i+1; k<n; k++)
+ {
+ K factor = A[k][i]/A[i][i];
+ A[k][i] = factor;
+ for (int j=i+1; j<n; j++)
+ A[k][j] -= factor*A[i][j];
+ func(factor, k, i);
+ }
+ }
+ }
+
template <class K, int n, int m>
template <class V>
inline void FieldMatrix<K,n,m>::solve(V& x, const V& b) const
@@ -533,65 +641,20 @@ namespace Dune {
x[1] = detinv*(p[0][0]*b[1]-p[1][0]*b[0]);
} else {
-
- // ////////////////////////////////////////////////////
- // Gaussian elimination with maximum column pivot
- // ////////////////////////////////////////////////////
-
- // make a copy of a to store factorization
+ V& rhs = x; // use x to store rhs
+ rhs = b; // copy data
+ Elim<V> elim(rhs);
FieldMatrix<K,n,n> A(*this);
- double norm=A.infinity_norm_real(); // for relative thresholds
- double pivthres = std::max(FMatrixPrecision<>::absolute_limit(),norm*FMatrixPrecision<>::pivoting_limit());
- double singthres = std::max(FMatrixPrecision<>::absolute_limit(),norm*FMatrixPrecision<>::singular_limit());
- V& rhs = x; // use x to store rhs
- rhs = b; // copy data
-
- // elimination phase
- for (int i=0; i<n; i++) // loop over all rows
- {
- double pivmax=fvmeta_absreal(A[i][i]);
-
- // pivoting ?
- if (pivmax<pivthres)
- {
- // compute maximum of row
- int imax=i; double abs;
- for (int k=i+1; k<n; k++)
- if ((abs=fvmeta_absreal(A[k][i]))>pivmax)
- {
- pivmax = abs; imax = k;
- }
- // swap rows
- if (imax!=i)
- for (int j=i; j<n; j++)
- std::swap(A[i][j],A[imax][j]);
- }
-
- // singular ?
- if (pivmax<singthres)
- DUNE_THROW(FMatrixError,"matrix is singular");
-
- // eliminate
- for (int k=i+1; k<n; k++)
- {
- K factor = -A[k][i]/A[i][i];
- for (int j=i+1; j<n; j++)
- A[k][j] += factor*A[i][j];
- rhs[k] += factor*rhs[i];
- }
- }
+ luDecomposition(A, elim);
// backsolve
- for (int i=n-1; i>=0; i--)
- {
+ for(int i=n-1; i>=0; i--) {
for (int j=i+1; j<n; j++)
rhs[i] -= A[i][j]*x[j];
x[i] = rhs[i]/A[i][i];
}
-
}
-
}
template <class K, int n, int m>
@@ -622,69 +685,35 @@ namespace Dune {
} else {
FieldMatrix<K,n,n> A(*this);
- FieldMatrix<K,n,n>& L=A;
- FieldMatrix<K,n,n>& U=A;
-
- double norm=A.infinity_norm_real(); // for relative thresholds
- double pivthres = std::max(FMatrixPrecision<>::absolute_limit(),norm*FMatrixPrecision<>::pivoting_limit());
- double singthres = std::max(FMatrixPrecision<>::absolute_limit(),norm*FMatrixPrecision<>::singular_limit());
-
- // LU decomposition of A in A
- for (int i=0; i<n; i++) // loop over all rows
- {
- double pivmax=fvmeta_absreal(A[i][i]);
-
- // pivoting ?
- if (pivmax<pivthres)
- {
- // compute maximum of column
- int imax=i; double abs;
- for (int k=i+1; k<n; k++)
- if ((abs=fvmeta_absreal(A[k][i]))>pivmax)
- {
- pivmax = abs; imax = k;
- }
- // swap rows
- if (imax!=i)
- for (int j=i; j<n; j++)
- std::swap(A[i][j],A[imax][j]);
- }
-
- // singular ?
- if (pivmax<singthres)
- DUNE_THROW(FMatrixError,"matrix is singular");
-
- // eliminate
- for (int k=i+1; k<n; k++)
- {
- K factor = A[k][i]/A[i][i];
- L[k][i] = factor;
- for (int j=i+1; j<n; j++)
- A[k][j] -= factor*A[i][j];
- }
- }
+ size_type pivot[n];
+ luDecomposition(A, ElimPivot(pivot));
+ FieldMatrix<K,n,m>& L=A;
+ FieldMatrix<K,n,m>& U=A;
// initialize inverse
- *this = 0;
- for (int i=0; i<n; i++) p[i][i] = 1;
+ *this=0;
+
+ for(int i=0; i<n; ++i)
+ p[pivot[i]][i]=1;
// L Y = I; multiple right hand sides
- for (int i=0; i<n; i++)
+ for (int i=0; i<n; i++) {
+ int row = pivot[i];
for (int j=0; j<i; j++)
for (int k=0; k<n; k++)
- p[i][k] -= L[i][j]*p[j][k];
+ p[row][k] -= L[i][j]*p[j][k];
+ }
// U A^{-1} = Y
- for (int i=n-1; i>=0; i--)
- for (int k=0; k<n; k++)
- {
+ for (int i=n-1; i>=0; i--) {
+ int row = pivot[i];
+ for (int k=0; k<n; k++) {
for (int j=i+1; j<n; j++)
- p[i][k] -= U[i][j]*p[j][k];
- p[i][k] /= U[i][i];
+ p[row][k] -= U[i][j]*p[j][k];
+ p[row][k] /= U[i][i];
}
-
+ }
}
-
}
// implementation of the determinant
diff --git a/common/test/fmatrixtest.cc b/common/test/fmatrixtest.cc
index 7be2dd34596007efd1014c781b79a7857a6ab74a..b97372681063fcf229dbf0a14e0a0f9ff6324d3c 100644
--- a/common/test/fmatrixtest.cc
+++ b/common/test/fmatrixtest.cc
@@ -8,6 +8,107 @@
using namespace Dune;
+template<typename T, int n>
+int test_invert_solve(T A_data[n*n], T inv_data[n*n],
+ T x_data[n], T b_data[n])
+{
+ int ret=0;
+
+ std::cout <<"Checking invertion of:"<<std::endl;
+
+ FieldMatrix<T,n,n> A, inv, calced_inv;
+ FieldVector<T,n> x, b, calced_x;
+
+ for(int i =0; i < n; ++i) {
+ x[i]=x_data[i];
+ b[i]=b_data[i];
+ for(int j=0; j <n; ++j) {
+ A[i][j] = A_data[i*n+j];
+ inv[i][j] = inv_data[i*n+j];
+ }
+ }
+
+ std::cout<<A<<std::endl;
+ FieldMatrix<T,n,n> copy(A);
+ A.invert();
+
+ calced_inv = A;
+ A-=inv;
+
+
+ bool equal=true;
+ double singthres = FMatrixPrecision<>::singular_limit()*10;
+ for(int i =0; i < n; ++i)
+ for(int j=0; j <n; ++j)
+ if(std::abs(A[i][j])>singthres) {
+ std::cerr<<"calculated inverse wrong at ("<<i<<","<<j<<")"<<std::endl;
+ equal=false;
+ }
+
+ if(!equal) {
+ ret++;
+ std::cerr<<"Calculated inverse was:"<<std::endl;
+ std::cerr <<calced_inv<<std::endl;
+ std::cerr<<"Should have been"<<std::endl;
+ std::cerr<<inv;
+ }else
+ std::cout<<"Result is"<<std::endl<<calced_inv<<std::endl;
+
+
+ std::cout<<"Checking solution for rhs="<<b<<std::endl;
+
+ copy.solve(calced_x, b);
+ FieldVector<T,n> xcopy(calced_x);
+ xcopy-=x;
+
+ equal=true;
+
+ for(int i =0; i < n; ++i)
+ if(std::abs(xcopy[i])>singthres) {
+ std::cerr<<"calculated isolution wrong at ("<<i<<")"<<std::endl;
+ equal=false;
+ }
+
+ if(!equal) {
+ ret++;
+ std::cerr<<"Calculated solution was:"<<std::endl;
+ std::cerr <<calced_x<<std::endl;
+ std::cerr<<"Should have been"<<std::endl;
+ std::cerr<<x;
+ std::cerr<<"difference is "<<xcopy<<std::endl;
+ }else
+ std::cout<<"Result is "<<calced_x<<std::endl;
+
+ return ret;
+}
+
+
+int test_invert_solve()
+{
+ int ret=0;
+
+ double A_data[9] = {1, 5, 7, 2, 14, 15, 4, 40, 39};
+ double inv_data[9] = {-9.0/4, 85.0/24, -23.0/24, -3.0/4, 11.0/24, -1.0/24, 1, -5.0/6, 1.0/6};
+ double b[3] = {32,75,201};
+ double x[3] = {1,2,3};
+
+
+ ret += test_invert_solve<double,3>(A_data, inv_data, x, b);
+
+ double A_data1[9] = {0, 1, 0, 1, 0, 0, 0, 0, 1};
+ double b1[3] = {0,1,2};
+ double x1[3] = {1,0,2};
+
+ ret += test_invert_solve<double,3>(A_data1, A_data1, x1, b1);
+
+ double A_data2[9] ={3, 1, 6, 2, 1, 3, 1, 1, 1};
+ double inv_data2[9] ={-2, 5, -3, 1, -3, 3, 1, -2, 1};
+ double b2[3] = {2, 7, 4};
+ double x2[3] = {19,-7,-8};
+
+ return ret + test_invert_solve<double,3>(A_data2, inv_data2, x2, b2);
+}
+
template<class K, int n, int m>
void test_matrix()
{
@@ -82,6 +183,7 @@ int main()
test_matrix<double, 1, 1>();
test_matrix<int, 10, 5>();
test_matrix<double, 5, 10>();
+ return test_invert_solve();
}
catch (Dune::Exception & e)
{