src/matrices.hh

 #ifndef SRC_MATRICES_HH
 #define SRC_MATRICES_HH
 
-template <class Matrix> struct Matrices {
-  Matrix elasticity;
-  Matrix damping;
-  Matrix mass;
+template <class Matrix> class Matrices {
+public:
+  std::vector<std::shared_ptr<Matrix>> elasticity;
+  std::vector<std::shared_ptr<Matrix>> damping;
+  std::vector<std::shared_ptr<Matrix>> mass;
+
+  Matrices(size_t n) {
+    elasticity.resize(n);
+    damping.resize(n);
+    mass.resize(n);
+
+      for (size_t i=0; i<n; i++) {
+        elasticity[i] = std::make_shared<Matrix>();
+        damping[i] = std::make_shared<Matrix>();
+        mass[i] = std::make_shared<Matrix>();
+      }
+  }
 };
 #endif

src/multi-body-problem-2D.cfg

+# -*- mode:conf -*-
+[boundary.friction]
+smallestDiameter= 2e-3  # [m]
+
+[timeSteps]
+refinementTolerance = 1e-5
+
+[u0.solver]
+tolerance         = 1e-8
+
+[a0.solver]
+tolerance         = 1e-8
+
+[v.solver]
+tolerance         = 1e-8
+
+[v.fpi]
+tolerance         = 1e-5
+
+[solver.tnnmg.linear]
+tolerance          = 1e-10

src/multi-body-problem-3D.cfg

+# -*- mode:conf -*-
+[boundary.friction]
+smallestDiameter= 2e-2  # [m]
+
+[boundary.friction.weakening]
+patchType       = Trapezoidal
+
+[timeSteps]
+refinementTolerance = 1e-5
+
+[u0.solver]
+tolerance         = 1e-6
+
+[a0.solver]
+tolerance         = 1e-6
+
+[v.solver]
+tolerance         = 1e-6
+
+[v.fpi]
+tolerance         = 1e-5
+
+[solver.tnnmg.linear]
+tolerance          = 1e-10

src/multi-body-problem-data/bc.hh

+#ifndef SRC_ONE_BODY_PROBLEM_DATA_BC_HH
+#define SRC_ONE_BODY_PROBLEM_DATA_BC_HH
+
+class VelocityDirichletCondition
+    : public Dune::VirtualFunction<double, double> {
+  void evaluate(double const &relativeTime, double &y) const {
+    // Assumed to vanish at time zero
+    double const finalVelocity = -5e-5;
+    y = (relativeTime <= 0.1)
+            ? finalVelocity * (1.0 - std::cos(relativeTime * M_PI / 0.1)) / 2.0
+            : finalVelocity;
+  }
+};
+
+class NeumannCondition : public Dune::VirtualFunction<double, double> {
+  void evaluate(double const &relativeTime, double &y) const { y = 0.0; }
+};
+#endif

src/multi-body-problem-data/cuboidgeometry.cc

+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <fstream>
+
+#ifdef HAVE_CAIROMM
+#include <cairomm/context.h>
+#include <cairomm/fontface.h>
+#include <cairomm/surface.h>
+#endif
+
+#include "cuboidgeometry.hh"
+
+/*
+const CuboidGeometry::LocalVector CuboidGeometry::lift(const double v0, const double v1) {
+  vc = 0;
+  vc[0] = vc2D[0];
+  vc[1] = vc2D[1];
+}
+
+const CuboidGeometry::LocalVector& CuboidGeometry::A() const { return A_; }
+const CuboidGeometry::LocalVector& CuboidGeometry::B() const { return B_; }
+const CuboidGeometry::LocalVector& CuboidGeometry::C() const { return C_; }
+const CuboidGeometry::LocalVector& CuboidGeometry::D() const { return D_; }
+const CuboidGeometry::LocalVector& CuboidGeometry::X() const { return X_; }
+const CuboidGeometry::LocalVector& CuboidGeometry::Y() const { return Y_; }
+double CuboidGeometry::depth() const { return depth_; }
+
+void CuboidGeometry::setupWeak(const LocalVector& weakOrigin, const double weakLength) {
+    lift(weakOrigin, X_);
+    const LocalVector2D Y({X_[0]+weakLength, X_[1]});
+    lift(Y, Y_);
+}
+*/
+
+#if MY_DIM == 3
+CuboidGeometry::CuboidGeometry(const LocalVector& origin, const LocalVector& weakOrigin, const double weakLength, const double length, const double width, const double depth_):
+    length_(length*lengthScale),
+    width_(width*lengthScale),
+    A(origin),
+    B({origin[0]+length_, origin[1], 0}),
+    C({origin[0]+length_, origin[1]+width_, 0}),
+    D({origin[0], origin[1]+width_, 0}),
+    X(weakOrigin),
+    Y({X[0]+weakLength, X[1], 0}),
+    depth(depth_*lengthScale) {}
+#else
+CuboidGeometry::CuboidGeometry(const LocalVector& origin, const LocalVector& weakOrigin, const double weakLength, const double length, const double width):
+    length_(length*lengthScale),
+    width_(width*lengthScale),
+    A(origin),
+    B({origin[0]+length_, origin[1]}),
+    C({origin[0]+length_, origin[1]+width_}),
+    D({origin[0], origin[1]+width_}),
+    X(weakOrigin),
+    Y({X[0]+weakLength, X[1]}) {}
+#endif
+
+
+void CuboidGeometry::write() const {
+  std::fstream writer("geometry", std::fstream::out);
+  writer << "A = " << A << std::endl;
+  writer << "B = " << B << std::endl;
+  writer << "C = " << C << std::endl;
+  writer << "D = " << D << std::endl;
+  writer << "X = " << X << std::endl;
+  writer << "Y = " << Y << std::endl;
+}
+
+void CuboidGeometry::render() const {
+#ifdef HAVE_CAIROMM
+  std::string const filename = "geometry.png";
+  double const width = 600;
+  double const height = 400;
+  double const widthScale = 400;
+  double const heightScale = 400;
+
+  auto surface =
+      Cairo::ImageSurface::create(Cairo::FORMAT_ARGB32, width, height);
+  auto cr = Cairo::Context::create(surface);
+
+  auto const setRGBColor = [&](int colour) {
+    cr->set_source_rgb(((colour & 0xFF0000) >> 16) / 255.0,
+                       ((colour & 0x00FF00) >> 8) / 255.0,
+                       ((colour & 0x0000FF) >> 0) / 255.0);
+  };
+  auto const moveTo = [&](LocalVector2D const &v) { cr->move_to(v[0], -v[1]); };
+  auto const lineTo = [&](LocalVector2D const &v) { cr->line_to(v[0], -v[1]); };
+
+  cr->scale(widthScale, heightScale);
+  cr->translate(0.1, 0.1);
+  cr->set_line_width(0.0025);
+
+  // triangle
+  {
+    moveTo(reference::A);
+    lineTo(reference::B);
+    lineTo(reference::C);
+    cr->close_path();
+    cr->stroke();
+  }
+
+  // dashed lines
+  {
+    cr->save();
+    std::vector<double> dashPattern = { 0.005 };
+    cr->set_dash(dashPattern, 0);
+    moveTo(reference::Z);
+    lineTo(reference::Y);
+    moveTo(reference::U);
+    lineTo(reference::X);
+    cr->stroke();
+    cr->restore();
+  }
+
+  // fill viscoelastic region
+  {
+    cr->save();
+    setRGBColor(0x0097E0);
+    moveTo(reference::B);
+    lineTo(reference::K);
+    lineTo(reference::M);
+    cr->fill();
+    cr->restore();
+  }
+
+  // mark weakening region
+  {
+    cr->save();
+    setRGBColor(0x7AD3FF);
+    cr->set_line_width(0.005);
+    moveTo(reference::X);
+    lineTo(reference::Y);
+    cr->stroke();
+    cr->restore();
+  }
+
+  // mark points
+  {
+    auto const drawCircle = [&](LocalVector2D const &v) {
+      cr->arc(v[0], -v[1], 0.0075, -M_PI, M_PI); // x,y,radius,angle1,angle2
+      cr->fill();
+    };
+
+    cr->save();
+    setRGBColor(0x002F47);
+    drawCircle(reference::A);
+    drawCircle(reference::B);
+    drawCircle(reference::C);
+    drawCircle(reference::Y);
+    drawCircle(reference::X);
+    drawCircle(reference::Z);
+    drawCircle(reference::U);
+    drawCircle(reference::K);
+    drawCircle(reference::M);
+    drawCircle(reference::G);
+    drawCircle(reference::H);
+    drawCircle(reference::J);
+    drawCircle(reference::I);
+    cr->restore();
+  }
+
+  // labels
+  {
+    auto const label = [&](LocalVector2D const &v, std::string l) {
+      moveTo(v);
+      cr->rel_move_to(0.005, -0.02);
+      cr->show_text(l);
+    };
+    auto font = Cairo::ToyFontFace::create(
+        "monospace", Cairo::FONT_SLANT_NORMAL, Cairo::FONT_WEIGHT_NORMAL);
+
+    cr->save();
+    cr->set_font_face(font);
+    cr->set_font_size(0.03);
+
+    label(A, "A");
+    label(B, "B");
+    label(C, "C");
+    label(D, "D");
+    label(X, "X");
+    label(Y, "Y");
+
+    cr->restore();
+  }
+
+  surface->write_to_png(filename);
+#endif
+}

src/multi-body-problem-data/cuboidgeometry.hh

+#ifndef SRC_MULTI_BODY_PROBLEM_DATA_CUBOIDGEOMETRY_HH
+#define SRC_MULTI_BODY_PROBLEM_DATA_CUBOIDGEOMETRY_HH
+
+#include <dune/common/fvector.hh>
+
+#include "midpoint.hh"
+
+class CuboidGeometry {
+  public:
+    typedef Dune::FieldVector<double, MY_DIM> LocalVector;
+
+    constexpr static double const lengthScale = 1.0; // scaling factor
+
+  private:
+    const double length_;
+    const double width_;
+    //const double weakLength_;     // length of the weak zone
+
+  public:
+    const LocalVector A;
+    const LocalVector B;
+    const LocalVector C;
+    const LocalVector D;
+    const LocalVector X;
+    const LocalVector Y;
+
+#if MY_DIM == 3
+    const double depth;
+
+    CuboidGeometry(const LocalVector& origin, const LocalVector& weakOrigin, const double weakLength = 0.20, const double length = 1.00, const double width = 0.27, const double depth = 0.60);
+#else
+        CuboidGeometry(const LocalVector& origin, const LocalVector& weakOrigin, const double weakLength = 0.20, const double length = 1.00, const double width = 0.27);
+#endif
+
+    void write() const;
+
+    void render() const;
+};
+#endif
+
+  /* from Elias
+  double const s = 1.0; // scaling factor
+
+  double const rightLeg = 0.27 * s;
+  double const leftLeg = 1.00 * s;
+  double const leftAngle = atan(rightLeg / leftLeg);
+  double const viscoHeight = 0.06 * s; // Height of the viscous bottom layer
+  double const weakLen = 0.20 * s;     // Length of the weak zone
+
+  double const zDistance = 0.35;
+
+  LocalVector2D const A = {0, 0};
+  LocalVector2D const B = {leftLeg, -rightLeg};
+  LocalVector2D const C = {leftLeg, 0};
+
+  LocalVector2D const Z = {zDistance * s, 0};
+  LocalVector2D const Y = {zDistance * s, -zDistance *s / leftLeg *rightLeg};
+  LocalVector2D const X = {Y[0] - weakLen * std::cos(leftAngle),
+                           Y[1] + weakLen *std::sin(leftAngle)};
+
+  LocalVector2D const U = {X[0], 0};
+
+  LocalVector2D const K = {B[0] - leftLeg * viscoHeight / rightLeg,
+                           B[1] + viscoHeight};
+  LocalVector2D const M = {B[0], B[1] + viscoHeight};
+
+  LocalVector2D const G = midPoint(A, X);
+  LocalVector2D const H = midPoint(X, Y);
+  LocalVector2D const J = midPoint(Y, B);
+
+  LocalVector2D const I = {Y[0] + G[0], Y[1] + G[1]};
+
+  LocalVector2D const zenith = {0, 1};
+
+  LocalMatrix2D const rotation = {{std::cos(leftAngle), -std::sin(leftAngle)},
+                                  {std::sin(leftAngle), std::cos(leftAngle)}};
+  */

src/multi-body-problem-data/geometry.tex

+\documentclass[tikz]{minimal}
+
+\usepackage{tikz}
+\usetikzlibrary{calc}
+\usetikzlibrary{decorations.pathreplacing}
+
+\usepackage{siunitx}
+
+\begin{document}
+\pgfmathsetmacro{\rightleg}{0.27}
+\pgfmathsetmacro{\leftleg}{1.00}
+\pgfmathsetmacro{\leftangle}{atan(\rightleg/\leftleg)}
+\begin{tikzpicture}[scale=12, rotate=\leftangle]
+  \pgfmathsetmacro{\mysin}{sin(\leftangle)}
+  \pgfmathsetmacro{\mycos}{cos(\leftangle)}
+  \pgfmathsetmacro{\viscoheight}{0.06}
+  \pgfmathsetmacro{\Zx}{0.35}
+  \pgfmathsetmacro{\weaklen}{0.20}
+
+  \coordinate (A) at (0,0);
+  \node at (A) [left] {A};
+  \coordinate (B) at (\leftleg,-\rightleg);
+  \node at (B) [right] {B};
+  \coordinate (C) at (\leftleg,0);
+  \node at (C) [right] {C};
+
+  \draw (A) -- (B) -- (C) -- node [above=.5cm, sloped] {$\overline{AC}=\SI{100}{cm}$} (A);
+
+  \coordinate (Z) at (\Zx,0);
+  \node at (Z) [above] {Z};
+  \coordinate (Y) at ($(\Zx,-\Zx/\leftleg * \rightleg)$);
+  \node at (Y) [below] {Y};
+  \coordinate (X) at ($(Y) + (-\weaklen*\mycos,\weaklen*\mysin)$);
+  \node at (X) [below] {X};
+  \path let \p1 = (X) in coordinate (U) at ($(\x1, 0)$);
+  \node at (U) [above] {U};
+
+  \path (A) -- node [above=.25cm, sloped] {$\overline{AZ} = \SI{35}{cm}$} (Z);
+
+  \draw[color=red, thick] (X) -- node [below=.25cm] {$\overline{XY}=\SI{20}{cm}$} (Y);
+  \draw[dashed] (Y) -- (Z);
+  \draw[dashed] (U) -- (X);
+
+  \coordinate (K) at ($(B) + (-\leftleg * \viscoheight / \rightleg,\viscoheight)$);
+  \node at (K) [below] {K};
+  \coordinate (M) at ($(B) + (0, \viscoheight)$);
+  \node at (M) [right] {M};
+  \path (C) -- node [right=.5cm] {$\overline{CM} = \SI{21}{cm}$} (M);
+
+  \path[fill=blue] (K) -- (B) -- node [right=.75cm] {$\overline{BM}=\SI{6}{cm}$} (M) -- cycle;
+
+  \coordinate (G) at ($(A) ! 0.5 ! (X)$);
+  \node at (G) [below] {G};
+  \coordinate (H) at ($(X) ! 0.5 ! (Y)$);
+  \node at (H) [below] {H};
+  \coordinate (J) at ($(Y) ! 0.5 ! (B)$);
+  \node at (J) [below] {J};
+
+  \coordinate (I) at ($(Y) + (G)$);
+  \node at (I) [below] {I};
+
+  \node[align=left] at (0.5,-0.225) {
+    $Z$: coast line\\
+    $\overline{XY}$: velocity weakening zone\\
+    $BKM$: visco-elastic domain};
+\end{tikzpicture}
+
+\end{document}

src/multi-body-problem-data/midpoint.hh

+#ifndef SRC_MIDPOINT_HH
+#define SRC_MIDPOINT_HH
+
+#include <dune/matrix-vector/axpy.hh>
+
+template <class Vector> Vector midPoint(Vector const &x, Vector const &y) {
+  Vector ret(0);
+  Dune::MatrixVector::addProduct(ret, 0.5, x);
+  Dune::MatrixVector::addProduct(ret, 0.5, y);
+  return ret;
+}
+#endif

src/multi-body-problem-data/mybody.hh

+#ifndef SRC_MULTI_BODY_PROBLEM_DATA_MYBODY_HH
+#define SRC_MULTI_BODY_PROBLEM_DATA_MYBODY_HH
+
+#include <dune/common/fvector.hh>
+
+#include <dune/fufem/functions/constantfunction.hh>
+
+#include <dune/tectonic/body.hh>
+#include <dune/tectonic/gravity.hh>
+
+#include "cuboidgeometry.hh"
+#include "segmented-function.hh"
+
+template <int dimension> class MyBody : public Body<dimension> {
+  using typename Body<dimension>::ScalarFunction;
+  using typename Body<dimension>::VectorField;
+
+public:
+  MyBody(Dune::ParameterTree const &parset)
+      : poissonRatio_(parset.get<double>("body.poissonRatio")),
+        youngModulus_(3.0 * parset.get<double>("body.bulkModulus") *
+                      (1.0 - 2.0 * poissonRatio_)),
+        shearViscosityField_(
+            parset.get<double>("body.elastic.shearViscosity"),
+            parset.get<double>("body.viscoelastic.shearViscosity")),
+        bulkViscosityField_(
+            parset.get<double>("body.elastic.bulkViscosity"),
+            parset.get<double>("body.viscoelastic.bulkViscosity")),
+        densityField_(parset.get<double>("body.elastic.density"),
+                      parset.get<double>("body.viscoelastic.density")),
+        gravityField_(densityField_, MyGeometry::zenith,
+                      parset.get<double>("gravity")) {}
+
+  double getPoissonRatio() const override { return poissonRatio_; }
+  double getYoungModulus() const override { return youngModulus_; }
+  ScalarFunction const &getShearViscosityField() const override {
+    return shearViscosityField_;
+  }
+  ScalarFunction const &getBulkViscosityField() const override {
+    return bulkViscosityField_;
+  }
+  ScalarFunction const &getDensityField() const override {
+    return densityField_;
+  }
+  VectorField const &getGravityField() const override { return gravityField_; }
+
+private:
+  double const poissonRatio_;
+  double const youngModulus_;
+  SegmentedFunction const shearViscosityField_;
+  SegmentedFunction const bulkViscosityField_;
+  SegmentedFunction const densityField_;
+  Gravity<dimension> const gravityField_;
+};
+#endif

src/multi-body-problem-data/myglobalfrictiondata.hh

+#ifndef SRC_ONE_BODY_PROBLEM_DATA_MYGLOBALFRICTIONDATA_HH
+#define SRC_ONE_BODY_PROBLEM_DATA_MYGLOBALFRICTIONDATA_HH
+
+#include <dune/common/function.hh>
+
+#include <dune/tectonic/globalfrictiondata.hh>
+
+#include "patchfunction.hh"
+
+template <class LocalVector>
+class MyGlobalFrictionData : public GlobalFrictionData<LocalVector::dimension> {
+private:
+  using typename GlobalFrictionData<LocalVector::dimension>::VirtualFunction;
+
+public:
+  MyGlobalFrictionData(Dune::ParameterTree const &parset,
+                       ConvexPolyhedron<LocalVector> const &segment)
+      : C_(parset.get<double>("C")),
+        L_(parset.get<double>("L")),
+        V0_(parset.get<double>("V0")),
+        a_(parset.get<double>("strengthening.a"),
+           parset.get<double>("weakening.a"), segment),
+        b_(parset.get<double>("strengthening.b"),
+           parset.get<double>("weakening.b"), segment),
+        mu0_(parset.get<double>("mu0")) {}
+
+  double const &C() const override { return C_; }
+  double const &L() const override { return L_; }
+  double const &V0() const override { return V0_; }
+  VirtualFunction const &a() const override { return a_; }
+  VirtualFunction const &b() const override { return b_; }
+  double const &mu0() const override { return mu0_; }
+
+private:
+  double const C_;
+  double const L_;
+  double const V0_;
+  PatchFunction const a_;
+  PatchFunction const b_;
+  double const mu0_;
+};
+#endif

src/multi-body-problem-data/mygrids.cc

+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <dune/fufem/geometry/polyhedrondistance.hh>
+
+#include "mygrids.hh"
+#include "midpoint.hh"
+#include "../diameter.hh"
+
+#if MY_DIM == 3
+SimplexManager::SimplexManager(unsigned int shift) : shift_(shift) {}
+#endif
+
+// back-to-front, front-to-back, front-to-back
+void SimplexManager::addFromVerticesFBB(unsigned int U, unsigned int V,
+                                        unsigned int W) {
+#if MY_DIM == 3
+  unsigned int const U2 = U + shift_;
+  unsigned int const V2 = V + shift_;
+  unsigned int const W2 = W + shift_;
+
+  simplices_.push_back({ U, V, W, U2 });
+  simplices_.push_back({ V, V2, W2, U2 });
+  simplices_.push_back({ W, W2, U2, V });
+#else
+  simplices_.push_back({ U, V, W });
+#endif
+}
+
+// back-to-front, back-to-front, front-to-back
+void SimplexManager::addFromVerticesFFB(unsigned int U, unsigned int V,
+                                        unsigned int W) {
+#if MY_DIM == 3
+  unsigned int const U2 = U + shift_;
+  unsigned int const V2 = V + shift_;
+  unsigned int const W2 = W + shift_;
+
+  simplices_.push_back({ U, V, W, U2 });
+  simplices_.push_back({ V, V2, W, U2 });
+  simplices_.push_back({ V2, W, U2, W2 });
+#else
+  simplices_.push_back({ U, V, W });
+#endif
+}
+
+auto SimplexManager::getSimplices() -> SimplexList const & {
+  return simplices_;
+}
+
+// Fix: 3D case (still Elias code)
+template <class Grid> GridsConstructor<Grid>::GridsConstructor(std::vector<std::shared_ptr<CuboidGeometry>> const &cuboidGeometries_) :
+  cuboidGeometries(cuboidGeometries_)
+{
+  size_t const gridCount = cuboidGeometries.size();
+  grids.resize(gridCount);
+  gridFactories.resize(gridCount);
+
+  for (size_t idx=0; idx<grids.size(); idx++) {
+    const auto& cuboidGeometry = *cuboidGeometries[idx];
+
+    const auto& A = cuboidGeometry.A;
+    const auto& B = cuboidGeometry.B;
+    const auto& C = cuboidGeometry.C;
+    const auto& D = cuboidGeometry.D;
+
+    unsigned int const vc = 4;
+
+#if MY_DIM == 3
+    Dune::FieldMatrix<double, 2 * vc, MY_DIM> vertices;
+#else
+    Dune::FieldMatrix<double, vc, MY_DIM> vertices;
+#endif
+    for (size_t i = 0; i < 2; ++i) {
+#if MY_DIM == 3
+      size_t numXYplanes = 2;
+#else
+      size_t numXYplanes = 1;
+#endif
+      size_t k = 0;
+      for (size_t j = 1; j <= numXYplanes; ++j) {
+        vertices[k++][i] = A[i];
+        vertices[k++][i] = B[i];
+        vertices[k++][i] = C[i];
+        vertices[k++][i] = D[i];
+        assert(k == j * vc);
+      }
+    }
+
+#if MY_DIM == 3
+    for (size_t k = 0; k < vc; ++k) {
+      vertices[k][2] = -cuboidGeometry.depth / 2.0;
+      vertices[k + vc][2] = cuboidGeometry.depth / 2.0;
+    }
+#endif
+
+    for (size_t i = 0; i < vertices.N(); ++i)
+      gridFactories[idx].insertVertex(vertices[i]);
+
+    Dune::GeometryType cell;
+#if MY_DIM == 3
+    cell = Dune::GeometryTypes::tetrahedron;
+#else
+    cell = Dune::GeometryTypes::triangle;
+#endif
+
+#if MY_DIM == 3
+    SimplexManager sm(vc);
+#else
+    SimplexManager sm;
+#endif
+    sm.addFromVerticesFFB(1, 2, 0);
+    sm.addFromVerticesFFB(2, 3, 0);
+    auto const &simplices = sm.getSimplices();
+
+    // sanity-check choices of simplices
+    for (size_t i = 0; i < simplices.size(); ++i) {
+      Dune::FieldMatrix<double, MY_DIM, MY_DIM> check;
+      for (size_t j = 0; j < MY_DIM; ++j)
+        check[j] = vertices[simplices[i][j + 1]] - vertices[simplices[i][j]];
+      assert(check.determinant() > 0);
+      gridFactories[idx].insertElement(cell, simplices[i]);
+    }
+
+    grids[idx] = std::shared_ptr<Grid>(gridFactories[idx].createGrid());
+  }
+}
+
+template <class Grid>
+std::vector<std::shared_ptr<Grid>>& GridsConstructor<Grid>::getGrids() {
+  return grids;
+}
+
+template <class Grid>
+template <class GridView>
+MyFaces<GridView> GridsConstructor<Grid>::constructFaces(
+    GridView const &gridView, CuboidGeometry const &cuboidGeometry) {
+  return MyFaces<GridView>(gridView, cuboidGeometry);
+}
+
+template <class GridView>
+template <class Vector>
+bool MyFaces<GridView>::xyCollinear(Vector const &a, Vector const &b,
+                                    Vector const &c) {
+  return isClose2((b[0] - a[0]) * (c[1] - a[1]), (b[1] - a[1]) * (c[0] - a[0]));
+}
+
+template <class GridView>
+template <class Vector>
+bool MyFaces<GridView>::xyBoxed(Vector const &v1, Vector const &v2,
+                                Vector const &x) {
+  auto const minmax0 = std::minmax(v1[0], v2[0]);
+  auto const minmax1 = std::minmax(v1[1], v2[1]);
+
+  if (minmax0.first - 1e-14 * cuboidGeometry.lengthScale > x[0] or
+      x[0] > minmax0.second + 1e-14 * cuboidGeometry.lengthScale)
+    return false;
+  if (minmax1.first - 1e-14 * cuboidGeometry.lengthScale > x[1] or
+      x[1] > minmax1.second + 1e-14 * cuboidGeometry.lengthScale)
+    return false;
+
+  return true;
+}
+
+template <class GridView>
+template <class Vector>
+bool MyFaces<GridView>::xyBetween(Vector const &v1, Vector const &v2,
+                                  Vector const &x) {
+  return xyCollinear(v1, v2, x) && xyBoxed(v1, v2, x);
+}
+
+template <class GridView>
+MyFaces<GridView>::MyFaces(GridView const &gridView, CuboidGeometry const &cuboidGeometry_)
+      :
+  #if MY_DIM == 3
+        lower(gridView),
+        right(gridView),
+        upper(gridView),
+        left(gridView),
+        front(gridView),
+        back(gridView),
+  #else
+        lower(gridView),
+        right(gridView),
+        upper(gridView),
+        left(gridView),
+  #endif
+        cuboidGeometry(cuboidGeometry_)
+  {
+    lower.insertFacesByProperty([&](typename GridView::Intersection const &in) {
+      return xyBetween(cuboidGeometry.A, cuboidGeometry.B, in.geometry().center());
+    });
+
+    right.insertFacesByProperty([&](typename GridView::Intersection const &in) {
+      return xyBetween(cuboidGeometry.B, cuboidGeometry.C, in.geometry().center());
+    });
+
+    upper.insertFacesByProperty([&](typename GridView::Intersection const &in) {
+      return xyBetween(cuboidGeometry.D, cuboidGeometry.C, in.geometry().center());
+    });
+
+    left.insertFacesByProperty([&](typename GridView::Intersection const &in) {
+      return xyBetween(cuboidGeometry.A, cuboidGeometry.D, in.geometry().center());
+    });
+
+  #if MY_DIM == 3
+    front.insertFacesByProperty([&](typename GridView::Intersection const &in) {
+      return isClose(cuboidGeometry.depth / 2.0, in.geometry().center()[2]);
+    });
+
+    back.insertFacesByProperty([&](typename GridView::Intersection const &in) {
+      return isClose(-cuboidGeometry.depth / 2.0, in.geometry().center()[2]);
+    });
+  #endif
+  }
+
+double computeAdmissibleDiameter(double distance, double smallestDiameter, double lengthScale) {
+  return (distance / 0.0125 / lengthScale + 1.0) * smallestDiameter;
+}
+
+template <class Grid, class LocalVector>
+void refine(Grid &grid, ConvexPolyhedron<LocalVector> const &weakPatch,
+            double smallestDiameter, double lengthScale) {
+  bool needRefine = true;
+  while (true) {
+    needRefine = false;
+    for (auto &&e : elements(grid.leafGridView())) {
+      auto const geometry = e.geometry();
+
+      auto const weakeningRegionDistance =
+          distance(weakPatch, geometry, 1e-6 * lengthScale);
+      auto const admissibleDiameter =
+          computeAdmissibleDiameter(weakeningRegionDistance, smallestDiameter, lengthScale);
+
+      if (diameter(geometry) <= admissibleDiameter)
+        continue;
+
+      needRefine = true;
+      grid.mark(1, e);
+    }
+    if (!needRefine)
+      break;
+
+    grid.preAdapt();
+    grid.adapt();
+    grid.postAdapt();
+  }
+}
+
+#include "mygrids_tmpl.cc"

src/multi-body-problem-data/mygrids.hh

+#ifndef SRC_MULTI_BODY_PROBLEM_DATA_MYGRIDS_HH
+#define SRC_MULTI_BODY_PROBLEM_DATA_MYGRIDS_HH
+
+#include <dune/common/fmatrix.hh>
+#include <dune/grid/common/gridfactory.hh>
+
+#include <dune/fufem/boundarypatch.hh>
+#include <dune/fufem/geometry/convexpolyhedron.hh>
+
+#include "cuboidgeometry.hh"
+
+template <class GridView> struct MyFaces {
+  BoundaryPatch<GridView> lower;
+  BoundaryPatch<GridView> right;
+  BoundaryPatch<GridView> upper;
+  BoundaryPatch<GridView> left;
+
+#if MY_DIM == 3
+  BoundaryPatch<GridView> front;
+  BoundaryPatch<GridView> back;
+#endif
+
+  MyFaces(GridView const &gridView, CuboidGeometry const &cuboidGeometry_);
+
+private:
+  CuboidGeometry const &cuboidGeometry;
+
+  bool isClose(double a, double b) {
+    return std::abs(a - b) < 1e-14 * cuboidGeometry.lengthScale;
+  };
+
+  bool isClose2(double a, double b) {
+    return std::abs(a - b) <
+           1e-14 * cuboidGeometry.lengthScale * cuboidGeometry.lengthScale;
+  };
+
+  template <class Vector>
+  bool xyBoxed(Vector const &v1, Vector const &v2, Vector const &x);
+
+  template <class Vector>
+  bool xyCollinear(Vector const &a, Vector const &b, Vector const &c);
+
+  template <class Vector>
+  bool xyBetween(Vector const &v1, Vector const &v2, Vector const &x);
+};
+
+class SimplexManager {
+public:
+  using SimplexList = std::vector<std::vector<unsigned int>>;
+
+#if MY_DIM == 3
+  SimplexManager(unsigned int shift);
+#endif
+
+  void addFromVerticesFBB(unsigned int U, unsigned int V, unsigned int W);
+  void addFromVerticesFFB(unsigned int U, unsigned int V, unsigned int W);
+
+  SimplexList const &getSimplices();
+
+private:
+  SimplexList simplices_;
+
+#if MY_DIM == 3
+  unsigned int const shift_;
+#endif
+};
+
+template <class Grid> class GridsConstructor {
+public:
+  GridsConstructor(std::vector<std::shared_ptr<CuboidGeometry>> const &cuboidGeometries_);
+
+  std::vector<std::shared_ptr<Grid>>& getGrids();
+
+  template <class GridView>
+  MyFaces<GridView> constructFaces(GridView const &gridView, CuboidGeometry const &cuboidGeometry);
+
+private:
+  std::vector<std::shared_ptr<CuboidGeometry>> const &cuboidGeometries;
+  std::vector<Dune::GridFactory<Grid>> gridFactories;
+  std::vector<std::shared_ptr<Grid>> grids;
+};
+
+double computeAdmissibleDiameter(double distance, double smallestDiameter, double lengthScale);
+
+template <class Grid, class LocalVector>
+void refine(Grid &grid, ConvexPolyhedron<LocalVector> const &weakPatch,
+            double smallestDiameter, double lengthScale);
+
+#endif
+
+

src/multi-body-problem-data/mygrids_tmpl.cc

+#ifndef MY_DIM
+#error MY_DIM unset
+#endif
+
+#include "../explicitgrid.hh"
+#include "../explicitvectors.hh"
+#include "cuboidgeometry.hh"
+
+template class GridsConstructor<Grid>;
+
+template struct MyFaces<DeformedGrid::LeafGridView>;
+template struct MyFaces<DeformedGrid::LevelGridView>;
+
+template MyFaces<DeformedGrid::LeafGridView> GridsConstructor<Grid>::constructFaces(
+    DeformedGrid::LeafGridView const &gridView, CuboidGeometry const &CuboidGeometry_);
+
+template MyFaces<DeformedGrid::LevelGridView> GridsConstructor<Grid>::constructFaces(
+    DeformedGrid::LevelGridView const &gridView, CuboidGeometry const &CuboidGeometry_);
+
+template void refine<Grid, LocalVector>(
+    Grid &grid, ConvexPolyhedron<LocalVector> const &weakPatch,
+    double smallestDiameter, double lengthScale);

src/multi-body-problem-data/patchfunction.hh

+#ifndef SRC_ONE_BODY_PROBLEM_DATA_PATCHFUNCTION_HH
+#define SRC_ONE_BODY_PROBLEM_DATA_PATCHFUNCTION_HH
+
+#include <dune/common/function.hh>
+#include <dune/common/fvector.hh>
+#include <dune/common/parametertree.hh>
+
+#include <dune/fufem/geometry/polyhedrondistance.hh>
+
+class PatchFunction
+    : public Dune::VirtualFunction<Dune::FieldVector<double, MY_DIM>,
+                                   Dune::FieldVector<double, 1>> {
+private:
+  using Polyhedron = ConvexPolyhedron<Dune::FieldVector<double, MY_DIM>>;
+
+  double const v1_;
+  double const v2_;
+  Polyhedron const &segment_;
+
+public:
+  PatchFunction(double v1, double v2, Polyhedron const &segment)
+      : v1_(v1), v2_(v2), segment_(segment) {}
+
+  void evaluate(Dune::FieldVector<double, MY_DIM> const &x,
+                Dune::FieldVector<double, 1> &y) const {
+    y = distance(x, segment_, 1e-6 * MyGeometry::lengthScale) <= 1e-5 ? v2_
+                                                                      : v1_;
+  }
+};
+
+#endif

src/multi-body-problem-data/segmented-function.hh

+#ifndef SRC_ONE_BODY_PROBLEM_DATA_SEGMENTED_FUNCTION_HH
+#define SRC_ONE_BODY_PROBLEM_DATA_SEGMENTED_FUNCTION_HH
+
+#include <dune/common/function.hh>
+#include <dune/common/fvector.hh>
+#include <dune/common/parametertree.hh>
+
+#include "cuboidgeometry.hh"
+
+class SegmentedFunction
+    : public Dune::VirtualFunction<Dune::FieldVector<double, MY_DIM>,
+                                   Dune::FieldVector<double, 1>> {
+private:
+  bool liesBelow(Dune::FieldVector<double, MY_DIM> const &x,
+                 Dune::FieldVector<double, MY_DIM> const &y,
+                 Dune::FieldVector<double, MY_DIM> const &z) const {
+    return x[1] + (z[0] - x[0]) * (y[1] - x[1]) / (y[0] - x[0]) >= z[1];
+  };
+  bool insideRegion2(Dune::FieldVector<double, MY_DIM> const &z) const {
+    return liesBelow(MyGeometry::K, MyGeometry::M, z);
+  };
+
+  double const _v1;
+  double const _v2;
+
+public:
+  SegmentedFunction(double v1, double v2) : _v1(v1), _v2(v2) {}
+
+  void evaluate(Dune::FieldVector<double, MY_DIM> const &x,
+                Dune::FieldVector<double, 1> &y) const {
+    y = insideRegion2(x) ? _v2 : _v1;
+  }
+};
+#endif

src/multi-body-problem-data/weakpatch.hh

+#ifndef SRC_MULTI_BODY_PROBLEM_DATA_WEAKPATCH_HH
+#define SRC_MULTI_BODY_PROBLEM_DATA_WEAKPATCH_HH
+
+#include "cuboidgeometry.hh"
+
+template <class LocalVector>
+ConvexPolyhedron<LocalVector> getWeakPatch(Dune::ParameterTree const &parset, CuboidGeometry const &cuboidGeometry) {
+  ConvexPolyhedron<LocalVector> weakPatch;
+#if MY_DIM == 3
+  weakPatch.vertices.resize(4);
+  weakPatch.vertices[0] = weakPatch.vertices[2] = cuboidGeometry.X;
+  weakPatch.vertices[1] = weakPatch.vertices[3] = cuboidGeometry.Y;
+  for (size_t k = 0; k < 2; ++k) {
+    weakPatch.vertices[k][2] = -cuboidGeometry.depth / 2.0;
+    weakPatch.vertices[k + 2][2] = cuboidGeometry.depth / 2.0;
+  }
+  switch (parset.get<Config::PatchType>("patchType")) {
+    case Config::Rectangular:
+      break;
+    case Config::Trapezoidal:
+      weakPatch.vertices[1][0] += 0.05 * cuboidGeometry.lengthScale;
+      weakPatch.vertices[3][0] -= 0.05 * cuboidGeometry.lengthScale;
+      break;
+    default:
+      assert(false);
+  }
+#else
+  weakPatch.vertices.resize(2);
+  weakPatch.vertices[0] = cuboidGeometry.X;
+  weakPatch.vertices[1] = cuboidGeometry.Y;
+#endif
+  return weakPatch;
+};
+#endif

src/multi-body-problem.cc

+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#ifdef HAVE_IPOPT
+#undef HAVE_IPOPT
+#endif
+
+#include <atomic>
+#include <cmath>
+#include <csignal>
+#include <exception>
+#include <fstream>
+#include <iostream>
+#include <iomanip>
+#include <memory>
+
+#include <dune/common/bitsetvector.hh>
+#include <dune/common/exceptions.hh>
+#include <dune/common/fmatrix.hh>
+#include <dune/common/function.hh>
+#include <dune/common/fvector.hh>
+#include <dune/common/parallel/mpihelper.hh>
+#include <dune/common/parametertree.hh>
+#include <dune/common/parametertreeparser.hh>
+
+#include <dune/grid/common/mcmgmapper.hh>
+#include <dune/istl/bcrsmatrix.hh>
+#include <dune/istl/bvector.hh>
+
+#include <dune/fufem/boundarypatch.hh>
+#include <dune/fufem/formatstring.hh>
+
+#include <dune/solvers/norms/energynorm.hh>
+
+
+/*
+#include <dune/solvers/solvers/loopsolver.hh>
+#include <dune/solvers/solvers/solver.hh>*/
+#include <dune/tnnmg/problem-classes/convexproblem.hh>
+
+#include <dune/contact/common/deformedcontinuacomplex.hh>
+#include <dune/contact/common/couplingpair.hh>
+#include <dune/contact/assemblers/nbodyassembler.hh>
+//#include <dune/contact/assemblers/dualmortarcoupling.hh>
+#include <dune/contact/projections/normalprojection.hh>
+
+#include <dune/tectonic/geocoordinate.hh>
+#include <dune/tectonic/myblockproblem.hh>
+#include <dune/tectonic/globalfriction.hh>
+#include <dune/fufem/hdf5/file.hh>
+
+#include "assemblers.hh"
+#include "diameter.hh"
+#include "enumparser.hh"
+#include "enums.hh"
+#include "gridselector.hh"
+#include "hdf5-writer.hh"
+#include "hdf5/restart-io.hh"
+#include "matrices.hh"
+#include "program_state.hh"
+#include "multi-body-problem-data/bc.hh"
+#include "multi-body-problem-data/mybody.hh"
+#include "multi-body-problem-data/cuboidgeometry.hh"
+#include "multi-body-problem-data/myglobalfrictiondata.hh"
+#include "multi-body-problem-data/mygrids.hh"
+#include "multi-body-problem-data/weakpatch.hh"
+//#include "spatial-solving/solverfactory.hh"
+//#include "time-stepping/adaptivetimestepper.hh"
+#include "time-stepping/rate.hh"
+#include "time-stepping/state.hh"
+#include "time-stepping/updaters.hh"
+#include "vtk.hh"
+
+// for getcwd
+#include <unistd.h>
+
+#include <dune/grid/io/file/vtk/vtkwriter.hh>
+
+#define USE_OLD_TNNMG
+
+size_t const dims = MY_DIM;
+
+template <class GridView>
+void writeToVTK(const GridView& gridView, const std::string path, const std::string name) {
+    Dune::VTKWriter<GridView> vtkwriter(gridView);
+
+    //std::ofstream lStream( "garbage.txt" );
+    std::streambuf* lBufferOld = std::cout.rdbuf();
+    //std::cout.rdbuf(lStream.rdbuf());
+
+    vtkwriter.pwrite(name, path, path);
+
+    std::cout.rdbuf( lBufferOld );
+}
+
+
+Dune::ParameterTree getParameters(int argc, char *argv[]) {
+  Dune::ParameterTree parset;
+  Dune::ParameterTreeParser::readINITree("/home/mi/podlesny/software/dune/dune-tectonic/src/multi-body-problem.cfg", parset);
+  Dune::ParameterTreeParser::readINITree(
+      Dune::Fufem::formatString("/home/mi/podlesny/software/dune/dune-tectonic/src/multi-body-problem-%dD.cfg", dims), parset);
+  Dune::ParameterTreeParser::readOptions(argc, argv, parset);
+  return parset;
+}
+
+static std::atomic<bool> terminationRequested(false);
+void handleSignal(int signum) { terminationRequested = true; }
+
+int main(int argc, char *argv[]) {
+  try {
+    Dune::MPIHelper::instance(argc, argv);
+
+    char buffer[256];
+    char *val = getcwd(buffer, sizeof(buffer));
+    if (val) {
+        std::cout << buffer << std::endl;
+        std::cout << argv[0] << std::endl;
+    }
+
+    auto const parset = getParameters(argc, argv);
+
+    using Vector = Dune::BlockVector<Dune::FieldVector<double, dims>>;
+    using DeformedGridComplex = typename Dune::Contact::DeformedContinuaComplex<Grid, Vector>;
+    using DeformedGrid = DeformedGridComplex::DeformedGridType;
+
+    using DefLeafGridView = DeformedGrid::LeafGridView;
+    using DefLevelGridView = DeformedGrid::LevelGridView;
+
+    using Assembler = MyAssembler<DefLeafGridView, dims>;
+    using Matrix = Assembler::Matrix;
+    using LocalVector = Vector::block_type;
+    using ScalarMatrix = Assembler::ScalarMatrix;
+    using ScalarVector = Assembler::ScalarVector;
+    using field_type = Matrix::field_type;
+
+    // set up material properties of all bodies
+    MyBody<dims> const body(parset);
+
+    // set up cuboid geometries
+    const size_t bodyCount = parset.get<int>("problem.bodyCount");
+    std::vector<std::shared_ptr<CuboidGeometry>> cuboidGeometries(bodyCount);
+
+    double const length = 1.00;
+    double const width = 0.27;
+    double const weakLength = 0.20;
+
+#if MY_DIM == 3
+    double const depth = 0.60;
+    // TODO: replace with make_shared
+    cuboidGeometries[0] = std::shared_ptr<CuboidGeometry>(new CuboidGeometry({0,0,0}, {0.2,width,0}, weakLength, length, width, depth));
+    for (size_t i=1; i<bodyCount; i++) {
+        cuboidGeometries[i] = std::shared_ptr<CuboidGeometry>(new CuboidGeometry(cuboidGeometries[i-1]->D, {0.6,i*width,0}, weakLength, length, width, depth));
+    }
+#else
+    // TODO: replace with make_shared
+    cuboidGeometries[0] = std::shared_ptr<CuboidGeometry>(new CuboidGeometry({0,0}, {0.2,width}, weakLength, length, width));
+    for (size_t i=1; i<bodyCount; i++) {
+        cuboidGeometries[i] = std::shared_ptr<CuboidGeometry>(new CuboidGeometry(cuboidGeometries[i-1]->D, {0.6,i*width}, weakLength, length, width));
+    }
+#endif
+
+    // set up grids
+    GridsConstructor<Grid> gridConstructor(cuboidGeometries);
+    auto& grids = gridConstructor.getGrids();
+
+    std::vector<ConvexPolyhedron<LocalVector>> weakPatches(grids.size());
+
+    for (size_t i=0; i<grids.size(); i++) {
+        const auto& cuboidGeometry = *cuboidGeometries[i];
+
+        // define weak patch and refine grid
+        auto& weakPatch = weakPatches[i];
+        weakPatch = getWeakPatch<LocalVector>(parset.sub("boundary.friction.weakening"), cuboidGeometry);
+        refine(*grids[i], weakPatch, parset.get<double>("boundary.friction.smallestDiameter"), cuboidGeometry.lengthScale);
+
+        writeToVTK(grids[i]->leafGridView(), "", "grid" + std::to_string(i));
+
+        // determine minDiameter and maxDiameter
+        double minDiameter = std::numeric_limits<double>::infinity();
+        double maxDiameter = 0.0;
+        for (auto &&e : elements(grids[i]->leafGridView())) {
+          auto const geometry = e.geometry();
+          auto const diam = diameter(geometry);
+          minDiameter = std::min(minDiameter, diam);
+          maxDiameter = std::max(maxDiameter, diam);
+        }
+        std::cout << "Grid" << i << " min diameter: " << minDiameter << std::endl;
+        std::cout << "Grid" << i << " max diameter: " << maxDiameter << std::endl;
+    }
+
+    // set up deformed grids
+    DeformedGridComplex deformedGridComplex;
+    for (size_t i=0; i<grids.size(); i++) {
+        deformedGridComplex.addGrid(*grids[i], "body" + std::to_string(i));
+    }
+
+    const auto deformedGrids = deformedGridComplex.gridVector();
+
+    /*
+    // test deformed grids
+    for (size_t i=0; i<deformedGrids.size(); i++) {
+      Vector def(deformedGrids[i]->size(dims));
+      def = 1;
+      deformedGridComplex.setDeformation(def, i);
+
+      writeToVTK(deformedGrids[i]->leafGridView(), "", "deformedGrid" + std::to_string(i));
+    }
+    // test deformed grids
+    */
+
+    std::vector<std::shared_ptr<DefLeafGridView>> leafViews(deformedGrids.size());
+    std::vector<std::shared_ptr<DefLevelGridView>> levelViews(deformedGrids.size());
+    std::vector<size_t> leafVertexCounts(deformedGrids.size());
+
+    using LeafFaces = MyFaces<DefLeafGridView>;
+    using LevelFaces = MyFaces<DefLevelGridView>;
+    std::vector<std::shared_ptr<LeafFaces>> leafFaces(deformedGrids.size());
+    std::vector<std::shared_ptr<LevelFaces>> levelFaces(deformedGrids.size());
+
+    for (size_t i=0; i<deformedGrids.size(); i++) {
+        leafViews[i] = std::make_shared<DefLeafGridView>(deformedGrids[i]->leafGridView());
+        levelViews[i] = std::make_shared<DefLevelGridView>(deformedGrids[i]->levelGridView(0));
+
+        leafVertexCounts[i] = leafViews[i]->size(dims);
+
+        const auto& cuboidGeometry = *cuboidGeometries[i];
+        leafFaces[i] = std::make_shared<LeafFaces>(*leafViews[i], cuboidGeometry);
+        levelFaces[i] = std::make_shared<LevelFaces>(*levelViews[i], cuboidGeometry);
+    }
+
+    // set up contact couplings
+    using LeafBoundaryPatch = BoundaryPatch<DefLeafGridView>;
+    using LevelBoundaryPatch = BoundaryPatch<DefLevelGridView>;
+
+    using CouplingPair = Dune::Contact::CouplingPair<DeformedGrid, DeformedGrid, field_type>;
+    std::vector<std::shared_ptr<CouplingPair>> couplings(bodyCount-1);
+
+    for (size_t i=0; i<couplings.size(); i++) {
+      couplings[i] = std::make_shared<CouplingPair>();
+
+      auto nonmortarPatch = std::make_shared<LevelBoundaryPatch>(levelFaces[i]->upper);
+      auto mortarPatch = std::make_shared<LevelBoundaryPatch>(levelFaces[i+1]->lower);
+
+      auto contactProjection = std::make_shared<Dune::Contact::NormalProjection<LeafBoundaryPatch>>();
+      std::shared_ptr<CouplingPair::BackEndType> backend = nullptr;
+      couplings[i]->set(i, i+1, nonmortarPatch, mortarPatch, 0.1, Dune::Contact::CouplingPairBase::STICK_SLIP, contactProjection, backend);
+    }
+
+    // set up dune-contact nBodyAssembler
+    /*std::vector<const DeformedGrid*> const_grids(bodyCount);
+    for (size_t i=0; i<const_grids.size(); i++) {
+        const_grids[i] = &deformedGrids.grid("body" + std::to_string(i));
+    }*/
+
+    Dune::Contact::NBodyAssembler<DeformedGrid, Vector> nBodyAssembler(bodyCount, bodyCount-1);
+    nBodyAssembler.setGrids(deformedGrids);
+
+    for (size_t i=0; i<couplings.size(); i++) {
+        nBodyAssembler.setCoupling(*couplings[i], i);
+    }
+
+    nBodyAssembler.assembleTransferOperator();
+    nBodyAssembler.assembleObstacle();
+
+    // set up boundary conditions
+    std::vector<BoundaryPatch<DefLeafGridView>> neumannBoundaries(bodyCount);
+    std::vector<BoundaryPatch<DefLeafGridView>> surfaces(bodyCount);
+
+    // friction boundary
+    std::vector<BoundaryPatch<DefLeafGridView>> frictionalBoundaries(bodyCount);
+    std::vector<Dune::BitSetVector<1>> frictionalNodes(bodyCount);
+
+    // Dirichlet boundary
+    std::vector<Dune::BitSetVector<dims>> noNodes(bodyCount);
+    std::vector<Dune::BitSetVector<dims>> dirichletNodes(bodyCount);
+
+    // set up functions for time-dependent boundary conditions
+    using Function = Dune::VirtualFunction<double, double>;
+
+    std::vector<const Function*> velocityDirichletFunctions(grids.size());
+    const Function& neumannFunction = NeumannCondition();
+
+    for (size_t i=0; i<grids.size(); i++) {
+      const auto& leafVertexCount = leafVertexCounts[i];
+
+      std::cout << "Grid" << i << " Number of DOFs: " << leafVertexCount << std::endl;
+
+      // Neumann boundary
+      neumannBoundaries[i] = BoundaryPatch<DefLeafGridView>(*leafViews[i]);
+
+      // friction boundary
+      auto& frictionalBoundary = frictionalBoundaries[i];
+      if (i==0) {
+        frictionalBoundary = BoundaryPatch<DefLeafGridView>(leafFaces[i]->upper);
+      } else if (i==bodyCount-1) {
+        frictionalBoundary = BoundaryPatch<DefLeafGridView>(leafFaces[i]->lower);
+      } else {
+          frictionalBoundary = BoundaryPatch<DefLeafGridView>(leafFaces[i]->lower);
+          frictionalBoundary.addPatch(BoundaryPatch<DefLeafGridView>(leafFaces[i]->upper));
+      }
+      frictionalNodes[i] = *frictionalBoundary.getVertices();
+      //surfaces[i] = BoundaryPatch<GridView>(myFaces.upper);
+
+      // TODO: Dirichlet Boundary
+      velocityDirichletFunctions[i] = new VelocityDirichletCondition();
+
+      noNodes[i] = Dune::BitSetVector<dims>(leafVertexCount);
+      dirichletNodes[i] = Dune::BitSetVector<dims>(leafVertexCount);
+      auto& gridDirichletNodes = dirichletNodes[i];
+      for (int j=0; j<leafVertexCount; j++) {
+        if (leafFaces[i]->right.containsVertex(j))
+          gridDirichletNodes[j][0] = true;
+
+        if (leafFaces[i]->lower.containsVertex(j))
+          gridDirichletNodes[j][0] = true;
+
+        #if MY_DIM == 3
+        if (leafFaces[i]->front.containsVertex(j) || leafFaces[i]->back.containsVertex(j))
+            gridDirichletNodes[j][2] = true;
+        #endif
+      }
+    }
+
+    // set up individual assemblers for each body, assemble problem (matrices, forces, rhs)
+    std::vector<std::shared_ptr<Assembler>> assemblers(bodyCount);
+    Matrices<Matrix> matrices(bodyCount);
+
+    std::vector<Vector> gravityFunctionals(bodyCount);
+    std::vector<std::function<void(double, Vector&)>> externalForces(bodyCount);
+    std::vector<const EnergyNorm<ScalarMatrix, ScalarVector>* > stateEnergyNorms(bodyCount);
+
+    for (size_t i=0; i<assemblers.size(); i++) {
+      auto& assembler = assemblers[i];
+      assembler = std::make_shared<Assembler>(*leafViews[i]);
+
+      assembler->assembleElasticity(body.getYoungModulus(), body.getPoissonRatio(), *matrices.elasticity[i]);
+      assembler->assembleViscosity(body.getShearViscosityField(), body.getBulkViscosityField(), *matrices.damping[i]);
+      assembler->assembleMass(body.getDensityField(), *matrices.mass[i]);
+
+      ScalarMatrix relativeFrictionalBoundaryMass;
+      assembler->assembleFrictionalBoundaryMass(frictionalBoundaries[i], relativeFrictionalBoundaryMass);
+      relativeFrictionalBoundaryMass /= frictionalBoundaries[i].area();
+      stateEnergyNorms[i] = new EnergyNorm<ScalarMatrix, ScalarVector>(relativeFrictionalBoundaryMass);
+
+      // assemble forces
+      assembler->assembleBodyForce(body.getGravityField(), gravityFunctionals[i]);
+
+      // Problem formulation: right-hand side
+      externalForces[i] =
+            [&](double _relativeTime, Vector &_ell) {
+              assemblers[i]->assembleNeumann(neumannBoundaries[i], _ell, neumannFunction,
+                                          _relativeTime);
+              _ell += gravityFunctionals[i];
+      };
+    }
+
+    /* Jonny 2bcontact
+    // make dirichlet bitfields containing dirichlet information for both grids
+           int size = rhs[0].size() + rhs[1].size();
+
+           Dune::BitSetVector<dims> totalDirichletNodes(size);
+
+           for (size_t i=0; i<dirichletNodes[0].size(); i++)
+               for (int j=0; j<dims; j++)
+                   totalDirichletNodes[i][j] = dirichletNodes[0][i][j];
+
+           int offset = rhs[0].size();
+           for (size_t i=0; i<dirichletNodes[1].size(); i++)
+               for (int j=0; j<dims; j++)
+                   totalDirichletNodes[offset + i][j] = dirichletNodes[1][i][j];
+
+           // assemble separate linear elasticity problems
+           std::array<MatrixType,2> stiffnessMatrix;
+           std::array<const MatrixType*, 2> submat;
+
+           for (size_t i=0; i<2; i++) {
+               OperatorAssembler<P1Basis,P1Basis> globalAssembler(*p1Basis[i],*p1Basis[i]);
+
+               double s = (i==0) ? E0 : E1;
+               StVenantKirchhoffAssembler<GridType, P1Basis::LocalFiniteElement, P1Basis::LocalFiniteElement> localAssembler(s, nu);
+
+               globalAssembler.assemble(localAssembler, stiffnessMatrix[i]);
+
+               submat[i] = &stiffnessMatrix[i];
+           }
+
+           MatrixType bilinearForm;
+           contactAssembler.assembleJacobian(submat, bilinearForm);
+ */
+
+    using MyProgramState = ProgramState<Vector, ScalarVector>;
+    MyProgramState programState(leafVertexCounts);
+    auto const firstRestart = parset.get<size_t>("io.restarts.first");
+    auto const restartSpacing = parset.get<size_t>("io.restarts.spacing");
+    auto const writeRestarts = parset.get<bool>("io.restarts.write");
+    auto const writeData = parset.get<bool>("io.data.write");
+    bool const handleRestarts = writeRestarts or firstRestart > 0;
+
+
+    auto dataFile =
+        writeData ? std::make_unique<HDF5::File>("output.h5") : nullptr;
+
+    auto restartFile = handleRestarts
+                           ? std::make_unique<HDF5::File>(
+                                 "restarts.h5",
+                                 writeRestarts ? HDF5::Access::READWRITE
+                                               : HDF5::Access::READONLY)
+                           : nullptr;
+
+
+    auto restartIO = handleRestarts
+                         ? std::make_unique<RestartIO<MyProgramState>>(
+                               *restartFile, leafVertexCounts)
+                         : nullptr;
+
+    if (firstRestart > 0) // automatically adjusts the time and timestep
+      restartIO->read(firstRestart, programState);
+    else
+     programState.setupInitialConditions(parset, nBodyAssembler, externalForces,
+                                          matrices, assemblers, dirichletNodes,
+                                          noNodes, frictionalBoundaries, body);
+
+
+
+    // assemble friction
+    std::vector<std::shared_ptr<MyGlobalFrictionData<LocalVector>>> frictionInfo(weakPatches.size());
+    std::vector<std::shared_ptr<GlobalFriction<Matrix, Vector>>> globalFriction(weakPatches.size());
+    for (size_t i=0; i<frictionInfo.size(); i++) {
+        frictionInfo[i] = std::make_shared<MyGlobalFrictionData<LocalVector>>(parset.sub("boundary.friction"), weakPatches[i]);
+        globalFriction[i] = assemblers[i]->assembleFrictionNonlinearity(parset.get<Config::FrictionModel>("boundary.friction.frictionModel"), frictionalBoundaries[i], *frictionInfo[i], programState.weightedNormalStress[i]);
+        globalFriction[i]->updateAlpha(programState.alpha[i]);
+    }
+
+
+    using MyVertexBasis = typename Assembler::VertexBasis;
+    using MyCellBasis = typename Assembler::CellBasis;
+    std::vector<Vector> vertexCoordinates(leafVertexCounts.size());
+    std::vector<const MyVertexBasis* > vertexBases(leafVertexCounts.size());
+    std::vector<const MyCellBasis* > cellBases(leafVertexCounts.size());
+
+    for (size_t i=0; i<vertexCoordinates.size(); i++) {
+      vertexBases[i] = &(assemblers[i]->vertexBasis);
+      cellBases[i] = &(assemblers[i]->cellBasis);
+
+      auto& vertexCoords = vertexCoordinates[i];
+      vertexCoords.resize(leafVertexCounts[i]);
+
+      Dune::MultipleCodimMultipleGeomTypeMapper<
+          DefLeafGridView, Dune::MCMGVertexLayout> const vertexMapper(*leafViews[i], Dune::mcmgVertexLayout());
+      for (auto &&v : vertices(*leafViews[i]))
+        vertexCoords[vertexMapper.index(v)] = geoToPoint(v.geometry());
+    }
+
+
+    auto dataWriter =
+        writeData ? std::make_unique<
+                        HDF5Writer<MyProgramState, MyVertexBasis, DefLeafGridView>>(
+                        *dataFile, vertexCoordinates, vertexBases,
+                        surfaces, frictionalBoundaries, weakPatches)
+                  : nullptr;
+
+    const MyVTKWriter<MyVertexBasis, MyCellBasis> vtkWriter(cellBases, vertexBases, "body");
+
+    IterationRegister iterationCount;
+
+    auto const report = [&](bool initial = false) {
+      if (writeData) {
+        dataWriter->reportSolution(programState, globalFriction);
+        if (!initial)
+          dataWriter->reportIterations(programState, iterationCount);
+        dataFile->flush();
+      }
+
+      if (writeRestarts and !initial and
+          programState.timeStep % restartSpacing == 0) {
+        restartIO->write(programState);
+        restartFile->flush();
+      }
+
+      if (parset.get<bool>("io.printProgress"))
+        std::cout << "timeStep = " << std::setw(6) << programState.timeStep
+                  << ", time = " << std::setw(12) << programState.relativeTime
+                  << ", tau = " << std::setw(12) << programState.relativeTau
+                  << std::endl;
+
+      if (parset.get<bool>("io.vtk.write")) {
+        std::vector<ScalarVector> stress(assemblers.size());
+
+        for (size_t i=0; i<stress.size(); i++) {
+          assemblers[i]->assembleVonMisesStress(body.getYoungModulus(),
+                                           body.getPoissonRatio(),
+                                           programState.u[i], stress[i]);
+        }
+
+        vtkWriter.write(programState.timeStep, programState.u, programState.v,
+                        programState.alpha, stress);
+      }
+    };
+    report(true);
+
+    // Set up TNNMG solver
+    using NonlinearFactory = SolverFactory<DeformedGrid, GlobalFriction<Matrix, Vector>, Matrix, Vector>;
+    NonlinearFactory factory(parset.sub("solver.tnnmg"), nBodyAssembler, dirichletNodes);
+
+    using MyUpdater = Updaters<RateUpdater<Vector, Matrix, Function, dims>,
+                               StateUpdater<ScalarVector, Vector>>;
+
+    std::vector<double> L(bodyCount, parset.get<double>("boundary.friction.L"));
+    std::vector<double> V0(bodyCount, parset.get<double>("boundary.friction.V0"));
+    MyUpdater current(
+        initRateUpdater(parset.get<Config::scheme>("timeSteps.scheme"),
+                        velocityDirichletFunctions, dirichletNodes, matrices,
+                        programState.u, programState.v, programState.a),
+        initStateUpdater<ScalarVector, Vector>(
+            parset.get<Config::stateModel>("boundary.friction.stateModel"),
+            programState.alpha, frictionalNodes,
+            L, V0));
+
+
+    auto const refinementTolerance =
+        parset.get<double>("timeSteps.refinementTolerance");
+    auto const mustRefine = [&](MyUpdater &coarseUpdater,
+                                MyUpdater &fineUpdater) {
+      std::vector<ScalarVector> coarseAlpha;
+      coarseUpdater.state_->extractAlpha(coarseAlpha);
+
+      std::vector<ScalarVector> fineAlpha;
+      fineUpdater.state_->extractAlpha(fineAlpha);
+
+      ScalarVector::field_type energyNorm = 0;
+      for (size_t i=0; i<stateEnergyNorms.size(); i++) {
+          energyNorm += stateEnergyNorms[i]->diff(fineAlpha[i], coarseAlpha[i]);
+      }
+      return energyNorm > refinementTolerance;
+    };
+
+
+    std::signal(SIGXCPU, handleSignal);
+    std::signal(SIGINT, handleSignal);
+    std::signal(SIGTERM, handleSignal);
+
+    AdaptiveTimeStepper<NonlinearFactory, MyUpdater,
+                        EnergyNorm<ScalarMatrix, ScalarVector>>
+        adaptiveTimeStepper(factory, parset, globalFriction, current,
+                            programState.relativeTime, programState.relativeTau,
+                            externalForces, stateEnergyNorms, mustRefine);
+    while (!adaptiveTimeStepper.reachedEnd()) {
+      programState.timeStep++;
+
+      iterationCount = adaptiveTimeStepper.advance();
+
+      programState.relativeTime = adaptiveTimeStepper.relativeTime_;
+      programState.relativeTau = adaptiveTimeStepper.relativeTau_;
+      current.rate_->extractDisplacement(programState.u);
+      current.rate_->extractVelocity(programState.v);
+      current.rate_->extractAcceleration(programState.a);
+      current.state_->extractAlpha(programState.alpha);
+
+      for (size_t i=0; i<deformedGridComplex.size() ; i++) {
+        deformedGridComplex.setDeformation(programState.u[i], i);
+      }
+      nBodyAssembler.assembleTransferOperator();
+      nBodyAssembler.assembleObstacle();
+
+      report();
+
+      if (terminationRequested) {
+        std::cerr << "Terminating prematurely" << std::endl;
+        break;
+      }
+    }
+
+  } catch (Dune::Exception &e) {
+    Dune::derr << "Dune reported error: " << e << std::endl;
+  } catch (std::exception &e) {
+    std::cerr << "Standard exception: " << e.what() << std::endl;
+  }
+}

src/multi-body-problem.cfg

+# -*- mode:conf -*-
+gravity         = 9.81  # [m/s^2]
+
+[io]
+data.write      = false #true
+printProgress   = false
+restarts.first  = 0
+restarts.spacing= 20
+restarts.write  = false #true
+vtk.write       = false
+
+[problem]
+finalTime       = 1000  # [s]
+bodyCount       = 2
+
+[body]
+bulkModulus     = 0.5e5 # [Pa]
+poissonRatio    = 0.3   # [1]
+[body.elastic]
+density         = 900   # [kg/m^3]
+shearViscosity  = 1e3   # [Pas]
+bulkViscosity   = 1e3   # [Pas]
+[body.viscoelastic]
+density         = 1000  # [kg/m^3]
+shearViscosity  = 1e4   # [Pas]
+bulkViscosity   = 1e4   # [Pas]
+
+[boundary.friction]
+C               = 10    # [Pa]
+mu0             = 0.7   # [ ]
+V0              = 5e-5  # [m/s]
+L               = 2.25e-5 # [m]
+initialAlpha    = 0     # [ ]
+stateModel      = AgeingLaw
+frictionModel   = Regularised
+[boundary.friction.weakening]
+a               = 0.002 # [ ]
+b               = 0.017 # [ ]
+[boundary.friction.strengthening]
+a               = 0.020 # [ ]
+b               = 0.005 # [ ]
+
+[timeSteps]
+scheme = newmark
+
+[u0.solver]
+maximumIterations = 100000
+verbosity         = quiet
+
+[a0.solver]
+maximumIterations = 100000
+verbosity         = quiet
+
+[v.solver]
+maximumIterations = 100000
+verbosity         = quiet
+
+[v.fpi]
+maximumIterations = 10000
+lambda            = 0.5
+
+[solver.tnnmg.linear]
+maxiumumIterations = 100000
+pre                = 3
+cycle              = 1  # 1 = V, 2 = W, etc.
+post               = 3
+
+[solver.tnnmg.main]
+pre   = 1
+multi = 5 # number of multigrid steps
+post  = 0

src/one-body-problem.cc

@@ -31,9 +31,14 @@
 #include <dune/fufem/formatstring.hh>
 
 #include <dune/solvers/norms/energynorm.hh>
+
+
+/*
 #include <dune/solvers/solvers/loopsolver.hh>
 #include <dune/solvers/solvers/solver.hh>
 #include <dune/tnnmg/problem-classes/convexproblem.hh>
+*/
+
 
 #include <dune/tectonic/geocoordinate.hh>
 #include <dune/tectonic/myblockproblem.hh>
@@ -62,13 +67,18 @@
 #include "time-stepping/updaters.hh"
 #include "vtk.hh"
 
+// for getcwd
+#include <unistd.h>
+
+#define USE_OLD_TNNMG
+
 size_t const dims = MY_DIM;
 
 Dune::ParameterTree getParameters(int argc, char *argv[]) {
   Dune::ParameterTree parset;
-  Dune::ParameterTreeParser::readINITree("one-body-problem.cfg", parset);
+  Dune::ParameterTreeParser::readINITree("/home/mi/podlesny/software/dune/dune-tectonic/src/one-body-problem.cfg", parset);
   Dune::ParameterTreeParser::readINITree(
-      Dune::Fufem::formatString("one-body-problem-%dD.cfg", dims), parset);
+      Dune::Fufem::formatString("/home/mi/podlesny/software/dune/dune-tectonic/src/one-body-problem-%dD.cfg", dims), parset);
   Dune::ParameterTreeParser::readOptions(argc, argv, parset);
   return parset;
 }
