From 07266dcfd0e2843aa7e82672b9198cb84900ea32 Mon Sep 17 00:00:00 2001
From: podlesny <podlesny@mi.fu-berlin.de>
Date: Mon, 19 Aug 2019 14:43:42 +0200
Subject: [PATCH] .

---
 dune/tectonic/frictionpotential.hh            | 15 ++-
 dune/tectonic/localfriction.hh                | 13 ++-
 src/CMakeLists.txt                            |  6 +-
 src/data-structures/program_state.hh          |  4 +-
 src/multi-body-problem-2D.cfg                 |  3 +
 src/multi-body-problem-data/bc.hh             |  4 +-
 src/multi-body-problem.cfg                    |  2 +-
 src/solverfactorytest.cc                      | 93 ++++++++++++++++---
 .../levelpatchpreconditioner.hh               |  2 +-
 .../multilevelpatchpreconditioner.hh          | 21 ++++-
 .../preconditioners/supportpatchfactory.hh    | 25 ++---
 src/spatial-solving/solverfactory.cc          | 17 ++--
 src/spatial-solving/solverfactory.hh          |  7 +-
 src/spatial-solving/solverfactory_tmpl.cc     |  6 +-
 src/spatial-solving/tnnmg/linearization.hh    | 28 +++++-
 15 files changed, 190 insertions(+), 56 deletions(-)

diff --git a/dune/tectonic/frictionpotential.hh b/dune/tectonic/frictionpotential.hh
index 60b474ed..2ed879b3 100644
--- a/dune/tectonic/frictionpotential.hh
+++ b/dune/tectonic/frictionpotential.hh
@@ -41,17 +41,22 @@ class TruncatedRateState : public FrictionPotential {
   }
 
   double differential(double V) const override {
+    //std::cout << "differential: " <<  weight * fd.C - weightedNormalStress * coefficientOfFriction(V) << std::endl;
     return weight * fd.C - weightedNormalStress * coefficientOfFriction(V);
   }
 
