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