@@ -79,6 +89,14 @@ void handleSignal(int signum) { terminationRequested = true; }
 int main(int argc, char *argv[]) {
   try {
     Dune::MPIHelper::instance(argc, argv);
+
+    char buffer[256];
+    char *val = getcwd(buffer, sizeof(buffer));
+    if (val) {
+        std::cout << buffer << std::endl;
+        std::cout << argv[0] << std::endl;
+    }
+
     auto const parset = getParameters(argc, argv);
 
     MyGeometry::render();
@@ -140,6 +158,7 @@ int main(int argc, char *argv[]) {
 #endif
     }
 
+
     // Set up functions for time-dependent boundary conditions
     using Function = Dune::VirtualFunction<double, double>;
     Function const &velocityDirichletFunction = VelocityDirichletCondition();
@@ -184,8 +203,10 @@ int main(int argc, char *argv[]) {
     auto const writeData = parset.get<bool>("io.data.write");
     bool const handleRestarts = writeRestarts or firstRestart > 0;
 
+    /*
     auto dataFile =
         writeData ? std::make_unique<HDF5::File>("output.h5") : nullptr;
+
     auto restartFile = handleRestarts
                            ? std::make_unique<HDF5::File>(
                                  "restarts.h5",
@@ -214,7 +235,7 @@ int main(int argc, char *argv[]) {
     Vector vertexCoordinates(leafVertexCount);
     {
       Dune::MultipleCodimMultipleGeomTypeMapper<
-          GridView, Dune::MCMGVertexLayout> const vertexMapper(leafView);
+          GridView, Dune::MCMGVertexLayout> const vertexMapper(leafView, Dune::mcmgVertexLayout());
       for (auto &&v : vertices(leafView))
         vertexCoordinates[vertexMapper.index(v)] = geoToPoint(v.geometry());
     }
@@ -229,6 +250,7 @@ int main(int argc, char *argv[]) {
     MyVTKWriter<MyVertexBasis, typename MyAssembler::CellBasis> const vtkWriter(
         myAssembler.cellBasis, myAssembler.vertexBasis, "obs");
 
+
     IterationRegister iterationCount;
     auto const report = [&](bool initial = false) {
       if (writeData) {
@@ -261,6 +283,7 @@ int main(int argc, char *argv[]) {
     };
     report(true);
 
+
     // Set up TNNMG solver
     using NonlinearFactory = SolverFactory<
         dims,
@@ -321,6 +344,8 @@ int main(int argc, char *argv[]) {
         break;
       }
     }
+
+    */
   } catch (Dune::Exception &e) {
     Dune::derr << "Dune reported error: " << e << std::endl;
   } catch (std::exception &e) {

src/program_state.hh

@@ -3,66 +3,139 @@
 
 #include <dune/common/parametertree.hh>
 
+#include <dune/matrix-vector/axpy.hh>
+
 #include <dune/fufem/boundarypatch.hh>
 #include <dune/tnnmg/nonlinearities/zerononlinearity.hh>
 #include <dune/tnnmg/problem-classes/blocknonlineartnnmgproblem.hh>
 
+#include <dune/contact/assemblers/nbodyassembler.hh>
+
 #include <dune/tectonic/body.hh>
 
 #include "assemblers.hh"
 #include "matrices.hh"
 #include "spatial-solving/solverfactory.hh"
 
+template <class VectorTEMPLATE, class ScalarVectorTEMPLATE> class BodyState {
+public:
+  using Vector = VectorTEMPLATE;
+  using ScalarVector = ScalarVectorTEMPLATE;
+
+  BodyState(Vector * _u, Vector * _v, Vector * _a, ScalarVector * _alpha, ScalarVector * _weightedNormalStress)
+    : u(_u),
+      v(_v),
+      a(_a),
+      alpha(_alpha),
+      weightedNormalStress(_weightedNormalStress) {}
+
+public:
+  Vector * const u;
+  Vector * const v;
+  Vector * const a;
+  ScalarVector * const alpha;
+  ScalarVector * const weightedNormalStress;
+};
+
+
 template <class VectorTEMPLATE, class ScalarVectorTEMPLATE> class ProgramState {
 public:
   using Vector = VectorTEMPLATE;
   using ScalarVector = ScalarVectorTEMPLATE;
+  using BodyState = BodyState<Vector, ScalarVector>;
+
+  ProgramState(const std::vector<size_t>& leafVertexCounts)
+    : bodyCount_(leafVertexCounts.size()),
+      bodies(bodyCount_),
+      u(bodyCount_),
+      v(bodyCount_),
+      a(bodyCount_),
+      alpha(bodyCount_),
+      weightedNormalStress(bodyCount_) {
+
+    for (size_t i=0; i<bodyCount_; i++) {
+      size_t leafVertexCount = leafVertexCounts[i];
+
+      u[i].resize(leafVertexCount),
+      v[i].resize(leafVertexCount),
+      a[i].resize(leafVertexCount),
+      alpha[i].resize(leafVertexCount),
+      weightedNormalStress[i].resize(leafVertexCount),
+
+      bodies[i] = new BodyState(&u[i], &v[i], &a[i], &alpha[i], &weightedNormalStress[i]);
+    }
+  }
+
+  ~ProgramState() {
+    for (size_t i=0; i<bodyCount_; i++) {
+      delete bodies[i];
+    }
+  }
+
+  size_t size() const {
+      return bodyCount_;
+  }
 
-  ProgramState(int leafVertexCount)
-      : u(leafVertexCount),
-        v(leafVertexCount),
-        a(leafVertexCount),
-        alpha(leafVertexCount),
-        weightedNormalStress(leafVertexCount) {}
 
   // Set up initial conditions
   template <class Matrix, class GridView>
   void setupInitialConditions(
-      Dune::ParameterTree const &parset,
-      std::function<void(double, Vector &)> externalForces,
-      Matrices<Matrix> const matrices,
-      MyAssembler<GridView, Vector::block_type::dimension> const &myAssembler,
-      Dune::BitSetVector<Vector::block_type::dimension> const &dirichletNodes,
-      Dune::BitSetVector<Vector::block_type::dimension> const &noNodes,
-      BoundaryPatch<GridView> const &frictionalBoundary,
-      Body<Vector::block_type::dimension> const &body) {
+      const Dune::ParameterTree& parset,
+      const Dune::Contact::NBodyAssembler<typename GridView::Grid, Vector>& nBodyAssembler,
+      std::vector<std::function<void(double, Vector &)>> externalForces,
+      const Matrices<Matrix>& matrices,
+      const std::vector<std::shared_ptr<MyAssembler<GridView, Vector::block_type::dimension>>>& assemblers,
+      const std::vector<Dune::BitSetVector<Vector::block_type::dimension>>& dirichletNodes,
+      const std::vector<Dune::BitSetVector<Vector::block_type::dimension>>& noNodes,
+      const std::vector<BoundaryPatch<GridView>>& frictionalBoundaries,
+      const Body<Vector::block_type::dimension>& body) {
 
     using LocalVector = typename Vector::block_type;
-    using LocalMatrix = typename Matrix::block_type;
+    //using LocalMatrix = typename Matrix::block_type;
     auto constexpr dims = LocalVector::dimension;
 
+
     // Solving a linear problem with a multigrid solver
     auto const solveLinearProblem = [&](
-        Dune::BitSetVector<dims> const &_dirichletNodes, Matrix const &_matrix,
-        Vector const &_rhs, Vector &_x,
+        const std::vector<Dune::BitSetVector<dims>>& _dirichletNodes, const std::vector<std::shared_ptr<Matrix>>& _matrices,
+        const std::vector<Vector>& _rhs, std::vector<Vector>& _x,
         Dune::ParameterTree const &_localParset) {
 
-      using LinearFactory = SolverFactory<
-          dims, BlockNonlinearTNNMGProblem<ConvexProblem<
-                    ZeroNonlinearity<LocalVector, LocalMatrix>, Matrix>>,
-          typename GridView::Grid>;
-      ZeroNonlinearity<LocalVector, LocalMatrix> zeroNonlinearity;
+      std::vector<const Matrix*> matrices_ptr(_matrices.size());
+      for (size_t i=0; i<matrices_ptr.size(); i++) {
+            matrices_ptr[i] = _matrices[i].get();
+      }
 
-      LinearFactory factory(parset.sub("solver.tnnmg"), // FIXME
-                            myAssembler.gridView.grid(), _dirichletNodes);
+      /*std::vector<Matrix> matrices(velocityMatrices.size());
+          std::vector<Vector> rhs(velocityRHSs.size());
+          for (size_t i=0; i<globalFriction_.size(); i++) {
+            matrices[i] = velocityMatrices[i];
+            rhs[i] = velocityRHSs[i];
 
-      typename LinearFactory::ConvexProblem convexProblem(
-          1.0, _matrix, zeroNonlinearity, _rhs, _x);
-      typename LinearFactory::BlockProblem problem(parset, convexProblem);
+            globalFriction_[i]->addHessian(v_rel[i], matrices[i]);
+            globalFriction_[i]->addGradient(v_rel[i], rhs[i]);
+
+            matrices_ptr[i] = &matrices[i];
+          }*/
+
+      // assemble full global contact problem
+      Matrix bilinearForm;
+      nBodyAssembler.assembleJacobian(matrices_ptr, bilinearForm);
+
+      Vector totalRhs;
+      nBodyAssembler.assembleRightHandSide(_rhs, totalRhs);
+
+      Vector totalX;
+      nBodyAssembler.nodalToTransformed(_x, totalX);
+
+      using LinearFactory = SolverFactory<typename GridView::Grid, GlobalFriction<Matrix, Vector>, Matrix, Vector>;
+      LinearFactory factory(parset.sub("solver.tnnmg"), nBodyAssembler, _dirichletNodes);
 
       auto multigridStep = factory.getStep();
-      multigridStep->setProblem(_x, problem);
-      EnergyNorm<Matrix, Vector> const norm(_matrix);
+      multigridStep->setProblem(bilinearForm, totalX, totalRhs);
+
+      const EnergyNorm<Matrix, Vector> norm(bilinearForm);
+
       LoopSolver<Vector> solver(
           multigridStep.get(), _localParset.get<size_t>("maximumIterations"),
           _localParset.get<double>("tolerance"), &norm,
@@ -71,17 +144,24 @@ template <class VectorTEMPLATE, class ScalarVectorTEMPLATE> class ProgramState {
 
       solver.preprocess();
       solver.solve();
+
+      nBodyAssembler.postprocess(multigridStep->getSol(), _x);
     };
 
     timeStep = 0;
     relativeTime = 0.0;
     relativeTau = 1e-6;
 
-    Vector ell0(u.size());
-    externalForces(relativeTime, ell0);
+    std::vector<Vector> ell0(bodyCount_);
+    for (size_t i=0; i<bodyCount_; i++) {
+      // Initial velocity
+      v[i] = 0.0;
 
-    // Initial velocity
-    v = 0.0;
+      ell0[i].resize(u[i].size());
+      ell0[i] = 0.0;
+      // TODO
+      //externalForces[i](relativeTime, ell0[i]);
+    }
 
     // Initial displacement: Start from a situation of minimal stress,
     // which is automatically attained in the case [v = 0 = a].
@@ -91,29 +171,37 @@ template <class VectorTEMPLATE, class ScalarVectorTEMPLATE> class ProgramState {
 
     // Initial acceleration: Computed in agreement with Ma = ell0 - Au
     // (without Dirichlet constraints), again assuming dPhi(v = 0) = 0
-    Vector accelerationRHS = ell0;
-    Arithmetic::subtractProduct(accelerationRHS, matrices.elasticity, u);
-    solveLinearProblem(noNodes, matrices.mass, accelerationRHS, a,
-                       parset.sub("a0.solver"));
+    std::vector<Vector> accelerationRHS = ell0;
+    for (size_t i=0; i<bodyCount_; i++) {
+      // Initial state
+      alpha[i] = parset.get<double>("boundary.friction.initialAlpha");
+
+      // Initial normal stress
+      assemblers[i]->assembleWeightedNormalStress(
+        frictionalBoundaries[i], weightedNormalStress[i], body.getYoungModulus(),
+        body.getPoissonRatio(), u[i]);
 
-    // Initial state
-    alpha = parset.get<double>("boundary.friction.initialAlpha");
+      Dune::MatrixVector::subtractProduct(accelerationRHS[i], *matrices.elasticity[i], u[i]);
+    }
 
-    // Initial normal stress
-    myAssembler.assembleWeightedNormalStress(
-        frictionalBoundary, weightedNormalStress, body.getYoungModulus(),
-        body.getPoissonRatio(), u);
+    solveLinearProblem(noNodes, matrices.mass, accelerationRHS, a,
+                       parset.sub("a0.solver"));
   }
 
+private:
+  const size_t bodyCount_;
+
 public:
-  Vector u;
-  Vector v;
-  Vector a;
-  ScalarVector alpha;
-  ScalarVector weightedNormalStress;
+  std::vector<BodyState* > bodies;
+  std::vector<Vector> u;
+  std::vector<Vector> v;
+  std::vector<Vector> a;
+  std::vector<ScalarVector> alpha;
+  std::vector<ScalarVector> weightedNormalStress;
   double relativeTime;
   double relativeTau;
   size_t timeStep;
-};
 
+
+};
 #endif