diff --git a/dune/elasticity/matrixops.hpp b/dune/elasticity/matrixops.hpp
index a255947..03ed906 100644
--- a/dune/elasticity/matrixops.hpp
+++ b/dune/elasticity/matrixops.hpp
@@ -55,6 +55,11 @@ class MatrixOps {
     //! \param[in] symmetric If true, augment symmetrically
     static Matrix augment(const Matrix& A, const Matrix& B,
                           size_t r0, size_t c0, bool symmetric);
+
+    //! \brief Extract the diagonal of a matrix into a new matrix
+    //! \param[in] The matrix to extract the diagonal from
+    //! \returns M = diag(A)
+    static Matrix extractDiagonal(const Matrix& A);
 };
 
 #include "matrixops_impl.hpp"
diff --git a/dune/elasticity/matrixops_impl.hpp b/dune/elasticity/matrixops_impl.hpp
index 7f6253c..7da51d7 100644
--- a/dune/elasticity/matrixops_impl.hpp
+++ b/dune/elasticity/matrixops_impl.hpp
@@ -144,3 +144,17 @@ Matrix MatrixOps::augment(const Matrix& A, const Matrix& B,
   
   return result;
 }
+
+Matrix MatrixOps::extractDiagonal(const Matrix& A)
+{
+  AdjacencyPattern adj;
+  adj.resize(A.M());
+  for (size_t i=0;i<A.M();++i)
+    adj[i].insert(i);
+  Matrix result;
+  fromAdjacency(result,adj,A.M(),A.M());
+  for (size_t i=0;i<A.M();++i)
+    result[i][i] = A[i][i];
+
+  return result;
+}