From 635d9d3e7936d4c886fb0f1e5327a8ff2b1a8c8f Mon Sep 17 00:00:00 2001
From: Jonathan Youett <youett@math.fu-berlin.de>
Date: Mon, 12 Nov 2018 10:06:16 +0100
Subject: [PATCH] Assembler of the contact problem when a primal-dual method is
 used

---
 .../common/primaldualcontactproblem.cc        | 486 ++++++++++++++++++
 .../common/primaldualcontactproblem.hh        | 156 ++++++
 2 files changed, 642 insertions(+)
 create mode 100644 dune/contact/common/primaldualcontactproblem.cc
 create mode 100644 dune/contact/common/primaldualcontactproblem.hh

diff --git a/dune/contact/common/primaldualcontactproblem.cc b/dune/contact/common/primaldualcontactproblem.cc
new file mode 100644
index 00000000..f3962ca6
--- /dev/null
+++ b/dune/contact/common/primaldualcontactproblem.cc
@@ -0,0 +1,486 @@
+// -*- tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 2 -*-
+// vi: set ts=4 sw=2 et sts=2:
+
+#include <dune/istl/matrixindexset.hh>
+#include <dune/istl/matrixmatrix.hh>
+
+#include <dune/fufem/assemblers/functionalassembler.hh>
+#include <dune/fufem/assemblers/operatorassembler.hh>
+#include <dune/fufem/quadraturerules/quadraturerulecache.hh>
+#include <dune/solvers/computeenergy.hh>
+
+namespace Dune {
+namespace Contact {
+
+template <class VectorType, class MaterialType>
+void PrimalDualContactProblem<VectorType, MaterialType>::updateActiveSetAndLagrangeMultiplier(
+    ScalarVector& lagrangeMultiplier) {
+  // the Lagrange multiplier with indexing of the old iteration has to be
+  // translated to the new one
+  auto globalToLocalOld =
+      this->contactAssembler_->storedNmPatch(0).makeGlobalToLocal();
+  auto coupling = std::dynamic_pointer_cast<ContactCoupling>(
+      this->contactAssembler_->getContactCouplings()[0]);
+  auto globalToLocalNew = coupling->nonmortarBoundary().makeGlobalToLocal();
+
+  // evaluate obstacles (assumes that grid-glue has been rebuild)
+  auto obstacles = coupling->assembleObstacles(false);
+
+  changedActiveNodes_ = 0;
+
+  Dune::BitSetVector<1> newActiveSet(obstacles.size(), false);
+  ScalarVector newLagrangeMultiplier(obstacles.size());
+  newLagrangeMultiplier = 0;
+
+  for (size_t i = 0; i < globalToLocalNew.size(); i++)
+    if (globalToLocalNew[i] >= 0) {
+      field_type test = -priDualConstant_ * obstacles[globalToLocalNew[i]];
+      if (globalToLocalOld[i] >= 0) {
+        test += lagrangeMultiplier[globalToLocalOld[i]];
+        newLagrangeMultiplier[globalToLocalNew[i]] = lagrangeMultiplier[globalToLocalOld[i]];
+      }
+      newActiveSet[globalToLocalNew[i]] = (test > 0);
+
+      // check if activity changed
+      if ((globalToLocalOld[i] >= 0 and (newActiveSet[globalToLocalNew[i]] !=
+                                         activeSet_[globalToLocalOld[i]])) or
+          (newActiveSet[globalToLocalNew[i]] == true and
+           globalToLocalOld[i] == -1))
+        changedActiveNodes_++;
+    } else // check if activity changed
+        if (globalToLocalOld[i] >= 0 and activeSet_[globalToLocalOld[i]].any())
+      changedActiveNodes_++;
+
+  activeSet_ = newActiveSet;
+  lagrangeMultiplier = newLagrangeMultiplier;
+}
+
+template <class VectorType, class MaterialType>
+void PrimalDualContactProblem<VectorType, MaterialType>::assembleLinearisations(
+    const std::vector<VectorType>& iterates) {
+  // assemble linear and quadratic terms
+  for (size_t i = 0; i < iterates.size(); i++) {
+
+    auto& material = (*this->materials_)[i];
+    const auto& basis = material.basis();
+
+    auto displace = std::make_shared<BasisGridFunction>(basis, iterates[i]);
+
+    // assemble quadratic term
+    OperatorAssembler<Basis, Basis> globalAssembler(basis, basis);
+    if (this->stiffMat_[i]) delete (this->stiffMat_[i]);
+
+    MatrixType* mat = new MatrixType;
+    globalAssembler.assemble(material.secondDerivative(displace), *mat);
+    this->stiffMat_[i] = mat;
+
+    // assemble linear term
+    this->rhs_[i].resize(iterates[i].size());
+
+    FunctionalAssembler<Basis> funcAssembler(basis);
+    funcAssembler.assemble(material.firstDerivative(displace), this->rhs_[i]);
+    this->rhs_[i] -= (*this->extForces_)[i];
+    this->rhs_[i] *= -1;
+  }
+  this->contactAssembler_->assembleLinearisations();
+
+  // store old patches in case they are still needed
+  this->contactAssembler_->storePatches();
+  currentIterates_ = iterates;
+}
+
+template <class VectorType, class MaterialType>
+void PrimalDualContactProblem<VectorType, MaterialType>::
+    assemblePrimalDualProblem(const ScalarVector& lagrangeMultiplier)
+{
+
+  if (full_linearisation_)
+    computeFDConstraintHessian(lagrangeMultiplier);
+  addPriDualMatrixIndices();
+  addPriDualMatrixEntries(lagrangeMultiplier);
+  assemblePriDualRhs(lagrangeMultiplier);
+}
+
+template <class VectorType, class MaterialType>
+void PrimalDualContactProblem<VectorType, MaterialType>::assemblePriDualRhs(const ScalarVector& lagrangeMultiplier)
+{
+  priDualRhs_.resize(priDualMatrix_.N());
+  priDualRhs_ = 0;
+
+  // first the energy and forces
+  for (size_t i = 0; i < 2; i++) {
+    size_t offset = (i==0) ? 0 : dim*this->rhs_[0].size();
+
+    for (size_t j = 0; j < this->rhs_[i].size(); j++)
+      for (int m = 0; m < dim; m++) // this already contains the minus
+        priDualRhs_[offset + j*dim + m] = this->rhs_[i][j][m];
+  }
+/*
+ * This should not be neccessary if the QP is reformulated appropriately
+  // then add the linearised constraints times the Lagrange multiplier
+  auto coupling = std::dynamic_pointer_cast<ContactCoupling>(
+      this->contactAssembler_->getContactCouplings()[0]);
+
+  const auto& nmMatrix = coupling->exactNonmortarMatrix();
+  const auto& mMatrix = coupling->exactMortarMatrix();
+  const auto& gridIdx = this->contactAssembler_->getCoupling(0).gridIdx_;
+
+  // then the parts involving the linearised constraints
+  for (size_t i = 0; i < 2; i++) {
+    const JacobianType* constraintMat = (i==0) ? &nmMatrix : &mMatrix;
+    const auto& mat = *constraintMat;
+    size_t offset = (gridIdx[i]==0) ? 0 : dim*this->rhs_[0].size();
+
+    for (size_t j = 0; j < mat.N(); j++) {
+      // add KKT derived w.r.t. Lagrange Multiplier, i.e. transposed jacobian
+      for (auto col = mat[j].begin(); col != mat[j].end(); col++)
+        for (int m = 0; m < dim; m++) // WARNING THIS MIGHT HAVE TO BE A PLUS HERE; DEPENDING HOW THE LAGRANGE FUNCTION LOOKS LIKE!
+          priDualRhs_[offset + col.index()*dim + m] += lagrangeMultiplier[j]*(*col)[0][m];
+    }
+  }
+*/
+  // the active constraints
+  int primalSize = priDualMatrix_.N() - lagrangeMultiplier.size();
+  const auto& constraints = getConstraints();
+  for (size_t i = 0; i < constraints.size(); i++)
+    if (activeSet_[i].any())
+      priDualRhs_[primalSize + i] = constraints[i];
+}
+
+template <class VectorType, class MaterialType>
+void PrimalDualContactProblem<VectorType, MaterialType>::computeFDConstraintHessian(const ScalarVector& lagrangeMultiplier)
+{
+
+  auto coupling = std::dynamic_pointer_cast<ContactCoupling>(
+      this->contactAssembler_->getContactCouplings()[0]);
+
+  const auto& gridIdx = this->contactAssembler_->getCoupling(0).gridIdx_;
+
+  std::array<std::vector<int>, 2> unredGlobalToLocal = {coupling->unrednonmortarBoundary().makeGlobalToLocal(),
+                                                        coupling->unredmortarBoundary().makeGlobalToLocal()};
+
+  std::array<std::vector<int>, 2> contactVertices;
+  contactVertices[0].resize(0);
+  contactVertices[1].resize(0);
+  for (size_t k=0; k<2; k++) {
+    int offset = (gridIdx[k]==0) ? 0 : currentIterates_[0].size();
+    for (size_t j=0; j<unredGlobalToLocal[k].size(); j++)
+      if(unredGlobalToLocal[k][j] >= 0)
+        contactVertices[k].push_back(offset + j);
+  }
+  assert((int) contactVertices[0].size() == coupling->unrednonmortarBoundary().numVertices());
+  assert((int) contactVertices[1].size() == coupling->unredmortarBoundary().numVertices());
+
+  int primalSize = dim * (this->rhs_[0].size() + this->rhs_[1].size());
+
+  // first create dense matrix to collect all entries
+  MatrixIndexSet indexSet(primalSize, primalSize);
+  for (int k=0; k<2; k++)
+    for (size_t i=0; i < contactVertices[k].size(); i++)
+      for (int l=0; l<2; l++)
+        for (size_t j=0; j < contactVertices[l].size(); j++)
+          indexSet.add(contactVertices[k][i], contactVertices[l][j]);
+
+  MatrixType denseConstraintHessian;
+  indexSet.exportIdx(denseConstraintHessian);
+  denseConstraintHessian = 0;
+
+  int counter(0);
+  // compute entries using a FD scheme
+  for (int k=0; k<2; k++) {
+    int offset = (gridIdx[k]==0) ? 0 : currentIterates_[0].size();
+    for (size_t j=0; j < contactVertices[k].size(); j++, counter+=dim)
+      addFDEntry(denseConstraintHessian, lagrangeMultiplier, k, contactVertices[k][j]-offset);
+  }
+
+  // remove all zero entries
+  condenseConstraintHessian(denseConstraintHessian);
+
+  // reset everything back to the beginning
+  this->deformGrids(currentIterates_);
+  coupling->buildProjection();
+  this->contactAssembler_->assembleLinearisations();
+}
+
+template <class VectorType, class MaterialType>
+void PrimalDualContactProblem<VectorType, MaterialType>::addFDEntry(MatrixType& denseConstraintHessian,
+                                                                    const ScalarVector& lagrangeMultiplier,
+                                                                    int gridIdx, int component)
+{
+
+  const auto& gridIndices = this->contactAssembler_->getCoupling(0).gridIdx_;
+  int colOffset = (gridIndices[gridIdx] == 0) ? 0 : currentIterates_[0].size();
+
+  size_t oldNumContactNodes = this->contactAssembler_->storedNmPatch(0).numVertices();
+  auto coupling = std::dynamic_pointer_cast<ContactCoupling>(this->contactAssembler_->getContactCouplings()[0]);
+
+  for (int k=0; k<dim; k++) {
+    // forward FD
+    currentIterates_[gridIdx][component][k] += fd_eps_;
+
+    this->deformGrids(currentIterates_);
+    coupling->buildProjection();
+    this->contactAssembler_->assembleLinearisations();
+
+    if (coupling->exactNonmortarMatrix().N() != oldNumContactNodes)
+      continue;
+
+    // make copies :-(
+    auto forwardNM = coupling->exactNonmortarMatrix();
+    auto forwardM = coupling->exactMortarMatrix();
+
+    // backward FD
+    currentIterates_[gridIdx][component][k] -= 2*fd_eps_;
+    this->deformGrids(currentIterates_);
+
+    coupling->buildProjection();
+    this->contactAssembler_->assembleLinearisations();
+
+    if (coupling->exactNonmortarMatrix().N() != oldNumContactNodes) {
+      std::cout<<"Skip component "<<component<<"\n";
+      continue;
+    }
+
+    const auto& backwardNM = coupling->exactNonmortarMatrix();
+    const auto& backwardM = coupling->exactMortarMatrix();
+
+    // non-mortar part
+    for (size_t i=0; i<forwardNM.N(); i++) {
+      int rowOffset = (gridIndices[0] == 0) ? 0 : currentIterates_[0].size();
+      auto colF = forwardNM[i].begin();
+      auto colB = backwardNM[i].begin();
+      for (; colF != forwardNM[i].end(); colF++,colB++) {
+        if (colF.index() != colB.index()) {
+          std::cout<<"Skip row, column indices not equal!\n";
+          break;
+        }
+        (*colF)[0] -= (*colB)[0];
+        (*colF)[0] /= 2*fd_eps_;
+        for (int m=0; m<dim; m++)
+          denseConstraintHessian[rowOffset + colF.index()][colOffset + component][m][k] += lagrangeMultiplier[i]*(*colF)[0][m];
+      }
+    }
+
+    // mortar part
+    for (size_t i=0; i<forwardM.N(); i++) {
+      int rowOffset = (gridIndices[1] == 0) ? 0 : currentIterates_[0].size();
+      auto colF = forwardM[i].begin();
+      auto colB = backwardM[i].begin();
+      for (; colF != forwardM[i].end(); colF++,colB++) {
+        if (colF.index() != colB.index()) {
+          std::cout<<"Skip row, column indices not equal!\n";
+          break;
+        }
+        (*colF)[0] -= (*colB)[0];
+        (*colF)[0] /= 2*fd_eps_;
+        for (int m=0; m < dim; m++)
+          denseConstraintHessian[rowOffset + colF.index()][colOffset + component][m][k] += (lagrangeMultiplier[i]*(*colF)[0][m]);
+      }
+    }
+    // reset deformation
+    currentIterates_[gridIdx][component][k] += fd_eps_;
+  }
+}
+
+template <class VectorType, class MaterialType>
+void PrimalDualContactProblem<VectorType, MaterialType>::addHardDirichlet(const ScalarVector& dirV, const BitSetVector<1>& dirN)
+{
+  for (size_t row = 0; row < priDualMatrix_.N(); row++)
+    for (auto col = priDualMatrix_[row].begin();
+         col != priDualMatrix_[row].end(); col++) {
+      // erase Dirichlet rows
+      if (row < dirV.size() and (dirN[row][0] == true)) {
+        (*col)[0][0] = (row == col.index()) ? 1 : 0;
+        priDualRhs_[row] = dirV[row];
+        continue;
+      }
+      if (col.index() >= dirV.size() or (dirN[col.index()][0] == false))
+        continue;
+      // move entry into rhs
+      priDualRhs_[row] -= (*col)[0][0]*dirV[col.index()];
+      *col = 0;
+    }
+}
+template <class VectorType, class MaterialType>
+void PrimalDualContactProblem<VectorType, MaterialType>::condenseConstraintHessian(const MatrixType& denseConstraintHessian)
+{
+  // condense into a scalar matrix
+  MatrixIndexSet indexSet(denseConstraintHessian.N()*dim, denseConstraintHessian.N()*dim);
+  for (size_t row = 0; row < denseConstraintHessian.N(); row++)
+    for (auto col = denseConstraintHessian[row].begin(); col != denseConstraintHessian[row].end(); col++)
+      for (int k = 0; k < dim; k++)
+        for (int l = 0; l < dim; l++)
+          if (std::fabs((*col)[k][l])>1e-13) {
+            indexSet.add(row*dim + k, col.index()*dim + l);
+            indexSet.add(col.index()*dim+l,row*dim + k);
+          }
+
+  indexSet.exportIdx(condensedConstraintHessian_);
+  condensedConstraintHessian_ = 0;
+
+  for (size_t row = 0; row < denseConstraintHessian.N(); row++)
+    for (auto col = denseConstraintHessian[row].begin(); col != denseConstraintHessian[row].end(); col++)
+      for (int k = 0; k < dim; k++)
+        for (int l = 0; l < dim; l++)
+          if (std::fabs((*col)[k][l])>1e-13) {
+            condensedConstraintHessian_[row*dim + k][col.index()*dim + l] = (*col)[k][l];
+            condensedConstraintHessian_[col.index()*dim + l][row*dim + k]= (*col)[k][l];
+          }
+}
+
+template <class VectorType, class MaterialType>
+void PrimalDualContactProblem<VectorType, MaterialType>::addPriDualMatrixEntries(const ScalarVector& lagrangeMultiplier)
+{
+  int primalSize = priDualMatrix_.N() - lagrangeMultiplier.size();
+
+  // first the linearisation of energy and forces
+  for (size_t i = 0; i < 2; i++) {
+    const auto& mat = *this->stiffMat_[i];
+    size_t offset = (i==0) ? 0 : dim*this->rhs_[0].size();
+
+    for (size_t j = 0; j < mat.N(); j++)
+      for (auto col = mat[j].begin(); col != mat[j].end(); col++)
+        for (int m = 0; m < dim; m++)
+          for (int n = 0; n < dim; n++)
+            priDualMatrix_[offset + j*dim + m][offset + col.index()*dim + n] = (*col)[m][n];
+  }
+
+  auto coupling = std::dynamic_pointer_cast<ContactCoupling>(
+      this->contactAssembler_->getContactCouplings()[0]);
+
+  const auto& nmMatrix = coupling->exactNonmortarMatrix();
+  const auto& mMatrix = coupling->exactMortarMatrix();
+  const auto& gridIdx = this->contactAssembler_->getCoupling(0).gridIdx_;
+
+  // then the parts involving the linearised constraints
+  for (size_t i = 0; i < 2; i++) {
+    const auto& mat = (i==0) ? nmMatrix : mMatrix;
+    size_t offset = (gridIdx[i]==0) ? 0 : dim*this->rhs_[0].size();
+
+    for (size_t j = 0; j < mat.N(); j++) {
+      // the active equality constraints and KKT w.r.t Lagrange multiplier
+      if (activeSet_[j].any()) {
+        for (auto col = mat[j].begin(); col != mat[j].end(); col++)
+          for (int m = 0; m < dim; m++) {
+            priDualMatrix_[primalSize + j][offset + col.index()*dim + m] -= (*col)[0][m];
+            priDualMatrix_[offset + col.index()*dim + m][primalSize + j] -= (*col)[0][m];
+          }
+      } else // this is added twice..
+        // and plain identity for the inactive constraints
+        priDualMatrix_[primalSize + j][primalSize + j] = 1.0;
+    }
+  }
+
+  if (full_linearisation_) {
+    // add Hessian of the constraint
+    for (size_t i = 0; i < condensedConstraintHessian_.N(); i++) {
+      auto& row = condensedConstraintHessian_[i];
+      for (auto col = row.begin(); col != row.end(); col++)
+        priDualMatrix_[i][col.index()] -= *col;
+    }
+  }
+}
+
+template <class VectorType, class MaterialType>
+void PrimalDualContactProblem<VectorType, MaterialType>::addPriDualMatrixIndices()
+{
+
+  auto coupling = std::dynamic_pointer_cast<ContactCoupling>(
+      this->contactAssembler_->getContactCouplings()[0]);
+
+  const auto& nmMatrix = coupling->exactNonmortarMatrix();
+  const auto& mMatrix = coupling->exactMortarMatrix();
+  const auto& gridIdx = this->contactAssembler_->getCoupling(0).gridIdx_;
+
+  int primalSize = dim * std::accumulate(this->rhs_.begin(), this->rhs_.end(), int{0},
+                  [](int size, auto vecIt) {return size + vecIt.size();});
+
+  MatrixIndexSet indexSet(primalSize + nmMatrix.N(), primalSize + nmMatrix.N());
+
+  // import primal linearisations indices and copy block entries into scalar
+  for (size_t i = 0; i < 2; i++) {
+    const auto& mat = *this->stiffMat_[i];
+    size_t offset = (i==0) ? 0 : dim*this->rhs_[0].size();
+
+    for (size_t j = 0; j < mat.N(); j++)
+      for (auto col = mat[j].begin(); col != mat[j].end(); col++)
+        for (int m = 0; m < dim; m++)
+          for (int n = 0; n < dim; n++)
+            indexSet.add(offset + j*dim + m, offset + col.index()*dim + n);
+  }
+
+  // then the parts involving the linearised constraints
+  for (size_t i = 0; i < 2; i++) {
+    const JacobianType* constraintMat = (i==0) ? &nmMatrix : &mMatrix;
+    const auto& mat = *constraintMat;
+    size_t offset = (gridIdx[i]==0) ? 0 : dim*this->rhs_[0].size();
+
+    for (size_t j = 0; j < mat.N(); j++) {
+      // the active equality constraints and KKT derived w.r.t. Lagrange Multiplier
+      if (activeSet_[j].any()) {
+        for (auto col = mat[j].begin(); col != mat[j].end(); col++)
+          for (int m = 0; m < dim; m++) {
+            indexSet.add(primalSize + j, offset + col.index()*dim + m);
+            indexSet.add(offset + col.index()*dim + m, primalSize + j);
+          }
+      } else // this is added twice..
+        // and plain identity for the inactive constraints
+        indexSet.add(primalSize + j, primalSize + j);
+    }
+  }
+
+  if (full_linearisation_)
+    indexSet.import(condensedConstraintHessian_);
+  indexSet.exportIdx(priDualMatrix_);
+}
+
+template <class VectorType, class MaterialType>
+typename PrimalDualContactProblem<VectorType, MaterialType>::ScalarVector PrimalDualContactProblem<VectorType, MaterialType>::leastSquaresLagrangeMultiplier()
+{
+
+  auto coupling = std::dynamic_pointer_cast<ContactCoupling>(
+        this->contactAssembler_->getContactCouplings()[0]);
+
+  const auto& nmMatrix = coupling->exactNonmortarMatrix();
+  const auto& mMatrix = coupling->exactMortarMatrix();
+  const auto& gridIdx = this->contactAssembler_->getCoupling(0).gridIdx_;
+
+  MatrixIndexSet indexSet(nmMatrix.N(), nmMatrix.M() + mMatrix.M());
+  indexSet.import(nmMatrix, 0, (gridIdx[0] ==0) ? 0 : mMatrix.M());
+  indexSet.import(mMatrix, 0, (gridIdx[1] ==0) ? 0 : nmMatrix.M());
+
+  JacobianType jacobian;
+  indexSet.exportIdx(jacobian);
+  jacobian = 0;
+  for (size_t i=0; i<2; i++) {
+    const auto& mat = (i==0) ? nmMatrix : mMatrix;
+    size_t offset = (gridIdx[i] == 0) ? 0 : this->rhs_[0].size();
+    for (size_t row = 0; row < mat.N(); row++)
+      for (auto col = mat[row].begin(); col != mat[row].end(); col++)
+        jacobian[row][offset + col.index()] = *col;
+  }
+
+  ScalarMatrix symmetricJacobian;
+  matMultTransposeMat(symmetricJacobian, jacobian, jacobian);
+
+  VectorType totalRhs;
+  this->contactAssembler_->concatenateVectors(this->rhs_, totalRhs);
+  ScalarVector dummy(jacobian.N());
+  dummy = 0;
+  jacobian.mmv(totalRhs, dummy);
+
+  // solve system for the multipliers
+  // Make small cg solver
+  Dune::MatrixAdapter<ScalarMatrix, ScalarVector, ScalarVector> op(symmetricJacobian);
+  Dune::SeqILU<ScalarMatrix, ScalarVector, ScalarVector> ilu0(symmetricJacobian,1.0);
+  Dune::BiCGSTABSolver<ScalarVector> bicgstab(op, ilu0, 1E-8, 1000, 0);
+  Dune::InverseOperatorResult statistics;
+
+  // Solve!
+  auto init = dummy;  // seems to be a good initial value
+  bicgstab.apply(init, dummy, statistics);
+  return init;
+}
+
+} /* namespace Contact */
+} /* namespace Dune */
diff --git a/dune/contact/common/primaldualcontactproblem.hh b/dune/contact/common/primaldualcontactproblem.hh
new file mode 100644
index 00000000..e0fd9a7a
--- /dev/null
+++ b/dune/contact/common/primaldualcontactproblem.hh
@@ -0,0 +1,156 @@
+// -*- tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 2 -*-
+// vi: set ts=4 sw=2 et sts=2:
+#ifndef DUNE_CONTACT_COMMON_PRIMAL_DUAL_CONTACT_PROBLEM_HH
+#define DUNE_CONTACT_COMMON_PRIMAL_DUAL_CONTACT_PROBLEM_HH
+
+#include <dune/contact/common/staticmgcontactproblem.hh>
+
+namespace Dune {
+namespace Contact {
+
+/** \brief Static contact problem class for the filter multigrid method.*/
+template <class VectorType, class MaterialType>
+class PrimalDualContactProblem : public StaticMgContactProblem<VectorType, MaterialType>
+{
+  using Base = StaticMgContactProblem<VectorType, MaterialType>;
+  using DefGridComplex = typename Base::DefGridComplex;
+
+  using DeformedGridType = typename Base::DeformedGridType;
+  using ContactCoupling = typename Base::ContactCoupling;
+  using Basis = typename MaterialType::GlobalBasis;
+  using BasisGridFunction = typename Base::BasisGridFunction;
+
+public:
+  using ContactAssembler = typename Base::ContactAssembler;
+  using BlockVector = VectorType;
+  constexpr static int dim = BlockVector::block_type::dimension;
+  using field_type = typename BlockVector::field_type;
+  using ObstacleVector = typename Base::ObstacleVector;
+  using ScalarVector = ObstacleVector;
+  using ScalarMatrix = Dune::BCRSMatrix<Dune::FieldMatrix<field_type, 1, 1> >;
+  using MatrixType = typename Base::MatrixType;
+  using JacobianType = typename Base::JacobianType;
+
+
+  PrimalDualContactProblem(ContactAssembler& contactAssembler,
+                         std::vector<MaterialType>& materials,
+                         DefGridComplex& complex,
+                         const std::vector<BlockVector>& extForces,
+                         field_type priDualConstant,
+                         field_type fd_eps,
+                         bool full_linearisation,
+                         field_type initActiveSetBound) :
+    Base(contactAssembler, materials, complex, extForces),
+    priDualConstant_(priDualConstant),
+    changedActiveNodes_(-1),
+    fd_eps_(fd_eps),
+    full_linearisation_(full_linearisation),
+    initActiveSetBound_(initActiveSetBound)
+  {
+    activeSet_.resize(1, false);
+  }
+
+  //! Assemble the linearisations of energy and constraints
+  void assembleLinearisations(const std::vector<BlockVector>& iterates);
+
+  //! Setup the primal-dual active set problem
+  void assemblePrimalDualProblem(const ScalarVector& lagrangeMultiplier);
+
+  //! Return the quadratic term of the linearised energy
+  const ScalarMatrix& priDualMatrix() const {return priDualMatrix_;}
+
+  //! Return the linear term of linearised energy
+  const ScalarVector& priDualRhs() const {return priDualRhs_;}
+
+  //! Return the nonlinear constraints
+  const ObstacleVector& getConstraints() const
+  {
+    return this->contactAssembler_->getContactCouplings()[0]->getWeakObstacle();
+  }
+
+  //! Update the active set and assemble new constraints
+  void updateActiveSetAndLagrangeMultiplier(ScalarVector& lagrangeMultiplier);
+
+  //! Get active set
+  const BitSetVector<1>& getActiveSet() const
+  {
+    return activeSet_;
+  }
+
+  void addHardDirichlet(const ScalarVector& dirV, const BitSetVector<1>& dirN);
+
+  //! Return how many nodes changed from active to inactive and vice versa
+  int changedActiveNodes() const {return changedActiveNodes_;}
+
+  void initialiseEmptyActiveSet() {
+    const auto& constraints = getConstraints();
+    activeSet_.resize(getConstraints().size(), false);
+    for (size_t i=0; i<constraints.size(); i++)
+      if (constraints[i][0]<initActiveSetBound_)
+        activeSet_[i] = true;
+  }
+
+  ScalarVector leastSquaresLagrangeMultiplier();
+
+  std::vector<BlockVector> residuals(const ScalarVector& lagrangeMultiplier) {
+    std::vector<BlockVector> residuals = this->rhs_;
+
+    auto coupling = std::dynamic_pointer_cast<ContactCoupling>(
+          this->contactAssembler_->getContactCouplings()[0]);
+    const auto& gridIdx = this->contactAssembler_->getCoupling(0).gridIdx_;
+
+    coupling->exactNonmortarMatrix().umtv(lagrangeMultiplier, residuals[gridIdx[0]]);
+    coupling->exactMortarMatrix().umtv(lagrangeMultiplier, residuals[gridIdx[1]]);
+
+    return residuals;
+  }
+
+protected:
+
+  //! Compute index pattern for the primal-dual system matrix
+  void addPriDualMatrixIndices();
+  //! Add the entries to the primal-dual system matrix
+  void addPriDualMatrixEntries(const ScalarVector& lagrangeMultiplier);
+  //! Assemble the right hand side for the primal-dual right hand side
+  void assemblePriDualRhs(const ScalarVector& lagrangeMultiplier);
+
+  //! Compute FD approximation of the constraint Hessian times Lagrange multiplier
+  void computeFDConstraintHessian(const ScalarVector& lagrangeMultiplier);
+
+  //! Compute j-th entry of k-th grid constraint Hessian entry
+  void addFDEntry(MatrixType& denseContactHessian, const ScalarVector& lagrangeMultiplier, int gridIdx, int component);
+
+  //! Condense the dense matrix into a scalar condensed one
+  void condenseConstraintHessian(const MatrixType& denseConstraintHessian);
+
+  std::vector<BlockVector> currentIterates_;
+
+  //! The condensed Hessian of the constraints
+  ScalarMatrix condensedConstraintHessian_;
+
+  //! The matrix of the Newton problem in the pri-dual method
+  ScalarMatrix priDualMatrix_;
+
+  //! The rhs of the pri-dual problem
+  ScalarVector priDualRhs_;
+  //! The active set
+  Dune::BitSetVector<1> activeSet_;
+
+  //! This is an algorithmic parameter used to detect the active set
+  field_type priDualConstant_;
+
+  int changedActiveNodes_;
+
+  field_type fd_eps_;
+
+  bool full_linearisation_;
+
+  field_type initActiveSetBound_;
+};
+
+} /* namespace Contact */
+} /* namespace Dune */
+
+#include "primaldualcontactproblem.cc"
+
+#endif
-- 
GitLab