-  double second_deriv(double V) const override {  
+  double second_deriv(double V) const override {
+    //std::cout << "second deriv: " << ((V<=Vmin) ? 0.0 : -weightedNormalStress * (fd.a / V)) << std::endl;
+
     if (V <= Vmin)
-      return 0;
+      return 0.0;
 
     return -weightedNormalStress * (fd.a / V);
   }
 
   double regularity(double V) const override {
+    //std::cout << "regularity: " << ((std::abs(V - Vmin) < 1e-14) ? std::numeric_limits<double>::infinity() : std::abs(second_deriv(V))) << std::endl;
+
     if (std::abs(V - Vmin) < 1e-14) // TODO
       return std::numeric_limits<double>::infinity();
 
@@ -59,7 +64,10 @@ class TruncatedRateState : public FrictionPotential {
   }
 
   double evaluate(double V) const override {
-      if (V <= Vmin)
+
+    //std::cout << "second deriv: " << ((V<=Vmin) ? 0.0 : weight * fd.C * V - weightedNormalStress * fd.a * V * (std::log(V / Vmin) - 1)) << std::endl;
+
+    if (V <= Vmin)
         return 0.0;
 
       return weight * fd.C * V - weightedNormalStress * fd.a * V * (std::log(V / Vmin) - 1);
@@ -68,6 +76,7 @@ class TruncatedRateState : public FrictionPotential {
   void updateAlpha(double alpha) override {
     double const logrest = (fd.mu0 + fd.b * alpha) / fd.a;
     Vmin = fd.V0 / std::exp(logrest);
+    //std::cout << "Vmin: " << Vmin << std::endl;
   }
 
 private:
diff --git a/dune/tectonic/localfriction.hh b/dune/tectonic/localfriction.hh
index 12b98e56..f2fb36ee 100644
--- a/dune/tectonic/localfriction.hh
+++ b/dune/tectonic/localfriction.hh
@@ -53,7 +53,7 @@ class WrappedScalarFriction : public LocalFriction<dimension> {
 
   double regularity(VectorType const &x) const override {
     double const xnorm = x.two_norm();
-    if (xnorm < 0.0)
+    if (xnorm <= 0.0)
       return std::numeric_limits<double>::infinity();
 
     return func_.regularity(xnorm);
@@ -91,17 +91,26 @@ class WrappedScalarFriction : public LocalFriction<dimension> {
       \f}
   */
   void addHessian(VectorType const &x, MatrixType &A) const override {
+    //std::cout << A << std::endl;
+    //std::cout << x << std::endl;
+
     double const xnorm2 = x.two_norm2();
     double const xnorm = std::sqrt(xnorm2);
     if (xnorm2 <= 0.0)
       return;
 
+    //std::cout << xnorm << " " << xnorm2 << std::endl;
+
     double const H1 = func_.differential(xnorm);
     double const H2 = func_.second_deriv(xnorm);
 
+    //std::cout << H1 << " " << H2 << std::endl;
+
     double const tensorweight = (H2 - H1 / xnorm) / xnorm2;
     double const idweight = H1 / xnorm;
 
+    //std::cout << tensorweight << " " << idweight << std::endl;
+
     for (size_t i = 0; i < dimension; ++i)
       for (size_t j = 0; j < i; ++j) {
         double const entry = tensorweight * x[i] * x[j];
@@ -113,6 +122,8 @@ class WrappedScalarFriction : public LocalFriction<dimension> {
       double const entry = tensorweight * x[k] * x[k];
       A[k][k] += entry + idweight;
     }
+
+    //std::cout << A << std::endl;
   }
 
   void addGradient(VectorType const &x, VectorType &y) const override {
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 6a92ecf9..b45d06d1 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -76,10 +76,10 @@ set(SFT_SOURCE_FILES
   multi-body-problem-data/grid/mygrids.cc
   multi-body-problem-data/grid/simplexmanager.cc
   #spatial-solving/solverfactory.cc
-  spatial-solving/fixedpointiterator.cc
+  #spatial-solving/fixedpointiterator.cc
   #spatial-solving/solverfactory_old.cc
-  time-stepping/adaptivetimestepper.cc
-  time-stepping/coupledtimestepper.cc
+  #time-stepping/adaptivetimestepper.cc
+  #time-stepping/coupledtimestepper.cc
   time-stepping/rate.cc
   time-stepping/rate/rateupdater.cc
   time-stepping/state.cc
diff --git a/src/data-structures/program_state.hh b/src/data-structures/program_state.hh
index afe9075f..ad0e796e 100644
--- a/src/data-structures/program_state.hh
+++ b/src/data-structures/program_state.hh
@@ -215,9 +215,9 @@ template <class VectorTEMPLATE, class ScalarVectorTEMPLATE> class ProgramState {
       Functional J(bilinearForm, totalRhs, ZeroNonlinearity(), lower, upper); //TODO
 
       // set up TNMMG solver
-      using Factory = SolverFactory<Functional, BitVector>;
+      using Factory = SolverFactory<Functional, BitVector, ContactNetwork>;
       //Factory factory(parset.sub("solver.tnnmg"), J, linearSolver, _dirichletNodes);
-      Factory factory(parset.sub("solver.tnnmg"), J, mgSolver, _dirichletNodes);
+      Factory factory(parset.sub("solver.tnnmg"), J, mgSolver, _dirichletNodes, contactNetwork);
 
      /* std::vector<BitVector> bodyDirichletNodes;
       nBodyAssembler.postprocess(_dirichletNodes, bodyDirichletNodes);
diff --git a/src/multi-body-problem-2D.cfg b/src/multi-body-problem-2D.cfg
index 676d1a40..4f122195 100644
--- a/src/multi-body-problem-2D.cfg
+++ b/src/multi-body-problem-2D.cfg
@@ -18,4 +18,7 @@ tolerance         = 1e-8
 tolerance         = 1e-5
 
 [solver.tnnmg.linear]
+tolerance          = 1e-8 #1e-10
+
+[solver.tnnmg.linear.preconditioner]
 tolerance          = 1e-10
diff --git a/src/multi-body-problem-data/bc.hh b/src/multi-body-problem-data/bc.hh
index b9b7ecda..f9e167ac 100644
--- a/src/multi-body-problem-data/bc.hh
+++ b/src/multi-body-problem-data/bc.hh
@@ -9,12 +9,12 @@ class VelocityDirichletCondition
     // Assumed to vanish at time zero
       double const finalVelocity = -5e-5;
     
-    /*std::cout << "VelocityDirichletCondition::evaluate()" << std::endl;
+    //std::cout << "VelocityDirichletCondition::evaluate()" << std::endl;
     
     if (relativeTime <= 0.1)
         std::cout << "- loading phase" << std::endl;
     else
-        std::cout << "- final velocity reached" << std::endl;*/
+        std::cout << "- final velocity reached" << std::endl;
     
     y = (relativeTime <= 0.1)
             ? finalVelocity * (1.0 - std::cos(relativeTime * M_PI / 0.1)) / 2.0
diff --git a/src/multi-body-problem.cfg b/src/multi-body-problem.cfg
index 83476cb7..eaade420 100644
--- a/src/multi-body-problem.cfg
+++ b/src/multi-body-problem.cfg
@@ -47,7 +47,7 @@ relativeTau = 1e-4 # 1e-6
 
 [timeSteps]
 scheme = newmark
-timeSteps = 100
+timeSteps = 1
 
 [u0.solver]
 maximumIterations = 100
diff --git a/src/solverfactorytest.cc b/src/solverfactorytest.cc
index a2c95ae0..8fea0370 100644
--- a/src/solverfactorytest.cc
+++ b/src/solverfactorytest.cc
@@ -91,7 +91,35 @@ Dune::Solvers::Criterion reductionFactorCriterion(
         double convRate = (normOfOldCorrection > 0) ? normOfCorrection / normOfOldCorrection : 0.0;
 
         if (convRate>1.0)
-            DUNE_THROW(Dune::Exception, "Solver convergence rate of " + std::to_string(convRate));
+            std::cout << "Solver convergence rate of " << convRate << std::endl;
+
+        normOfOldCorrection = normOfCorrection;
+        *lastIterate = *iterationStep.getIterate();
+
+        reductionFactors.push_back(convRate);
+        return std::make_tuple(convRate < 0, Dune::formatString(" % '.5f", convRate));
+      },
+      " reductionFactor");
+}
+
+
+template<class IterationStepType, class Functional, class ReductionFactorContainer>
+Dune::Solvers::Criterion energyCriterion(
+      const IterationStepType& iterationStep,
+      const Functional& f,
+      ReductionFactorContainer& reductionFactors)
+{
+  double normOfOldCorrection = 1;
+  auto lastIterate = std::make_shared<typename IterationStepType::Vector>(*iterationStep.getIterate());
+
+  return Dune::Solvers::Criterion(
+      [&, lastIterate, normOfOldCorrection] () mutable {
+        double normOfCorrection = std::abs(f(*lastIterate) - f(*iterationStep.getIterate())); //norm.diff(*lastIterate, *iterationStep.getIterate());
+
+        double convRate = (normOfOldCorrection != 0.0) ? 1.0 - (normOfCorrection / normOfOldCorrection) : 0.0;
+
+        if (convRate>1.0)
+            std::cout << "Solver convergence rate of " << convRate << std::endl;
 
         normOfOldCorrection = normOfCorrection;
         *lastIterate = *iterationStep.getIterate();
@@ -133,7 +161,7 @@ void solveProblem(const ContactNetwork& contactNetwork,
 
     using Linearization = Dune::TNNMG::BoxConstrainedQuadraticFunctionalConstrainedLinearization<ContactFunctional, BitVector>;
     using DefectProjection = Dune::TNNMG::ObstacleDefectProjection;
-    using Step = Dune::TNNMG::TNNMGStep<ContactFunctional, BitVector, Linearization, DefectProjection, LocalSolver>;
+    using Step = Dune::TNNMG::TNNMGStep<ContactFunctional, BitVector, Linearization, DefectProjection, LocalSolver, ContactNetwork>;
 
     // set multigrid solver
     auto smoother = TruncatedBlockGSStep<Matrix, Vector>{};
@@ -157,10 +185,12 @@ void solveProblem(const ContactNetwork& contactNetwork,
     linearMultigridStep->setTransferOperators(transfer);
 
     int mu = parset.get<int>("solver.tnnmg.main.multi"); // #multigrid steps in Newton step
-    auto step = Step(I, refX, nonlinearSmoother, linearMultigridStep, mu, DefectProjection(), LocalSolver());
+    auto step = Step(I, refX, nonlinearSmoother, linearMultigridStep, mu, DefectProjection(), LocalSolver(), contactNetwork);
 
     // compute reference solution with generic functional and solver
     auto norm = Norm(mat);
+
+    if (initial) {
     auto refSolver = Solver(step, parset.get<size_t>("u0.solver.maximumIterations"),
                             parset.get<double>("u0.solver.tolerance"), norm, Solver::FULL);
 
@@ -207,11 +237,30 @@ void solveProblem(const ContactNetwork& contactNetwork,
 
     //print(refX, "refX: ");
 
-    if (initial) {
         x = refX;
         return;
     }
     // set up solver factory solver
+    using PatchSolver = typename Dune::Solvers::LoopSolver<Vector, BitVector>;
+    using Preconditioner = MultilevelPatchPreconditioner<ContactNetwork, Matrix, Vector>;
+
+    const auto& preconditionerParset = parset.sub("solver.tnnmg.linear.preconditioner");
+
+    auto gsStep = Dune::Solvers::BlockGSStepFactory<Matrix, Vector>::create(Dune::Solvers::BlockGS::LocalSolvers::direct(0.0));
+    PatchSolver patchSolver(gsStep,
+                               preconditionerParset.get<size_t>("maximumIterations"),
+                               preconditionerParset.get<double>("tolerance"),
+                               nullptr,
+                               preconditionerParset.get<Solver::VerbosityMode>("verbosity"),
+                               false); // absolute error
+
+    Dune::BitSetVector<1> activeLevels(contactNetwork.nLevels(), true);
+    Preconditioner preconditioner(preconditionerParset, contactNetwork, activeLevels);
+    preconditioner.setPatchDepth(preconditionerParset.get<size_t>("patchDepth"));
+    preconditioner.build();
+
+    auto cgStep = std::make_shared<Dune::Solvers::CGStep<Matrix, Vector> >();
+    cgStep->setPreconditioner(preconditioner);
 
     // set up functional
     auto& globalFriction = contactNetwork.globalFriction();
@@ -232,11 +281,13 @@ void solveProblem(const ContactNetwork& contactNetwork,
 
     // set up TNMMG solver
     // dummy solver, uses direct solver for linear correction no matter what is set here
-    Norm mgNorm(*linearMultigridStep);
-    auto mgSolver = std::make_shared<Solver>(linearMultigridStep, parset.get<size_t>("solver.tnnmg.linear.maximumIterations"), parset.get<double>("solver.tnnmg.linear.tolerance"), mgNorm, Solver::QUIET);
+    //Norm mgNorm(*linearMultigridStep);
+    //auto mgSolver = std::make_shared<Solver>(linearMultigridStep, parset.get<size_t>("solver.tnnmg.linear.maximumIterations"), parset.get<double>("solver.tnnmg.linear.tolerance"), mgNorm, Solver::QUIET);
+    Norm mgNorm(*cgStep);
+    auto mgSolver = std::make_shared<Solver>(cgStep, parset.get<size_t>("solver.tnnmg.linear.maximumIterations"), parset.get<double>("solver.tnnmg.linear.tolerance"), mgNorm, Solver::QUIET);
 
-    using Factory = SolverFactory<MyFunctional, BitVector>;
-    Factory factory(parset.sub("solver.tnnmg"), J, *mgSolver, ignore);
+    using Factory = SolverFactory<MyFunctional, BitVector, ContactNetwork>;
+    Factory factory(parset.sub("solver.tnnmg"), J, *mgSolver, ignore, contactNetwork);
 
    /* std::vector<BitVector> bodyDirichletNodes;
     nBodyAssembler.postprocess(_dirichletNodes, bodyDirichletNodes);
@@ -262,7 +313,7 @@ void solveProblem(const ContactNetwork& contactNetwork,
             },
             "   energy      ");
 
-    initialEnergy = J(x);
+    double initialEnergy = J(x);
     solver.addCriterion(
             [&](){
             static double oldEnergy=initialEnergy;
@@ -289,6 +340,8 @@ void solveProblem(const ContactNetwork& contactNetwork,
     std::vector<double> factors;
     solver.addCriterion(reductionFactorCriterion(*tnnmgStep, norm, factors));
 
+    solver.addCriterion(energyCriterion(*tnnmgStep, J, factors));
+
     solver.preprocess();
     solver.solve();
 
@@ -427,7 +480,7 @@ void setupInitialConditions(const ContactNetwork& contactNetwork) {
     // Initial normal stress
 
     const auto& body = contactNetwork.body(i);
-    std::vector<std::shared_ptr<typename ContactNetwork::LeafBody::BoundaryCondition>> frictionBoundaryConditions;
+    /*std::vector<std::shared_ptr<typename ContactNetwork::LeafBody::BoundaryCondition>> frictionBoundaryConditions;
     body->boundaryConditions("friction", frictionBoundaryConditions);
     for (size_t j=0; j<frictionBoundaryConditions.size(); j++) {
         ScalarVector frictionBoundaryStress(weightedNormalStress[i].size());
@@ -437,11 +490,29 @@ void setupInitialConditions(const ContactNetwork& contactNetwork) {
           body->data()->getPoissonRatio(), u[i]);
 
         weightedNormalStress[i] += frictionBoundaryStress;
-    }
+    }*/
 
     Dune::MatrixVector::subtractProduct(accelerationRHS[i], *body->matrices().elasticity, u[i]);
   }
 
+  for (size_t i=0; i<contactNetwork.nCouplings(); i++) {
+    const auto& coupling = contactNetwork.coupling(i);
+    const auto& contactCoupling = contactNetwork.nBodyAssembler().getContactCouplings()[i];
+
+    const auto nonmortarIdx = coupling->gridIdx_[0];
+    const auto& body = contactNetwork.body(nonmortarIdx);
+
+    ScalarVector frictionBoundaryStress(weightedNormalStress[nonmortarIdx].size());
+
+    body->assembler()->assembleWeightedNormalStress(
+      contactCoupling->nonmortarBoundary(), frictionBoundaryStress, body->data()->getYoungModulus(),
+      body->data()->getPoissonRatio(), u[nonmortarIdx]);
+
+    weightedNormalStress[nonmortarIdx] += frictionBoundaryStress;
+  }
+
+  //print(weightedNormalStress, "weightedNormalStress: ");
+
   std::cout << "solving linear problem for a..." << std::endl;
 
   BitVector noNodes(dirichletNodes.size(), false);
diff --git a/src/spatial-solving/preconditioners/levelpatchpreconditioner.hh b/src/spatial-solving/preconditioners/levelpatchpreconditioner.hh
index 93612ee5..5713677a 100644
--- a/src/spatial-solving/preconditioners/levelpatchpreconditioner.hh
+++ b/src/spatial-solving/preconditioners/levelpatchpreconditioner.hh
@@ -207,7 +207,7 @@ class LevelPatchPreconditioner : public LinearIterationStep<MatrixType, VectorTy
             Vector newR;
             localProblem.getLocalRhs(x, newR);
 
-           /* print(ignore, "ignore:");
+            /*print(ignore, "ignore:");
             print(*this->mat_, "Mat:");
             print(localProblem.getMat(), "localMat:");*/
 
diff --git a/src/spatial-solving/preconditioners/multilevelpatchpreconditioner.hh b/src/spatial-solving/preconditioners/multilevelpatchpreconditioner.hh
index 68fa9b43..0ae5bfec 100644
--- a/src/spatial-solving/preconditioners/multilevelpatchpreconditioner.hh
+++ b/src/spatial-solving/preconditioners/multilevelpatchpreconditioner.hh
@@ -123,7 +123,7 @@ class MultilevelPatchPreconditioner : public LinearIterationStep<MatrixType, Vec
                                        parset.get<double>("tolerance"),
                                        *levelErrorNorms_[i].get(),
                                        parset.get<Solver::VerbosityMode>("verbosity"),
-                                       false); // absolute error */
+                                       false); // absolute error*/
                     std::make_shared<PatchSolver>();
             levelPatchPreconditioners_[i]->setPatchSolver(levelSolvers_[i]);
         }
@@ -311,8 +311,10 @@ class MultilevelPatchPreconditioner : public LinearIterationStep<MatrixType, Vec
 
         mgTransfer_[0]->prolong(levelX_[0], x);
 
+        //print(levelX_[0], "MultilevelPatchPreconditioner: levelX_[0]");
         //print(x, "MultilevelPatchPreconditioner: x0");
 
+
         for (size_t i=1; i<levelPatchPreconditioners_.size()-1; i++) {
             const auto& transfer = *mgTransfer_[i];
             auto& preconditioner = *levelPatchPreconditioners_[i];
@@ -320,13 +322,18 @@ class MultilevelPatchPreconditioner : public LinearIterationStep<MatrixType, Vec
             preconditioner.setIgnore(ignoreHierarchy_[i]);
             preconditioner.apply(levelX_[i], levelRhs_[i]);
 
+            //print(levelX_[i], "MultilevelPatchPreconditioner: levelX_[i] " + std::to_string(i));
+
             x += preconditioner.getSol();
 
             VectorType newX;
             transfer.prolong(x, newX);
 
+            //print(newX, "MultilevelPatchPreconditioner: newX " + std::to_string(i));
+
             x = newX;
-            //print(x, "MultilevelPatchPreconditioner: x" + std::to_string(i));
+
+            //print(x, "MultilevelPatchPreconditioner: x " + std::to_string(i));
         }
 
         auto& preconditioner = *levelPatchPreconditioners_.back();
@@ -339,7 +346,7 @@ class MultilevelPatchPreconditioner : public LinearIterationStep<MatrixType, Vec
         //print(x, "MultilevelPatchPreconditioner: xmax");
 
 
-        BitVector emptyIgnore(this->x_->size(), false);
+       /* BitVector emptyIgnore(this->x_->size(), false);
         LocalProblem<MatrixType, VectorType> globalProblem(*this->mat_.get(), *this->rhs_, emptyIgnore);
         globalProblem.getLocalRhs(*this->x_, newR);
         VectorType totalX = *(this->x_);
@@ -348,9 +355,11 @@ class MultilevelPatchPreconditioner : public LinearIterationStep<MatrixType, Vec
         globalSolver->preprocess();
         globalSolver->solve();
 
-        //print(totalX, "MultilevelPatchPreconditioner: totalX");
+        print(totalX, "MultilevelPatchPreconditioner: totalX"); */
 
         *(this->x_) = x;
+
+        //print(*(this->x_), "MultilevelPatchPreconditioner: sol");
     }
 
 
@@ -405,6 +414,10 @@ class MultilevelPatchPreconditioner : public LinearIterationStep<MatrixType, Vec
         }
     }
 
+    auto contactNetwork() const {
+        return contactNetwork_;
+    }
+
     size_t size() const {
         return levelPatchPreconditioners_.size();
     }
diff --git a/src/spatial-solving/preconditioners/supportpatchfactory.hh b/src/spatial-solving/preconditioners/supportpatchfactory.hh
index 761e1eae..3dbc81d5 100644
--- a/src/spatial-solving/preconditioners/supportpatchfactory.hh
+++ b/src/spatial-solving/preconditioners/supportpatchfactory.hh
@@ -210,36 +210,36 @@ class SupportPatchFactory
         }
 
         for (size_t i=0; i<nBodies_; i++) {
-            std::cout << "Coarse body" << i << ":" << std::endl;
+            //std::cout << "Coarse body" << i << ":" << std::endl;
 
             for (const auto& e : elements(coarseContactNetwork_.body(i)->gridView())) {
-                std::cout << "elemID: " << coarseIndices_.elementIndex(i, e) << std::endl;
-                std::cout << "vertexIDs: ";
+                //std::cout << "elemID: " << coarseIndices_.elementIndex(i, e) << std::endl;
+                //std::cout << "vertexIDs: ";
                 const int dimElement = Element::dimension;
                 const auto& refElement = Dune::ReferenceElements<double, dimElement>::general(e.type());
 
-                for (int j=0; j<refElement.size(dim); j++) {
+                /*for (int j=0; j<refElement.size(dim); j++) {
                     std::cout << coarseIndices_.vertexIndex(i, e, j) << " ";
                 }
-                std::cout << std::endl;
+                std::cout << std::endl;*/
 
-                std::cout << "intersectionIDs: ";
+                //std::cout << "intersectionIDs: ";
                 for (const auto& is : intersections(coarseContactNetwork_.body(i)->gridView(), e)) {
-                    std::cout << coarseIndices_.faceIndex(i, e, is.indexInInside()) << " (";
+                    //std::cout << coarseIndices_.faceIndex(i, e, is.indexInInside()) << " (";
 
                     std::set<size_t> dofs;
                     intersectionDofs(coarseIndices_, is, i, dofs);
-                    for (auto& d : dofs) {
+                    /*for (auto& d : dofs) {
                         std::cout << d << " ";
                     }
-                    std::cout << "); ";
+                    std::cout << "); ";*/
 
                 }
-                std::cout << std::endl  << "--------------" << std::endl;
+                //std::cout << std::endl  << "--------------" << std::endl;
             }
         }
 
-        std::cout << std::endl;
+        /*std::cout << std::endl;
         for (auto& kv : neighborFaceDofs_) {
             std::cout << "faceID: " << kv.first << " dofs: ";
 
@@ -248,7 +248,8 @@ class SupportPatchFactory
                 std::cout << d << " ";
             }
             std::cout << std::endl;
-        }
+        }*/
+
         /*
             // neighborElementDofs_ contains dofs of connected intersections that are neighbors to a face given by faceID,
             // boundary dofs are contained once, interior dofs multiple times
diff --git a/src/spatial-solving/solverfactory.cc b/src/spatial-solving/solverfactory.cc
index bca8290a..017f981d 100644
--- a/src/spatial-solving/solverfactory.cc
+++ b/src/spatial-solving/solverfactory.cc
@@ -10,13 +10,14 @@
 
 #include "../utils/debugutils.hh"
 
-template <class Functional, class BitVector>
+template <class Functional, class BitVector, class ContactNetwork>
 template <class LinearSolver>
-SolverFactory<Functional, BitVector>::SolverFactory(
+SolverFactory<Functional, BitVector, ContactNetwork>::SolverFactory(
     const Dune::ParameterTree& parset,
     Functional& J,
     LinearSolver&& linearSolver,
-    const BitVector& ignoreNodes) :
+    const BitVector& ignoreNodes,
+    const ContactNetwork& contactNetwork) :
         J_(Dune::Solvers::wrap_own_share<const Functional>(std::forward<Functional>(J))) {
 
     //auto localSolver = Dune::TNNMG::gaussSeidelLocalSolver(LocalSolver());
@@ -28,20 +29,20 @@ SolverFactory<Functional, BitVector>::SolverFactory(
 
     //tnnmgStep_ = std::make_shared<Step>(*J_, dummyIterate_, nonlinearSmoother_, linearSolver_ptr, DefectProjection(), Dune::TNNMG::ScalarObstacleSolver());
 
-    tnnmgStep_ = std::make_shared<Step>(*J_, dummyIterate_, nonlinearSmoother_, linearSolver_ptr, DefectProjection(), LineSearchSolver());
+    tnnmgStep_ = std::make_shared<Step>(*J_, dummyIterate_, nonlinearSmoother_, linearSolver_ptr, DefectProjection(), LineSearchSolver(), contactNetwork);
     tnnmgStep_->setPreSmoothingSteps(parset.get<int>("main.pre"));
     tnnmgStep_->setIgnore(ignoreNodes);
 }
 
-template <class Functional, class BitVector>
-void SolverFactory<Functional, BitVector>::setProblem(Vector& x) {
+template <class Functional, class BitVector, class ContactNetwork>
+void SolverFactory<Functional, BitVector, ContactNetwork>::setProblem(Vector& x) {
     nonlinearSmoother_->setProblem(x);
     tnnmgStep_->setProblem(x);
 }
 
 
-template <class Functional, class BitVector>
-auto SolverFactory<Functional, BitVector>::step()
+template <class Functional, class BitVector, class ContactNetwork>
+auto SolverFactory<Functional, BitVector, ContactNetwork>::step()
 -> std::shared_ptr<Step> {
     return tnnmgStep_;
 }
diff --git a/src/spatial-solving/solverfactory.hh b/src/spatial-solving/solverfactory.hh
index 1613dd45..703d0f9a 100644
--- a/src/spatial-solving/solverfactory.hh
+++ b/src/spatial-solving/solverfactory.hh
@@ -20,7 +20,7 @@
 #include "tnnmg/linesearchsolver.hh"
 #include "tnnmg/localbisectionsolver.hh"
 
-template <class FunctionalTEMPLATE, class BitVectorType>
+template <class FunctionalTEMPLATE, class BitVectorType, class ContactNetwork>
 class SolverFactory {
 public:
     using Functional = FunctionalTEMPLATE;
@@ -33,14 +33,15 @@ class SolverFactory {
     using Linearization = Linearization<Functional, BitVector>;
     using DefectProjection = typename Dune::TNNMG::ObstacleDefectProjection;
 
-    using Step = Dune::TNNMG::TNNMGStep<Functional, BitVector, Linearization, DefectProjection, LineSearchSolver>;
+    using Step = Dune::TNNMG::TNNMGStep<Functional, BitVector, Linearization, DefectProjection, LineSearchSolver, ContactNetwork>;
     //using Step = Dune::TNNMG::TNNMGStep<Functional, BitVector, Linearization, DefectProjection, Dune::TNNMG::ScalarObstacleSolver>;
 
   template <class LinearSolver>
   SolverFactory(const Dune::ParameterTree&,
                 Functional&,
                 LinearSolver&&,
-                const BitVector&);
+                const BitVector&,
+                const ContactNetwork&);
 
   void setProblem(Vector& x);
 
diff --git a/src/spatial-solving/solverfactory_tmpl.cc b/src/spatial-solving/solverfactory_tmpl.cc
index a136230f..996e7360 100644
--- a/src/spatial-solving/solverfactory_tmpl.cc
+++ b/src/spatial-solving/solverfactory_tmpl.cc
@@ -11,6 +11,8 @@
 #include "tnnmg/functional.hh"
 #include "tnnmg/zerononlinearity.hh"
 
+#include "../data-structures/contactnetwork.hh"
+
 #include "solverfactory.hh"
 
 using MyGlobalFriction = GlobalFriction<Matrix, Vector>;
@@ -20,9 +22,11 @@ using MyZeroFunctional = Functional<Matrix&, Vector&, ZeroNonlinearity&, Vector&
 
 using MyLinearSolver = Dune::Solvers::LoopSolver<Vector>;
 
+using MyContactNetwork =  ContactNetwork<Grid, Vector>;
+
 using MySolverFactory = SolverFactory<MyFunctional, BitVector>;
 template class SolverFactory<MyFunctional, BitVector>;
-template<> template<> SolverFactory<MyFunctional, BitVector>::SolverFactory(const Dune::ParameterTree&, MyFunctional&, MyLinearSolver&&, const BitVector&);
+template<> template<> SolverFactory<MyFunctional, BitVector>::SolverFactory(const Dune::ParameterTree&, MyFunctional&, MyLinearSolver&&, const BitVector&, const MyContactNetwork&);
 
 using MyZeroSolverFactory = SolverFactory<MyZeroFunctional, BitVector>;
 template class SolverFactory<MyZeroFunctional, BitVector>;
diff --git a/src/spatial-solving/tnnmg/linearization.hh b/src/spatial-solving/tnnmg/linearization.hh
index 362dcec4..556987c1 100644
--- a/src/spatial-solving/tnnmg/linearization.hh
+++ b/src/spatial-solving/tnnmg/linearization.hh
@@ -116,6 +116,8 @@ class Linearization {
 
       determineTruncation(x, f_.lowerObstacle(), f_.upperObstacle(), obstacleTruncationFlags, obstacleTruncationTolerance_); // obstacle truncation
 
+      //print(obstacleTruncationFlags, "obstacleTruncationFlags:");
+
       //std::cout << "    obstacle truncation: " << truncationFlags_.count();
       //std::cout << " " << obstacleTruncationTolerance_;
 
@@ -124,11 +126,13 @@ class Linearization {
       //std::cout << "    regularity truncation: " << truncationFlags_.count() << std::endl;
       //std::cout << " " << regularityTruncationTolerance_;
 
+      //print(regularityTruncationFlags, "regularityTruncationFlags:");
+
       truncationFlags_ = obstacleTruncationFlags;
       size_t blocksize = truncationFlags_[0].size();
       for (size_t i=0; i<truncationFlags_.size(); i++) {
           for (size_t j=0; j<blocksize; j++) {
-              //truncationFlags_[i][j] = truncationFlags_[i][j] and regularityTruncationFlags[i][j];
+              truncationFlags_[i][j] = truncationFlags_[i][j] or regularityTruncationFlags[i][j];
           }
       }
 
@@ -151,15 +155,21 @@ class Linearization {
           hessian_[i][it.index()] += *it;
       }
 
-      truncateMatrix(hessian_, obstacleTruncationFlags, obstacleTruncationFlags);
+      //truncateMatrix(hessian_, obstacleTruncationFlags, obstacleTruncationFlags);
 
-      //print(hessian_, "Hessian:");
+      //print(hessian_, "Hessian: ");
       //std::cout << "--------" << (double) hessian_[21][21] << "------------" << std::endl;
 
+      auto B = hessian_;
+
+      //print(x, "x: ");
+
       // nonlinearity part
       phi.addHessian(x, hessian_);
-      truncateMatrix(hessian_, regularityTruncationFlags, regularityTruncationFlags);
+      //truncateMatrix(hessian_, regularityTruncationFlags, regularityTruncationFlags);
 
+      B -= hessian_;
+      //print(B, "phi hessian: ");
 
       // compute gradient
       // ----------------
@@ -167,17 +177,27 @@ class Linearization {
       // quadratic part
       negativeGradient_ = derivative(f_)(x); // A*x - b
 
+      //print(negativeGradient_, "gradient: ");
+      auto C = negativeGradient_;
+
+
       // nonlinearity part
       phi.addGradient(x, negativeGradient_);
 
+      C -= negativeGradient_;
+      //print(C, "phi gradient: ");
+
       // -grad is needed for Newton step
       negativeGradient_ *= -1;
 
       // truncate gradient
       truncateVector(negativeGradient_, truncationFlags_);
+      truncateMatrix(hessian_, truncationFlags_, truncationFlags_);
 
       //print(hessian_, "hessian: ");
       //print(negativeGradient_, "negativeGradient: ");
+
+      //print(truncationFlags_, "truncationFlags:");
   }
 
 
-- 
GitLab