diff --git a/dune/solvers/common/genericvectortools.hh b/dune/solvers/common/genericvectortools.hh index fb9ed4e28c4b6bcbfad2196979517a53b232ab79..88ff16a4f591223eaab7715e2b33f5aefa377921 100644 --- a/dune/solvers/common/genericvectortools.hh +++ b/dune/solvers/common/genericvectortools.hh @@ -9,8 +9,11 @@ #include<bitset> #include <dune/common/fvector.hh> +#include <dune/common/fmatrix.hh> #include <dune/common/static_assert.hh> +#include <dune/istl/diagonalmatrix.hh> +#include <dune/istl/scaledidmatrix.hh> #include <dune/istl/bcrsmatrix.hh> #include <dune/istl/bvector.hh> @@ -205,6 +208,153 @@ struct GenericVector rvec[k][i][j] = lvec[i][j+k*offset]; } + template <class VectorType> + static std::size_t isnan(const VectorType& x) + { + std::size_t r = 0; + + typename VectorType::ConstIterator x_it=x.begin(); + typename VectorType::ConstIterator x_end=x.end(); + for (;x_it!=x_end; ++x_it) + r += isnan(*x_it); + + return r; + } + + template <class field_type, int n> + static std::size_t isnan(const Dune::FieldVector<field_type,n>& x) + { + std::size_t r=0; + + for (std::size_t i=0; i<n; ++i) + r += (size_t)(std::fpclassify(x[i])==FP_NAN); + + return r; + } +}; + +struct GenericMatrix +{ + + //! Set matrix to zero at indices that are true in bitvector recursivly + template <class MatrixType, class BitVectorTypeR, class BitVectorTypeC> + static void truncate(MatrixType& mat, const BitVectorTypeR& trows, const BitVectorTypeC& tcols, bool setTruncatedDiagonalOne) + { + assert(trows.size()==mat.N()); + assert(tcols.size()==mat.M()); + typename MatrixType::RowIterator row_it = mat.begin(); + typename MatrixType::RowIterator row_end = mat.end(); + for(; row_it!=row_end; ++row_it) + { + size_t row = row_it.index(); + + typename MatrixType::row_type::iterator col_it = row_it->begin(); + typename MatrixType::row_type::iterator col_end = row_it->end(); + for (; col_it!=col_end; ++col_it) + { + size_t col = col_it.index(); + GenericMatrix::truncate(*col_it, trows[row], tcols[col], setTruncatedDiagonalOne and (col==row)); + } + } + } + + template <class field_type, int n, int m, class BitVectorTypeR, class BitVectorTypeC> + static void truncate(Dune::FieldMatrix<field_type,n,m>& mat, const BitVectorTypeR& trows, const BitVectorTypeC& tcols, bool setTruncatedDiagonalOne) + { +// dune_static_assert(BitVectorTypeR.size()==n and BitVectorTypeC.size()==m,"BitVector length doesn't match matrix size."); + assert(trows.size()==n and tcols.size()==m); + + typedef typename Dune::FieldMatrix<field_type,n,m> MatrixType; + + for(size_t row=0; row<n; ++row) + { + if (trows[row]) + { + mat[row] = 0.0; + if (setTruncatedDiagonalOne) + mat[row][row] = 1.0; + continue; + } + + for (size_t col=0; col<m; ++col) + { + if (tcols[col]) + mat[row][col] = (setTruncatedDiagonalOne and col==row)?1.0:0.0; + } + } + } + + template <class field_type, int n, class BitVectorType> + static void truncate(Dune::DiagonalMatrix<field_type,n>& mat, const BitVectorType& trows, const BitVectorType& tcols, bool setTruncatedDiagonalOne) + { + typedef typename Dune::DiagonalMatrix<field_type,n> MatrixType; + + for(size_t row=0; row<n; ++row) + { + if (trows[row]) + { + mat.diagonal(row) = setTruncatedDiagonalOne?1.0:0.0; + continue; + } + + if (tcols[row]) + mat.diagonal(row) = setTruncatedDiagonalOne?1.0:0.0; + } + } + + template <class MatrixType> + static std::size_t isnan(const MatrixType& A) + { + std::size_t r=0; + + typename MatrixType::ConstRowIterator row_it=A.begin(); + typename MatrixType::ConstRowIterator row_end=A.end(); + for (;row_it!=row_end; ++row_it) + { + typename MatrixType::ConstColIterator col_it=row_it->begin(); + typename MatrixType::ConstColIterator col_end=row_it->end(); + for (;col_it!=col_end; ++col_it) + r += isnan(*col_it); + } + + return r; + } + + template <class field_type, int n, int m> + static std::size_t isnan(const Dune::FieldMatrix<field_type,n,m>& x) + { + std::size_t r=0; + + for (std::size_t i=0; i<n; ++i) + for (std::size_t j=0; j<m; ++j) + r += (size_t)(std::fpclassify(x[i][j])==FP_NAN); + + if (r!=0) + std::cout << x << std::endl; + + return r; + } + + template <class field_type, int n> + static std::size_t isnan(const Dune::DiagonalMatrix<field_type,n>& x) + { + std::size_t r=0; + + for (std::size_t i=0; i<n; ++i) + r += (size_t)(std::fpclassify(x.diagonal(i))==FP_NAN); + + if (r!=0) + std::cout << x << std::endl; + + return r; + } + + template <class field_type, int n> + static std::size_t isnan(const Dune::ScaledIdentityMatrix<field_type,n>& x) + { + return n*(size_t)(std::fpclassify(x.scalar())==FP_NAN); + } + }; #endif