.

src/factories/stackedblocksfactory.cc

@@ -62,11 +62,11 @@ void StackedBlocksFactory<GridType, dims>::setBodies() {
             cuboidGeometries_[i] = std::make_shared<CuboidGeometry>(origins[i], lowerWeakOrigins[i], upperWeakOrigins[i], weakLength, weakLength, length, width);
         }
         const size_t idx = this->bodyCount_-1;
-        origins[idx] = cuboidGeometries_[idx-1]->D;
+        origins[idx] = cuboidGeometries_[idx-1]->D + LocalVector({0.25,0});
         lowerWeakOrigins[idx] = {0.6,idx*width};
         upperWeakOrigins[idx] = {0.6,(idx+1)*width};
 
-        cuboidGeometries_[idx] = std::make_shared<CuboidGeometry>(origins[idx], lowerWeakOrigins[idx], upperWeakOrigins[idx], weakLength, 0.0, length, width);
+        cuboidGeometries_[idx] = std::make_shared<CuboidGeometry>(origins[idx], lowerWeakOrigins[idx], upperWeakOrigins[idx], weakLength, 0.0, 5.0/8*length, width);
 #else
 #error CuboidGeometry only supports 2D and 3D!"
 #endif

src/io/vtk.cc

@@ -29,7 +29,6 @@ void BodyVTKWriter<VertexBasis, CellBasis>::write(
           vertexBasis_, u, "displacement");
   writer.addVertexData(displacementPointer);
 
-  print(v, "VTK Writer v:");
   auto const velocityPointer =
       std::make_shared<VTKBasisGridFunction<VertexBasis, Vector> const>(
           vertexBasis_, v, "velocity");

src/multi-body-problem.cc

