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.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