@@ -154,7 +154,7 @@ int main(int argc, char *argv[]) {
     // ----------------------------
 
     levelContactNetwork.assemble();
-    printMortarBasis<Vector>(levelContactNetwork.nBodyAssembler());
+    //printMortarBasis<Vector>(levelContactNetwork.nBodyAssembler());
 
     // -----------------
     // init input/output
@@ -421,7 +421,7 @@ int main(int argc, char *argv[]) {
 
       report();
 
-      if (programState.timeStep==2000) {
+      if (programState.timeStep==50) {
         std::cout << "limit of timeSteps reached!" << std::endl;
         break; // TODO remove after debugging
       }

src/spatial-solving/fixedpointiterator.cc

@@ -69,7 +69,7 @@ FixedPointIterator<Factory, NBodyAssembler, Updaters, ErrorNorm>::run(
   const auto nBodies = nBodyAssembler_.nGrids();
 
   std::vector<const Matrix*> matrices_ptr(nBodies);
-  for (size_t i=0; i<nBodies; i++) {
+  for (int i=0; i<nBodies; i++) {
       matrices_ptr[i] = &velocityMatrices[i];
   }
 
@@ -84,9 +84,6 @@ FixedPointIterator<Factory, NBodyAssembler, Updaters, ErrorNorm>::run(
 
   //print(totalRhs, "totalRhs:");
 
-  Vector totalX;
-  nBodyAssembler_.nodalToTransformed(velocityIterates, totalX);
-
   // get lower and upper obstacles
   const auto& totalObstacles = nBodyAssembler_.totalObstacles_;
   Vector lower(totalObstacles.size());
@@ -102,18 +99,39 @@ FixedPointIterator<Factory, NBodyAssembler, Updaters, ErrorNorm>::run(
     }
   }
 
+  print(totalObstacles, "totalObstacles:");
+
+  print(lower, "lower obstacles:");
+  print(upper, "upper obstacles:");
+
+  // comput velocity obstacles
+  Vector vLower, vUpper;
+  std::vector<Vector> u0, v0;
+  updaters.rate_->extractOldVelocity(v0);
+  updaters.rate_->extractOldDisplacement(u0);
+
+  Vector totalu0, totalv0;
+  nBodyAssembler_.concatenateVectors(u0, totalu0);
+  nBodyAssembler_.concatenateVectors(v0, totalv0);
+
+  updaters.rate_->velocityObstacles(totalu0, lower, totalv0, vLower);
+  updaters.rate_->velocityObstacles(totalu0, upper, totalv0, vUpper);
+
+  print(vLower, "vLower obstacles:");
+  print(vUpper, "vUpper obstacles:");
+
   std::cout << "- Problem assembled: success" << std::endl;
 
   // set up functional and TNMMG solver
-  Functional J(bilinearForm, totalRhs, globalFriction_, lower, upper);
+  Functional J(bilinearForm, totalRhs, globalFriction_, vLower, vUpper);
   Factory solverFactory(parset_.sub("solver.tnnmg"), J, ignoreNodes_);
   auto step = solverFactory.step();
 
   std::cout << "- Functional and TNNMG step setup: success" << std::endl;
 
   EnergyNorm<Matrix, Vector> energyNorm(bilinearForm);
-  LoopSolver<Vector> velocityProblemSolver(step.get(), velocityMaxIterations_,
-                                           velocityTolerance_, &energyNorm,
+  LoopSolver<Vector> velocityProblemSolver(*step.get(), velocityMaxIterations_,
+                                           velocityTolerance_, energyNorm,
                                            verbosity_, false); // absolute error
 
   size_t fixedPointIteration;
@@ -129,10 +147,12 @@ FixedPointIterator<Factory, NBodyAssembler, Updaters, ErrorNorm>::run(
     Vector totalVelocityIterate;
     nBodyAssembler_.nodalToTransformed(velocityIterates, totalVelocityIterate);
 
+    //print(velocityIterates, "velocityIterates:");
+    //print(totalVelocityIterate, "totalVelocityIterate:");
     std::cout << "- FixedPointIteration iterate" << std::endl;
 
     // solve a velocity problem
-    step->setProblem(totalVelocityIterate);
+    solverFactory.setProblem(totalVelocityIterate);
 
     std::cout << "- Velocity problem set" << std::endl;
 
@@ -140,7 +160,10 @@ FixedPointIterator<Factory, NBodyAssembler, Updaters, ErrorNorm>::run(
     std::cout << "-- Preprocessed" << std::endl;
     velocityProblemSolver.solve();
     std::cout << "-- Solved" << std::endl;
-    nBodyAssembler_.postprocess(totalVelocityIterate, velocityIterates);
+
+    const auto& tnnmgSol = step->getSol();
+    nBodyAssembler_.postprocess(tnnmgSol, velocityIterates);
+    //nBodyAssembler_.postprocess(totalVelocityIterate, velocityIterates);
 
     //print(totalVelocityIterate, "totalVelocityIterate:");
     //print(velocityIterates, "velocityIterates:");
@@ -157,7 +180,7 @@ FixedPointIterator<Factory, NBodyAssembler, Updaters, ErrorNorm>::run(
 
     // extract relative velocities in mortar basis
     std::vector<Vector> v_rel;
-    relativeVelocities(totalVelocityIterate, v_rel);
+    relativeVelocities(tnnmgSol, v_rel);
 
     //print(v_rel, "v_rel");
 
@@ -186,6 +209,7 @@ FixedPointIterator<Factory, NBodyAssembler, Updaters, ErrorNorm>::run(
     }
 
     if (lambda_ < 1e-12 or breakCriterion) {
+      std::cout << "-FixedPointIteration finished! " << (lambda_ < 1e-12 ? "lambda" : "breakCriterion") << std::endl;
       fixedPointIteration++;
       break;
     }

src/spatial-solving/tnnmg/tnnmgstep.hh

@@ -144,8 +144,8 @@ class TNNMGStep :
     for (std::size_t i=0; i<preSmoothingSteps_; ++i)
         nonlinearSmoother_->iterate();
 
-    //std::cout << "- nonlinear presmoothing: success" << std::endl;
-    //print(x, "TNNMG iterate after smoothing:");
+    std::cout << "- nonlinear presmoothing: success" << std::endl;
+    print(x, "TNNMG iterate after smoothing:");
 
     // Compute constraint/truncated linearization
     if (not linearization_)
@@ -183,21 +183,21 @@ class TNNMGStep :
 
    // linearCorrection_(A, constrainedCorrection_, r);
 
-  //  std::cout << "- linear correction: success" << std::endl;
+    std::cout << "- linear correction: success" << std::endl;
 
     //print(constrainedCorrection_, "contrained correction:");
 
     linearization_->extendCorrection(constrainedCorrection_, correction_);
 
-  //  std::cout << "- extended correction: success" << std::endl;
-  //  print(correction_, "correction:");
+    std::cout << "- extended correction: success" << std::endl;
+    print(correction_, "correction:");
 
     // Project onto admissible set
     projection_(f, x, correction_);
 
-  //  std::cout << "- projection onto admissible set: success" << std::endl;
+    std::cout << "- projection onto admissible set: success" << std::endl;
 
-  //  print(correction_, "correction:");
+    print(correction_, "correction:");
 
     double const correctionNorm = correction_.two_norm();
     correction_ /= correctionNorm;

src/time-stepping/adaptivetimestepper.cc

@@ -71,7 +71,7 @@ IterationRegister AdaptiveTimeStepper<Factory, NBodyAssembler, Updaters, ErrorNo
   bool didCoarsen = false;
   iterationRegister_.reset();
   UpdatersWithCount R2;
-  UpdatersWithCount C; /*
+  UpdatersWithCount C;
   while (relativeTime_ + relativeTau_ <= 1.0) {
     R2 = step(R1_.updaters, relativeTime_ + relativeTau_, relativeTau_);
     std::cout << "AdaptiveTimeStepper R2 computed!" << std::endl << std::endl;
@@ -104,7 +104,7 @@ IterationRegister AdaptiveTimeStepper<Factory, NBodyAssembler, Updaters, ErrorNo
       R1_ = F1;
       R2 = F2;
     }
-  } */
+  }
 
   std::cout << "AdaptiveTimeStepper::advance() ...";
 
@@ -112,9 +112,9 @@ IterationRegister AdaptiveTimeStepper<Factory, NBodyAssembler, Updaters, ErrorNo
   relativeTime_ += relativeTau_;
   current_ = R1_.updaters;
 
-  UpdatersWithCount emptyR1;
-  R1_ = emptyR1;
-  //R1_ = R2;
+  //UpdatersWithCount emptyR1;
+  //R1_ = emptyR1;
+  R1_ = R2;
 
   std::cout << " done" << std::endl;
 

src/time-stepping/rate/backward_euler.cc

@@ -99,6 +99,16 @@ void BackwardEuler<Vector, Matrix, BoundaryFunctions, BoundaryNodes>::setup(
     }
 }
 
+template <class Vector, class Matrix, class BoundaryFunctions, class BoundaryNodes>
+void BackwardEuler<Vector, Matrix, BoundaryFunctions, BoundaryNodes>::velocityObstacles(const Vector& u0, const Vector& uObstacles, const Vector& v0, Vector& v1Obstacles) {
+    // v1 = 1/tau ( u1 - u0 )
+    v1Obstacles.resize(u0.size());
+    v1Obstacles = 0;
+
+    Dune::MatrixVector::addProduct(v1Obstacles, 1.0 / this->tau, uObstacles);
+    Dune::MatrixVector::subtractProduct(v1Obstacles, 1.0 / this->tau, u0);
+}
+
 template <class Vector, class Matrix, class BoundaryFunctions, class BoundaryNodes>
 void BackwardEuler<Vector, Matrix, BoundaryFunctions, BoundaryNodes>::postProcess(const std::vector<Vector>& iterate) {
     this->postProcessCalled = true;

src/time-stepping/rate/backward_euler.hh

@@ -20,6 +20,8 @@ class BackwardEuler : public RateUpdater<Vector, Matrix, BoundaryFunctions, Boun
             std::vector<Vector>&,
             std::vector<Matrix>&) override;
 
+    virtual void velocityObstacles(const Vector&, const Vector&, const Vector&, Vector&) override;
+
     virtual void postProcess(const std::vector<Vector>&) override;
 
     virtual auto clone() const -> std::shared_ptr<RateUpdater<Vector, Matrix, BoundaryFunctions, BoundaryNodes>> override;

src/time-stepping/rate/newmark.cc

@@ -111,6 +111,17 @@ void Newmark<Vector, Matrix, BoundaryFunctions, BoundaryNodes>::setup(
     print(iterate, "iterate:");
 }
 
+template <class Vector, class Matrix, class BoundaryFunctions, class BoundaryNodes>
+void Newmark<Vector, Matrix, BoundaryFunctions, BoundaryNodes>::velocityObstacles(const Vector& u0, const Vector& uObstacles, const Vector& v0, Vector& v1Obstacles) {
+    // v1 = 2/tau ( u1 - u0 ) - v0
+    v1Obstacles.resize(v0.size());
+    v1Obstacles = 0;
+
+    Dune::MatrixVector::addProduct(v1Obstacles, 2.0 / this->tau, uObstacles);
+    Dune::MatrixVector::subtractProduct(v1Obstacles, 2.0 / this->tau, u0);
+    v1Obstacles -= v0;
+}
+
 template <class Vector, class Matrix, class BoundaryFunctions, class BoundaryNodes>
 void Newmark<Vector, Matrix, BoundaryFunctions, BoundaryNodes>::postProcess(const std::vector<Vector>& iterate) {
     this->postProcessCalled = true;

src/time-stepping/rate/newmark.hh

@@ -20,6 +20,8 @@ class Newmark : public RateUpdater<Vector, Matrix, BoundaryFunctions, BoundaryNo
             std::vector<Vector>&,
             std::vector<Matrix>&) override;
 
+    virtual void velocityObstacles(const Vector& u0, const Vector& uObstacles, const Vector& v0, Vector& v1Obstacles) override;
+
     virtual void postProcess(const std::vector<Vector>&) override;
 
     virtual auto clone() const -> std::shared_ptr<RateUpdater<Vector, Matrix, BoundaryFunctions, BoundaryNodes>> override;

src/time-stepping/rate/rateupdater.cc

@@ -33,6 +33,11 @@ void RateUpdater<Vector, Matrix, BoundaryFunctions, BoundaryNodes>::extractDispl
     displacements = u;
 }
 
+template <class Vector, class Matrix, class BoundaryFunctions, class BoundaryNodes>
+void RateUpdater<Vector, Matrix, BoundaryFunctions, BoundaryNodes>::extractOldDisplacement(std::vector<Vector>& displacements) const {
+    displacements = u_o;
+}
+
 template <class Vector, class Matrix, class BoundaryFunctions, class BoundaryNodes>
 void RateUpdater<Vector, Matrix, BoundaryFunctions, BoundaryNodes>::extractVelocity(std::vector<Vector>& velocity) const {
     if (!postProcessCalled)

src/time-stepping/rate/rateupdater.hh

@@ -29,7 +29,9 @@ class RateUpdater {
             std::vector<Matrix>& AB) = 0;
 
   void virtual postProcess(const std::vector<Vector>& iterate) = 0;
+  void virtual velocityObstacles(const Vector& u0, const Vector& uObstacles, const Vector& v0, Vector& v1Obstacles) = 0;
   void extractDisplacement(std::vector<Vector>& displacements) const;
+  void extractOldDisplacement(std::vector<Vector>& displacements) const;
   void extractVelocity(std::vector<Vector>& velocity) const;
   void extractOldVelocity(std::vector<Vector>& velocity) const;
   void extractAcceleration(std::vector<Vector>& acceleration) const;

src/utils/debugutils.hh

@@ -368,5 +368,4 @@
        }
        std::cout << std::endl;
    }
-
 #endif