minor edits

adding x field

examples/contact/IPsolver.cpp

@@ -0,0 +1,907 @@
+#include "mfem.hpp"
+#include "IPsolver.hpp"
+#include "problems.hpp"
+#include <fstream>
+#include <iostream>
+#include <cstdlib>
+
+using namespace std;
+using namespace mfem;
+
+
+
+InteriorPointSolver::InteriorPointSolver(OptProblem * Problem, ParFiniteElementSpace *Vhin) 
+: problem(Problem), block_offsetsumlz(5), block_offsetsuml(4), block_offsetsx(3),
+ saveLogBarrierIterates(false), Vh(Vhin)
+{
+  tol  = 1.e-2;
+  max_iter = 20;
+  mu_k     = 1.0;
+
+  sMax     = 1.e2;
+  kSig     = 1.e10;   // control deviation from primal Hessian
+  tauMin   = 0.8;     // control rate at which iterates can approach the boundary
+  eta      = 1.e-4;   // backtracking constant
+  thetaMin = 1.e-4;   // allowed violation of the equality constraints
+
+  // constants in line-step A-5.4
+  delta    = 1.0;
+  sTheta   = 1.1;
+  sPhi     = 2.3;
+
+  // control the rate at which the penalty parameter is decreased
+  kMu     = 0.2;
+  thetaMu = 1.5;
+
+
+  thetaMax = 1.e6; // maximum constraint violation
+  // data for the second order correction
+  kSoc     = 0.99;
+
+  // equation (18)
+  gTheta = 1.e-5;
+  gPhi   = 1.e-5;
+
+  kEps   = 1.e1;
+
+  dimU = problem->GetDimU();
+  dimM = problem->GetDimM();
+  dimC = problem->GetDimC();
+  ckSoc.SetSize(dimC);
+  
+  block_offsetsumlz[0] = 0;
+  block_offsetsumlz[1] = dimU; // u
+  block_offsetsumlz[2] = dimM; // m
+  block_offsetsumlz[3] = dimC; // lambda
+  block_offsetsumlz[4] = dimM; // zl
+  block_offsetsumlz.PartialSum();
+
+  for(int i = 0; i < block_offsetsuml.Size(); i++)  { block_offsetsuml[i] = block_offsetsumlz[i]; }
+  for(int i = 0; i < block_offsetsx.Size(); i++)    { block_offsetsx[i]   = block_offsetsuml[i] ; }
+
+  // lower-bound for the inequality constraint m >= ml
+  ml = problem->Getml();
+  
+  lk.SetSize(dimC);   lk  = 0.0;
+  zlk.SetSize(dimM); zlk = 0.0;
+  mf.SetSize(dimM);   mf = 0.0;
+
+  linSolver = 0;
+  MyRank = 0;
+  iAmRoot = MyRank == 0 ? true : false;
+}
+
+double InteriorPointSolver::MaxStepSize(Vector &x, Vector &xl, Vector &xhat, double tau)
+{
+  double alphaMaxloc = 1.0;
+  double alphaTmp;
+  for(int i = 0; i < x.Size(); i++)
+  {
+    if( xhat(i) < 0. )
+    {
+      alphaTmp = -1. * tau * (x(i) - xl(i)) / xhat(i);
+      alphaMaxloc = min(alphaMaxloc, alphaTmp);
+    } 
+  }
+
+  // alphaMaxloc is the local maximum step size which is
+  // distinct on each MPI process. Need to compute
+  // the global maximum step size 
+  double alphaMaxglb;
+  alphaMaxglb = alphaMaxloc;
+  return alphaMaxglb;
+}
+
+double InteriorPointSolver::MaxStepSize(Vector &x, Vector &xhat, double tau)
+{
+  Vector zero(x.Size()); zero = 0.0;
+  return MaxStepSize(x, zero, xhat, tau);
+}
+
+
+void InteriorPointSolver::Mult(const Vector &x0, Vector &xf)
+{
+  BlockVector x0block(block_offsetsx); x0block = 0.0;
+  x0block.GetBlock(0).Set(1.0, x0);
+  // hard coded initialization :(
+  x0block.GetBlock(1) = 1.0;
+  x0block.GetBlock(1).Add(1.0, ml);
+  BlockVector xfblock(block_offsetsx); xfblock = 0.0;
+  Mult(x0block, xfblock);
+  xf.Set(1.0, xfblock.GetBlock(0));
+  mf.Set(1.0, xfblock.GetBlock(1));
+}
+
+void InteriorPointSolver::Mult(const BlockVector &x0, BlockVector &xf)
+{
+  converged = false;
+  IPNewtonKrylovIters.open("IPNewtonKrylovIters.dat", ios::out | ios::trunc);  
+  BlockVector xk(block_offsetsx), xhat(block_offsetsx); xk = 0; xhat = 0.0;
+  BlockVector Xk(block_offsetsumlz), Xhat(block_offsetsumlz); Xk = 0.0; Xhat = 0.0;
+  BlockVector Xhatuml(block_offsetsuml); Xhatuml = 0.0;
+  Vector zlhat(dimM); zlhat = 0.0;
+
+  xk.GetBlock(0).Set(1.0, x0.GetBlock(0));
+  xk.GetBlock(1).Set(1.0, x0.GetBlock(1));
+  // running estimate of the final values of the Lagrange multipliers
+  lk  = 0.0;
+  zlk = 0.0;
+
+  for(int i = 0; i < dimM; i++)
+  {
+    zlk(i) = 1.e1 * mu_k / (xk(i+dimU) - ml(i));
+  }
+
+  Xk.GetBlock(0).Set(1.0, xk.GetBlock(0));
+  Xk.GetBlock(1).Set(1.0, xk.GetBlock(1));
+  Xk.GetBlock(2).Set(1.0, lk);
+  Xk.GetBlock(3).Set(1.0, zlk);
+
+  /* set theta0 = theta(x0)
+   *     thetaMin
+   *     thetaMax
+   * when theta(xk) < thetaMin and the switching condition holds
+   * then we ask for the Armijo sufficient decrease of the barrier
+   * objective to be satisfied, in order to accept the trial step length alphakl
+   * 
+   * thetaMax controls how the filter is initialized for each log-barrier subproblem
+   * F0 = {(th, phi) s.t. th > thetaMax}
+   * that is the filter does not allow for iterates where the constraint violation
+   * is larger than that of thetaMax
+   */
+  double theta0 = theta(xk);
+  thetaMin = 1.e-4 * max(1.0, theta0);
+  thetaMax = 1.e8  * thetaMin;
+
+  double Eeval, maxBarrierSolves, Eevalmu0;
+  bool printOptimalityError; // control optimality error print to console for log-barrier subproblems
+  
+  maxBarrierSolves = 10;
+
+  for(jOpt = 0; jOpt < max_iter; jOpt++)
+  {
+    if(iAmRoot)
+    {
+      cout << "interior-point solve step " << jOpt << endl;
+    }
+    // A-2. Check convergence of overall optimization problem
+    printOptimalityError = false;
+    Eevalmu0 = E(xk, lk, zlk, printOptimalityError);
+    if(Eevalmu0 < tol)
+    {
+      converged = true;
+      if(iAmRoot)
+      {
+	      IPNewtonKrylovIters.close();
+        cout << "solved optimization problem :)\n";
+      }
+      break;
+    }
+    
+    if(jOpt > 0) { maxBarrierSolves = 1; }
+    
+    for(int i = 0; i < maxBarrierSolves; i++)
+    {
+      // A-3. Check convergence of the barrier subproblem
+      printOptimalityError = true;
+      Eeval = E(xk, lk, zlk, mu_k, printOptimalityError);
+      if(Eeval < kEps * mu_k)
+      {
+        if(iAmRoot)
+        {
+          cout << "solved barrier subproblem :), for mu = " << mu_k << endl;
+        }
+        // A-3.1. Recompute the barrier parameter
+        mu_k  = max(tol / 10., min(kMu * mu_k, pow(mu_k, thetaMu)));
+        // A-3.2. Re-initialize the filter
+        F1.DeleteAll();
+        F2.DeleteAll();
+      }
+      else
+      {
+        break;
+      }
+    }
+    
+    // A-4. Compute the search direction
+    // solve for (uhat, mhat, lhat)
+    if(iAmRoot)
+    {
+      cout << "\n** A-4. IP-Newton solve **\n";
+    }
+    zlhat = 0.0; Xhatuml = 0.0;
+    // why do we have Xhatuml ....???
+    // TO DO: remove Xhatuml in favor of passing Xhat
+    IPNewtonSolve(xk, lk, zlk, zlhat, Xhatuml, mu_k, false); 
+
+
+    // assign data stack, X = (u, m, l, zl)
+    Xk = 0.0;
+    Xk.GetBlock(0).Set(1.0, xk.GetBlock(0));
+    Xk.GetBlock(1).Set(1.0, xk.GetBlock(1));
+    Xk.GetBlock(2).Set(1.0, lk);
+    Xk.GetBlock(3).Set(1.0, zlk);
+
+    // assign data stack, Xhat = (uhat, mhat, lhat, zlhat)
+    Xhat = 0.0;
+    for(int i = 0; i < 3; i++)
+    {
+      Xhat.GetBlock(i).Set(1.0, Xhatuml.GetBlock(i));
+    }
+    Xhat.GetBlock(3).Set(1.0, zlhat);
+
+
+    // A-5. Backtracking line search.
+    if(iAmRoot)
+    {
+      cout << "\n** A-5. Linesearch **\n";
+      cout << "mu = " << mu_k << endl;
+    }
+    lineSearch(Xk, Xhat, mu_k);
+
+    if(lineSearchSuccess)
+    {
+      if(iAmRoot)
+      {
+        cout << "lineSearch successful :)\n";
+      }
+      if(!switchCondition || !sufficientDecrease)
+      {
+        F1.Append( (1. - gTheta) * thx0);
+        F2.Append( phx0 - gPhi * thx0);
+      }
+      // ----- A-6: Accept the trial point
+      // print info regarding zl...
+      xk.GetBlock(0).Add(alpha, Xhat.GetBlock(0));
+      xk.GetBlock(1).Add(alpha, Xhat.GetBlock(1));
+      lk.Add(alpha,   Xhat.GetBlock(2));
+      zlk.Add(alphaz, Xhat.GetBlock(3));
+      projectZ(xk, zlk, mu_k);
+    }
+    else
+    {
+      if(iAmRoot)
+      {
+        cout << "lineSearch not successful :(\n";
+        cout << "attempting feasibility restoration with theta = " << thx0 << endl;
+        cout << "no feasibility restoration implemented, exiting now \n";
+      }
+      break;
+      //cout << "feasibility restoration!!! :( :( :(\n";
+      //problem->feasibilityRestoration(x, 1.e-12);
+      //      break;
+    }
+    //
+    if(jOpt + 1 == max_iter && iAmRoot) 
+    { 
+      cout << "maximum optimization iterations :(\n";
+      IPNewtonKrylovIters.close();
+    }
+  }
+  // done with optimization routine, just reassign data to xf reference so
+  // that the application code has access to the optimal point
+  xf = 0.0;
+  xf.GetBlock(0).Set(1.0, xk.GetBlock(0));
+  xf.GetBlock(1).Set(1.0, xk.GetBlock(1));
+}
+
+void InteriorPointSolver::FormIPNewtonMat(BlockVector & x, Vector & l, Vector &zl, BlockOperator &Ak)
+{
+  // WARNING: Huu, Hum, Hmu, Hmm should all be Hessian terms of the Lagrangian, currently we 
+  //          them by Hessian terms of the objective function and neglect the Hessian of l^T c
+
+  Huu = problem->Duuf(x); Hum = problem->Dumf(x);
+  Hmu = problem->Dmuf(x); Hmm = problem->Dmmf(x);
+
+  Vector DiagLogBar(dimM); DiagLogBar = 0.0;
+  for(int ii = 0; ii < dimM; ii++)
+  {
+    DiagLogBar(ii) = zl(ii) / (x(ii+dimU) - ml(ii));
+  }
+  if(saveLogBarrierIterates)
+  {
+    std::ofstream diagStream;
+    char diagString[100];
+    snprintf(diagString, 100, "logBarrierHessiandata/D%d.dat", jOpt);
+    diagStream.open(diagString, ios::out | ios::trunc);
+    for(int ii = 0; ii < dimM; ii++)
+    {
+      diagStream << setprecision(30) << DiagLogBar(ii) << endl;
+    }
+    diagStream.close();
+  } 
+
+  delete Wmm;
+  if(Hmm != nullptr)
+  {
+    SparseMatrix * D = new SparseMatrix(DiagLogBar);
+    Wmm = Add(*Hmm, *D);
+    delete D;
+  }
+  else
+  {
+    Wmm = new SparseMatrix(DiagLogBar);
+  }
+
+  delete JuT;
+  delete JmT;
+  Ju = problem->Duc(x); JuT = Transpose(*Ju);
+  Jm = problem->Dmc(x); JmT = Transpose(*Jm);
+  
+  //         IP-Newton system matrix
+  //    Ak = [[H_(u,u)  H_(u,m)   J_u^T]
+  //          [H_(m,u)  W_(m,m)   J_m^T]
+  //          [ J_u      J_m       0  ]]
+
+  Ak.SetBlock(0, 0, Huu);                         Ak.SetBlock(0, 2, JuT);
+                          Ak.SetBlock(1, 1, Wmm); Ak.SetBlock(1, 2, JmT);
+  Ak.SetBlock(2, 0,  Ju); Ak.SetBlock(2, 1,  Jm);
+
+  if(Hum != nullptr) { Ak.SetBlock(0, 1, Hum); Ak.SetBlock(1, 0, Hmu); }
+}
+
+
+// perturbed KKT system solve
+// determine the search direction
+void InteriorPointSolver::IPNewtonSolve(BlockVector &x, Vector &l, Vector &zl, Vector &zlhat, BlockVector &Xhat, double mu, bool socSolve)
+{
+  // solve A x = b, where A is the IP-Newton matrix
+  BlockOperator A(block_offsetsuml, block_offsetsuml); BlockVector b(block_offsetsuml); b = 0.0;
+  FormIPNewtonMat(x, l, zl, A);
+
+  //       [grad_u phi + Ju^T l]
+  // b = - [grad_m phi + Jm^T l]
+  //       [          c        ]
+  BlockVector gradphi(block_offsetsx); gradphi = 0.0;
+  BlockVector JTl(block_offsetsx); JTl = 0.0;
+  Dxphi(x, mu, gradphi);
+  
+  (A.GetBlock(0,2)).Mult(l, JTl.GetBlock(0));
+  (A.GetBlock(1,2)).Mult(l, JTl.GetBlock(1));
+
+  for(int ii = 0; ii < 2; ii++)
+  {
+    b.GetBlock(ii).Set(1.0, gradphi.GetBlock(ii));
+    b.GetBlock(ii).Add(1.0, JTl.GetBlock(ii));
+  }
+  if(!socSolve) 
+  {
+    problem->c(x, b.GetBlock(2));
+  }
+  else
+  {
+    b.GetBlock(2).Set(1.0, ckSoc);
+  }
+  b *= -1.0; 
+  Xhat = 0.0;
+
+  #ifdef MFEM_USE_SUITESPARSE
+    // Direct solve for IP-Newton saddle-point system
+    // A = [ [ Huu   0    Ju^T]
+    //       [  0    D     -I ]
+    //       [ Ju   -I      0 ]]
+    if(linSolver == 0)
+    {
+      BlockMatrix ABlockMatrix(block_offsetsuml, block_offsetsuml);
+      for(int ii = 0; ii < 3; ii++)
+      {
+        for(int jj = 0; jj < 3; jj++)
+        {
+          if(!A.IsZeroBlock(ii, jj))
+          {
+            ABlockMatrix.SetBlock(ii, jj, dynamic_cast<SparseMatrix *>(&(A.GetBlock(ii, jj))));
+          }
+        }
+      }
+      /* direct solve of the 3x3 IP-Newton linear system */
+      UMFPackSolver ASolver;
+      SparseMatrix *ASparse = ABlockMatrix.CreateMonolithic();
+      ASolver.SetOperator(*ASparse);
+      ASolver.Mult(b, Xhat);
+
+      Vector residual(Xhat.Size());
+      ASparse->Mult(Xhat, residual);
+      residual.Add(-1.0, b);
+      delete ASparse;
+    }
+    else if(linSolver == 1)
+    {
+      // Direct solve for 0,0 Schur complement of IP-Newton system, Huu + Ju^T Wmm Ju,
+      // where Wmm = D for contact problems
+      SparseMatrix * Huuloc = new SparseMatrix(*dynamic_cast<SparseMatrix *>(&(A.GetBlock(0, 0))));
+      SparseMatrix * Wmmloc = new SparseMatrix(*dynamic_cast<SparseMatrix *>(&(A.GetBlock(1, 1))));
+      SparseMatrix * Juloc  = new SparseMatrix(*dynamic_cast<SparseMatrix *>(&(A.GetBlock(2, 0))));
+      SparseMatrix * JuTloc = new SparseMatrix(*dynamic_cast<SparseMatrix *>(&(A.GetBlock(0, 2))));
+      Vector Dvec(dimM); Dvec = 0.0;
+      Vector one(dimM); one = 1.0;
+      Wmmloc->Mult(one, Dvec);
+      SparseMatrix *JuTDJu = Mult_AtDA(*Juloc, Dvec);     // Ju^T D Ju
+      SparseMatrix *Areduced = Add(*Huuloc, *JuTDJu);  // Huu + Ju^T D Ju
+
+      
+      /* prepare the reduced rhs */
+      // breduced = bu + Ju^T (bm + Wmm bl)
+      Vector breduced(dimU); breduced = 0.0;
+      Vector tempVec(dimM); tempVec = 0.0;
+      Wmmloc->Mult(b.GetBlock(2), tempVec);
+      tempVec.Add(1.0, b.GetBlock(1));
+      JuTloc->Mult(tempVec, breduced);
+      breduced.Add(1.0, b.GetBlock(0));
+
+      // solve the reduced linear system
+      UMFPackSolver AreducedSolver;
+      AreducedSolver.SetOperator(*Areduced);
+      AreducedSolver.Mult(breduced, Xhat.GetBlock(0));
+
+      // now propagate solved uhat to obtain mhat and lhat
+      // xm = Ju xu - bl
+      Juloc->Mult(Xhat.GetBlock(0), Xhat.GetBlock(1));
+      Xhat.GetBlock(1).Add(-1.0, b.GetBlock(2));
+
+      // xl = Wmm xm - bm
+      Wmmloc->Mult(Xhat.GetBlock(1), Xhat.GetBlock(2));
+      Xhat.GetBlock(2).Add(-1.0, b.GetBlock(1));
+
+      delete Wmmloc;
+      delete Huuloc;
+      delete JuTDJu;
+      delete Juloc;
+      delete Areduced;
+    }
+  #else
+    MFEM_VERIFY(linSolver > 1, "linSolver = 0, 1 require MFEM_USE_SUITESPARSE=YES");
+  #endif
+  if (linSolver == 2 || linSolver == 3)
+  { 
+    // Iterative solve for 0,0 Schur complement of IP-Newton system, Huu + Ju^T Wmm Ju,
+    // where Wmm = D for contact problems
+    // here the iterative solver is a Jacobi-preconditioned CG-solve
+    SparseMatrix * Huuloc = new SparseMatrix(*dynamic_cast<SparseMatrix *>(&(A.GetBlock(0, 0))));
+    SparseMatrix * Wmmloc = new SparseMatrix(*dynamic_cast<SparseMatrix *>(&(A.GetBlock(1, 1))));
+    SparseMatrix * Juloc  = new SparseMatrix(*dynamic_cast<SparseMatrix *>(&(A.GetBlock(2, 0))));
+    SparseMatrix * JuTloc = new SparseMatrix(*dynamic_cast<SparseMatrix *>(&(A.GetBlock(0, 2))));
+    // Vector Dvec(dimM); Dvec = 0.0;
+    // Vector one(dimM); one = 1.0;
+    // Wmmloc->Mult(one, Dvec);
+
+    // SparseMatrix *JuTDJu = Mult_AtDA(*Juloc, Dvec);     // Ju^T D Ju
+    SparseMatrix *JuTDJu   = RAP(*Juloc,*Wmmloc,*Juloc);     // Ju^T D Ju
+    SparseMatrix *Areduced = Add(*Huuloc, *JuTDJu);  // Huu + Ju^T D Ju
+    /* prepare the reduced rhs */
+    // breduced = bu + Ju^T (bm + Wmm bl)
+    Vector breduced(dimU); breduced = 0.0;
+    Vector tempVec(dimM); tempVec = 0.0;
+    Wmmloc->Mult(b.GetBlock(2), tempVec);
+    tempVec.Add(1.0, b.GetBlock(1));
+    JuTloc->Mult(tempVec, breduced);
+    breduced.Add(1.0, b.GetBlock(0));
+    
+    /* set up an iterative solver */
+    int globalNumRows = dimU;
+    HYPRE_BigInt rowStarts[2];
+    rowStarts[0] = 0;
+    rowStarts[1] = dimU;
+    HypreParMatrix * Ahypre = new HypreParMatrix(MPI_COMM_WORLD, globalNumRows, rowStarts, Areduced);
+    // CGSolver Asolver(MPI_COMM_WORLD);
+    HyprePCG Asolver(MPI_COMM_WORLD);
+    HypreBoomerAMG * Aprec = new HypreBoomerAMG(*Ahypre);
+    Aprec->SetPrintLevel(0);
+    if(linSolver == 3)
+    {
+      Aprec->SetElasticityOptions(Vh);
+    }
+    Aprec->SetSystemsOptions(3,false);
+
+    Asolver.SetOperator(*Ahypre);
+    Asolver.SetPrintLevel(2);
+    Asolver.SetMaxIter(1000);
+    // Asolver.SetResidualConvergenceOptions();
+    Asolver.SetTol(1.e-6);
+    Asolver.SetPreconditioner(*Aprec);
+    // Asolver.SetResidualConvergenceOptions();
+
+    Asolver.Mult(breduced, Xhat.GetBlock(0));
+    int num_iterations;
+    Asolver.GetNumIterations(num_iterations);
+    cgnum_iterations.Append(num_iterations);
+    // int numNewtonKrylovIters = -1;
+    // numNewtonKrylovIters = Asolver.GetNumIterations();
+    // IPNewtonKrylovIters << numNewtonKrylovIters << endl;
+
+    delete Aprec;
+    delete Ahypre;
+    
+    // now propagate solved uhat to obtain mhat and lhat
+    // xm = Ju xu - bl
+    Juloc->Mult(Xhat.GetBlock(0), Xhat.GetBlock(1));
+    Xhat.GetBlock(1).Add(-1.0, b.GetBlock(2));
+
+    // // xl = Wmm xm - bm
+    Wmmloc->Mult(Xhat.GetBlock(1), Xhat.GetBlock(2));
+    Xhat.GetBlock(2).Add(-1.0, b.GetBlock(1));
+    
+
+    delete Wmmloc;
+    delete Huuloc;
+    delete JuTDJu;
+    delete Juloc;
+    delete Areduced;
+  }
+  else if(linSolver > 2)
+  { 
+    // Iterative solve for 0,0 Schur complement of IP-Newton system, Huu + Ju^T Wmm Ju,
+    // where Wmm = D for contact problems
+    // here the iterative solver is a Jacobi-preconditioned CG-solve
+    SparseMatrix * Huuloc = new SparseMatrix(*dynamic_cast<SparseMatrix *>(&(A.GetBlock(0, 0))));
+    SparseMatrix * Wmmloc = new SparseMatrix(*dynamic_cast<SparseMatrix *>(&(A.GetBlock(1, 1))));
+    SparseMatrix * Juloc  = new SparseMatrix(*dynamic_cast<SparseMatrix *>(&(A.GetBlock(2, 0))));
+    SparseMatrix * JuTloc = new SparseMatrix(*dynamic_cast<SparseMatrix *>(&(A.GetBlock(0, 2))));
+    Vector Dvec(dimM); Dvec = 0.0;
+    Vector one(dimM); one = 1.0;
+    Wmmloc->Mult(one, Dvec);
+    SparseMatrix *JuTDJu = Mult_AtDA(*Juloc, Dvec);     // Ju^T D Ju
+    SparseMatrix *Areduced = Add(*Huuloc, *JuTDJu);  // Huu + Ju^T D Ju
+
+    /* prepare the reduced rhs */
+    // breduced = bu + Ju^T (bm + Wmm bl)
+    Vector breduced(dimU); breduced = 0.0;
+    Vector tempVec(dimM); tempVec = 0.0;
+    Wmmloc->Mult(b.GetBlock(2), tempVec);
+    tempVec.Add(1.0, b.GetBlock(1));
+    JuTloc->Mult(tempVec, breduced);
+    breduced.Add(1.0, b.GetBlock(0));
+    
+    /* set up an iterative solver */
+    GSSmoother AreducedPrec((SparseMatrix &)(*Areduced));
+    GMRESSolver AreducedSolver;
+    AreducedSolver.SetOperator(*Areduced);
+    AreducedSolver.SetAbsTol(1.e-12);
+    AreducedSolver.SetRelTol(1.e-8);
+    AreducedSolver.SetMaxIter(500);
+    AreducedSolver.SetPreconditioner(AreducedPrec);
+    AreducedSolver.SetPrintLevel(1);
+    AreducedSolver.Mult(breduced, Xhat.GetBlock(0));
+    
+    // now propagate solved uhat to obtain mhat and lhat
+    // xm = Ju xu - bl
+    Juloc->Mult(Xhat.GetBlock(0), Xhat.GetBlock(1));
+    Xhat.GetBlock(1).Add(-1.0, b.GetBlock(2));
+
+    // xl = Wmm xm - bm
+    Wmmloc->Mult(Xhat.GetBlock(1), Xhat.GetBlock(2));
+    Xhat.GetBlock(2).Add(-1.0, b.GetBlock(1));
+
+    delete Wmmloc;
+    delete Huuloc;
+    delete JuTDJu;
+    delete Juloc;
+    delete Areduced;
+  }
+    
+  
+  /* backsolve to determine zlhat */
+  for(int ii = 0; ii < dimM; ii++)
+  {
+    zlhat(ii) = -1.*(zl(ii) + (zl(ii) * Xhat(ii + dimU) - mu) / (x(ii + dimU) - ml(ii)) );
+  }
+}
+
+// here Xhat, X will be BlockVectors w.r.t. the 4 partitioning X = (u, m, l, zl)
+
+void InteriorPointSolver::lineSearch(BlockVector& X0, BlockVector& Xhat, double mu)
+{
+  double tau  = max(tauMin, 1.0 - mu);
+  Vector u0   = X0.GetBlock(0);
+  Vector m0   = X0.GetBlock(1);
+  Vector l0   = X0.GetBlock(2);
+  Vector z0   = X0.GetBlock(3);
+  Vector uhat = Xhat.GetBlock(0);
+  Vector mhat = Xhat.GetBlock(1);
+  Vector lhat = Xhat.GetBlock(2);
+  Vector zhat = Xhat.GetBlock(3);
+  double alphaMax  = MaxStepSize(m0, ml, mhat, tau);
+  double alphaMaxz = MaxStepSize(z0, zhat, tau);
+  alphaz = alphaMaxz;
+
+
+  BlockVector x0(block_offsetsx); x0 = 0.0;
+  x0.GetBlock(0).Set(1.0, u0);
+  x0.GetBlock(1).Set(1.0, m0);
+  
+  BlockVector xhat(block_offsetsx); xhat = 0.0;
+  xhat.GetBlock(0).Set(1.0, uhat);
+  xhat.GetBlock(1).Set(1.0, mhat);
+  
+  BlockVector xtrial(block_offsetsx); xtrial = 0.0;
+  BlockVector Dxphi0(block_offsetsx); Dxphi0 = 0.0;
+  int maxBacktrack = 20;
+  alpha = alphaMax;
+
+
+  Vector ck0(dimC); ck0 = 0.0;
+  Vector zhatsoc(dimM); zhatsoc = 0.0;
+  BlockVector Xhatumlsoc(block_offsetsuml); Xhatumlsoc = 0.0;
+  BlockVector xhatsoc(block_offsetsx); xhatsoc = 0.0;
+  Vector uhatsoc(dimU); uhatsoc = 0.0;
+  Vector mhatsoc(dimM); mhatsoc = 0.0;
+
+  Dxphi(x0, mu, Dxphi0);
+  Dxphi0_xhat = InnerProduct(Dxphi0, xhat);
+  double xhat_L2norm = sqrt(InnerProduct(xhat, xhat));
+  double Dxphi_L2norm = sqrt(InnerProduct(Dxphi0, Dxphi0));
+  descentDirection = Dxphi0_xhat < 0. ? true : false;
+  if(descentDirection)
+  {
+    cout << "is a descent direction for the log-barrier objective\n";
+  }
+  else
+  {
+    cout << "is not a descent direction for the log-barrier objective\n";
+  }
+  cout << "Dxphi^T xhat / (|| Dxphi ||_2 * || xhat ||_2) = " << Dxphi0_xhat / (xhat_L2norm * Dxphi_L2norm) << endl;
+  thx0 = theta(x0);
+  phx0 = phi(x0, mu);
+
+  lineSearchSuccess = false;
+  for(int i = 0; i < maxBacktrack; i++)
+  {
+    cout << "\n--------- alpha = " << alpha << " ---------\n";
+
+    // ----- A-5.2. Compute trial point: xtrial = x0 + alpha_i xhat
+    xtrial.Set(1.0, x0);
+    xtrial.Add(alpha, xhat);
+
+    // ------ A-5.3. if not in filter region go to A.5.4 otherwise go to A-5.5.
+    thxtrial = theta(xtrial);
+    phxtrial = phi(xtrial, mu);
+    filterCheck(thxtrial, phxtrial);    
+    if(!inFilterRegion)
+    {
+      cout << "not in filter region :)\n";
+      // ------ A.5.4: Check sufficient decrease
+      if(!descentDirection)
+      {
+        switchCondition = false;
+      }
+      else
+      {
+        switchCondition = (alpha * pow(abs(Dxphi0_xhat), sPhi) > delta * pow(thx0, sTheta)) ? true : false;
+      }
+      cout << "alpha |Dxphi(x0)^T xhat|^sPhi = " << alpha * pow(abs(Dxphi0_xhat), sPhi) << endl;
+      cout << "delta * theta(x0)^sTheta = " << delta * pow(thx0, sTheta) << endl;
+      cout << "theta(x0) = "     << thx0     << ", thetaMin = "                  << thetaMin             << endl;
+      cout << "theta(xtrial) = " << thxtrial << ", (1-gTheta) *theta(x0) = "     << (1. - gTheta) * thx0 << endl;
+      cout << "phi(xtrial) = "   << phxtrial << ", phi(x0) - gPhi *theta(x0) = " << phx0 - gPhi * thx0   << endl;
+      
+      // Case I      
+      if(thx0 <= thetaMin && switchCondition)
+      {
+        sufficientDecrease = phxtrial <= phx0 + eta * alpha * Dxphi0_xhat ? true : false;
+        if(sufficientDecrease)
+        {
+          if(iAmRoot) { cout << "A-5.4. Case I -- accepted step length.\n"; } 
+          // accept the trial step
+          lineSearchSuccess = true;
+          break;
+        }
+      }
+      else
+      {
+        if(thxtrial <= (1. - gTheta) * thx0 || phxtrial <= phx0 - gPhi * thx0)
+        {
+          if(iAmRoot) { cout << "A-5.4. Case II -- accepted step length.\n"; } 
+          // accept the trial step
+          lineSearchSuccess = true;
+          break;
+        }
+      }
+      // A-5.5: Initialize the second-order correction
+      if((!(thx0 < thxtrial)) && i == 0)
+      {
+        cout << "second order correction\n";
+        problem->c(xtrial, ckSoc);
+        problem->c(x0, ck0);
+        ckSoc.Add(alphaMax, ck0);
+        // A-5.6 Compute the second-order correction.
+        IPNewtonSolve(x0, l0, z0, zhatsoc, Xhatumlsoc, mu, true);
+        mhatsoc.Set(1.0, Xhatumlsoc.GetBlock(1));
+        // alphasoc = MaxStepSize(m0, ml, mhatsoc, tau);
+        //WARNING: not complete but currently solver isn't entering this region
+      }
+    }
+    else
+    {
+      cout << "in filter region :(\n"; 
+    }
+
+    // include more if needed
+    alpha *= 0.5;
+
+  } 
+}
+
+void InteriorPointSolver::projectZ(const Vector &x, Vector &z, double mu)
+{
+  double zi;
+  double mudivmml;
+  for(int i = 0; i < dimM; i++)
+  {
+    zi = z(i);
+    mudivmml = mu / (x(i + dimU) - ml(i));
+    z(i) = max(min(zi, kSig * mudivmml), mudivmml / kSig);
+  }
+}
+
+void InteriorPointSolver::filterCheck(double th, double ph)
+{
+  inFilterRegion = false;
+  if(th > thetaMax)
+  {
+    inFilterRegion = true;
+  }
+  else
+  {
+    for(int i = 0; i < F1.Size(); i++)
+    {
+      if(th >= F1[i] && ph >= F2[i])
+      {
+        inFilterRegion = true;
+        break;
+      }
+    }
+  }
+}
+
+double InteriorPointSolver::E(const BlockVector &x, const Vector &l, const Vector &zl, double mu, bool print)
+{
+  double E1, E2, E3;
+  double sc, sd;
+  BlockVector gradL(block_offsetsx); gradL = 0.0; // stationarity grad L = grad f + J^T l - z
+  Vector cx(dimC); cx = 0.0;     // feasibility c = c(x)
+  Vector comp(dimM); comp = 0.0; // complementarity M Z - mu 1
+
+  DxL(x, l, zl, gradL);
+  E1 = gradL.Normlinf();
+
+  problem->c(x, cx);
+  E2 = cx.Normlinf();
+
+  for(int ii = 0; ii < dimM; ii++) 
+  { 
+    comp(ii) = x(dimU + ii) * zl(ii) - mu;
+  }
+  E3 = comp.Normlinf();
+
+  double ll1, zl1;
+  zl1 = zl.Norml1() / double(dimC + dimM);
+  ll1 =  l.Norml1();
+  sc = max(sMax, zl1 / (double(dimM)) ) / sMax;
+  sd = max(sMax, (ll1 + zl1) / (double(dimC + dimM))) / sMax;
+  if(iAmRoot && print)
+  {
+    cout << "evaluating optimality error for mu = " << mu << endl;
+    cout << "stationarity measure = "    << E1 / sd << endl;
+    cout << "feasibility measure  = "    << E2      << endl;
+    cout << "complimentarity measure = " << E3 / sc << endl;
+  }
+  return max(max(E1 / sd, E2), E3 / sc);
+}
+
+double InteriorPointSolver::E(const BlockVector &x, const Vector &l, const Vector &zl, bool print)
+{
+  return E(x, l, zl, 0.0, print);
+}
+
+double InteriorPointSolver::theta(const BlockVector &x)
+{
+  Vector cx(dimC); cx = 0.0;
+  problem->c(x, cx);
+  return sqrt(InnerProduct(cx, cx));
+}
+
+// log-barrier objective
+double InteriorPointSolver::phi(const BlockVector &x, double mu)
+{
+  double fx = problem->CalcObjective(x); 
+  double logBarrierLoc = 0.0;
+  for(int i = 0; i < dimM; i++) 
+  { 
+    logBarrierLoc += log(x(dimU+i)-ml(i));
+  }
+  double logBarrierGlb = 0.0;
+  logBarrierGlb = logBarrierLoc;
+  return fx - mu * logBarrierGlb;
+}
+
+
+// gradient of log-barrier objective with respect to x = (u, m)
+void InteriorPointSolver::Dxphi(const BlockVector &x, double mu, BlockVector &y)
+{
+  problem->CalcObjectiveGrad(x, y);
+  for(int i = 0; i < dimM; i++) 
+  { 
+    y(dimU + i) -= mu / (x(dimU + i) - ml(i));
+  } 
+}
+
+// Lagrangian function evaluation
+// L(x, l, zl) = f(x) + l^T c(x) - zl^T m
+double InteriorPointSolver::L(const BlockVector &x, const Vector &l, const Vector &zl)
+{
+  double fx = problem->CalcObjective(x);
+  Vector cx(dimC); problem->c(x, cx);
+  return (fx + InnerProduct(cx, l) - InnerProduct(x.GetBlock(1), zl));
+}
+
+void InteriorPointSolver::DxL(const BlockVector &x, const Vector &l, const Vector &zl, BlockVector &y)
+{
+  // evaluate the gradient of the objective with respect to the primal variables x = (u, m)
+  BlockVector gradxf(block_offsetsx); gradxf = 0.0;
+  problem->CalcObjectiveGrad(x, gradxf);
+  
+  SparseMatrix *Jacu, *Jacm, *JacuT, *JacmT;
+  Jacu = problem->Duc(x); Jacm = problem->Dmc(x);
+  JacuT = Transpose(*Jacu);
+  JacmT = Transpose(*Jacm);
+  JacuT->Mult(l, y.GetBlock(0));
+  JacmT->Mult(l, y.GetBlock(1));
+  delete Jacu; delete JacuT;
+  delete Jacm; delete JacmT;
+  y.Add(1.0, gradxf);
+  (y.GetBlock(1)).Add(-1.0, zl);
+}
+
+
+bool InteriorPointSolver::GetConverged() const
+{
+  return converged;
+}
+
+void InteriorPointSolver::SetTol(double Tol)
+{
+  tol = Tol;
+}
+
+void InteriorPointSolver::SetMaxIter(int max_it)
+{
+  max_iter = max_it;
+}
+
+void InteriorPointSolver::SetBarrierParameter(double mu_0)
+{
+  mu_k = mu_0;
+}
+
+void InteriorPointSolver::SaveLogBarrierHessianIterates(bool save)
+{
+  MFEM_ASSERT(MyRank == 0 || save == false, "currently can only save logbarrier hessian in serial codes");
+  saveLogBarrierIterates = save;
+}
+
+void InteriorPointSolver::SetLinearSolver(int LinSolver)
+{
+  linSolver = LinSolver;
+}
+
+
+
+InteriorPointSolver::~InteriorPointSolver() 
+{
+  delete Wmm;
+  delete Huu;
+  delete Hum;
+  delete Hmu;
+  delete Hmm;
+  delete Hum;
+  delete Ju;
+  delete Jm;
+  delete JuT;
+  delete JmT;
+
+  F1.DeleteAll();
+  F2.DeleteAll();
+  block_offsetsx.DeleteAll();
+  block_offsetsumlz.DeleteAll();
+  block_offsetsuml.DeleteAll();
+  ml.SetSize(0);
+}

examples/contact/IPsolver.hpp

@@ -0,0 +1,103 @@
+#include "mfem.hpp"
+#include "problems.hpp"
+#include <fstream>
+#include <iostream>
+
+using namespace std;
+using namespace mfem;
+
+
+#ifndef IPSOLVER 
+#define IPSOLVER
+
+class InteriorPointSolver
+{
+protected:
+    OptProblem* problem;
+    double tol;
+    int  max_iter;
+    double mu_k; // \mu_k
+    Vector lk, zlk, mf;
+
+    double sMax, kSig, tauMin, eta, thetaMin, delta, sTheta, sPhi, kMu, thetaMu;
+    double thetaMax, kSoc, gTheta, gPhi, kEps;
+	
+    // filter
+    Array<double> F1, F2;
+	
+    // quantities computed in lineSearch
+    double alpha, alphaz;
+    double thx0, thxtrial;
+    double phx0, phxtrial;
+    bool descentDirection, switchCondition, sufficientDecrease, lineSearchSuccess, inFilterRegion;
+    double Dxphi0_xhat;
+
+    int dimU, dimM, dimC;
+    Array<int> block_offsetsumlz, block_offsetsuml, block_offsetsx;
+    Vector ml;
+
+    Vector ckSoc;
+    SparseMatrix * Huu = nullptr; 
+    SparseMatrix * Hum = nullptr; 
+    SparseMatrix * Hmu = nullptr; 
+    SparseMatrix * Hmm = nullptr; 
+    SparseMatrix * Wmm = nullptr; 
+    SparseMatrix * Ju  = nullptr; 
+    SparseMatrix * Jm  = nullptr; 
+    SparseMatrix * JuT = nullptr; 
+    SparseMatrix * JmT  = nullptr;;
+    
+    int jOpt;
+    bool converged;
+    
+    int MyRank;
+    bool iAmRoot;
+
+    bool saveLogBarrierIterates;
+
+    int linSolver;
+    std::ofstream IPNewtonKrylovIters;
+
+    ParFiniteElementSpace *Vh;
+    Array<int> cgnum_iterations;
+
+
+    // not sure if this data is needed or if it can 
+    // all be accounted for in the problem class
+    // which variables have equality constraints
+    //Array<int> eqConstrainedVariables;
+    //Array<double> eqConstrainedValues;
+
+
+
+public:
+    InteriorPointSolver(OptProblem*, ParFiniteElementSpace *);
+    void Mult(const BlockVector& , BlockVector&); // used when the user wants to be aware of bound-constrained variable m >= ml
+    void Mult(const Vector&, Vector &); // useful when the user doesn't need to know about bound-constrained variable   m >= ml
+    double MaxStepSize(Vector& , Vector& , Vector& , double);
+    double MaxStepSize(Vector& , Vector& , double);
+    void FormIPNewtonMat(BlockVector& , Vector& , Vector& , BlockOperator &);
+    void IPNewtonSolve(BlockVector& , Vector& , Vector& , Vector&, BlockVector& , double, bool);
+    void lineSearch(BlockVector& , BlockVector& , double);
+    void projectZ(const Vector & , Vector &, double);
+    void filterCheck(double, double);
+    double E(const BlockVector &, const Vector &, const Vector &, double, bool);
+    double E(const BlockVector &, const Vector &, const Vector &, bool);
+    bool GetConverged() const;
+    // TO DO: include Hessian of Lagrangian
+    double theta(const BlockVector &);
+    double phi(const BlockVector &, double);
+    void Dxphi(const BlockVector &, double, BlockVector &);
+    double L(const BlockVector &, const Vector &, const Vector &);
+    void DxL(const BlockVector &, const Vector &, const Vector &, BlockVector &);
+    void SetTol(double);
+    void SetMaxIter(int);
+    void SetBarrierParameter(double);    
+    void SaveLogBarrierHessianIterates(bool);
+    void SetLinearSolver(int);
+    Vector GetBoundConstrainedVariable() {return mf;}
+    Array<int> & GetCGIterNumbers() {return cgnum_iterations;}
+    virtual ~InteriorPointSolver();
+};
+
+#endif

examples/contact/README.md

@@ -0,0 +1,17 @@
+# OneProcessAMGContact
+
+
+
+Be sure to edit the makefile so that it points to a parallel MFEM build
+
+specifically the MFEM_BUILD_DIR
+
+
+after building exQPContactBlockTL one can
+
+1. run the bash script scalingJobArray.bat via `source scalingJobArray.bat' which will populate the CG iterations required to solve
+      various linear systems into the data/ subdirectory
+2. run the python script data/process.py in order to put the scaling information into the single files algorithmicScaling_Elasticity.dat and algorithmicScaling_noElasticity.dat
+      in order to see the number of average AMG-CG iterations per optimization solve. 
+
+

examples/contact/exQPContactBlockTL.cpp

@@ -0,0 +1,274 @@
+//                                Contact example
+//
+// Compile with: make contact
+//
+// Sample runs:  ./contact -m1 block1.mesh -m2 block2.mesh -at "5 6 7 8"
+// Sample runs:  ./contact -m1 block1_d.mesh -m2 block2_d.mesh -at "5 6 7 8"
+
+#include <fstream>
+#include <iostream>
+#include <array>
+
+#include "mfem.hpp"
+#include "problems.hpp"
+#include "IPsolver.hpp"
+
+using namespace std;
+using namespace mfem;
+
+
+int main(int argc, char *argv[])
+{
+  Mpi::Init(argc, argv);
+  Hypre::Init();
+  int linSolver = 2;
+  int maxIPMiters = 30;
+  bool iAmRoot = true;
+  int ref_levels = 0;
+
+  OptionsParser args(argc, argv);
+  args.AddOption(&linSolver, "-linSolver", "--linearSolver", \
+       "IP-Newton linear system solution strategy.");
+  args.AddOption(&maxIPMiters, "-IPMiters", "--IPMiters",\
+		  "Maximum number of IPM iterations");
+  args.AddOption(&ref_levels, "-r", "--mesh_refinement", \
+		  "Mesh Refinement");
+
+   
+  args.Parse();
+  if(!args.Good())
+  {
+    args.PrintUsage(cout);
+    return 1;
+  }
+  else
+  {
+    if( iAmRoot )
+    {
+      args.PrintOptions(cout);
+    }
+  }
+	
+  // Create an instance of the nlp
+  ExContactBlockTL * contact = new ExContactBlockTL(ref_levels);
+  int ndofs = contact->GetDimD();
+  int nconstraints = contact->GetDimS();
+  std::ofstream problemDimStream;
+  problemDimStream.open("problemDim.dat", ios::out | ios::trunc);
+  problemDimStream << ndofs << endl;
+  problemDimStream.close();
+  std::ofstream problemDimConstraintsStream;
+  problemDimConstraintsStream.open("problemDimConstraints.dat", ios::out | ios::trunc);
+  problemDimConstraintsStream << nconstraints << endl;
+  problemDimConstraintsStream.close();
+  
+  // set up a QP-problem
+  // E(d) = 1 / 2 d^T K d + f^T d
+  // g(d) = J d + g0
+  // where K, J, f and g0 are evaluated at d0 (a valid configuration)
+  
+  // to do: seems more appropriate to evaluate at a valid configuration...
+  //        that is one where the Dirichlet conditions hold... need to pull
+  //        this data from contactBlockTL...
+  Vector d0(ndofs); d0 = 0.0;
+  Array<int> DirichletDofs = contact->GetDirichletDofs();
+  Array<double> DirichletVals = contact->GetDirichletVals();
+  SparseMatrix *K;
+  Vector f(ndofs); f = 0.0;
+  contact->DdE(d0, f); K = contact->DddE(d0);
+  for(int i = 0; i < DirichletDofs.Size(); i++)
+  {
+    d0(DirichletDofs[i]) = DirichletVals[i];
+  }
+  SparseMatrix *J;
+  Vector g0(nconstraints); g0 = 0.0;
+  J = contact->Ddg(d0); contact->g(d0, g0);
+  Vector temp(nconstraints);
+  J->Mult(d0, temp);
+  g0.Add(-1.0, temp);
+
+  // check which rows of the Jacobian are zero!
+  Vector ei(nconstraints); ei = 0.0;
+  Vector JTei(ndofs); JTei = 0.0;
+
+  double normJTei;
+
+  int reduced_nconstraints = 0; // find actual number of constraints
+  
+  
+  Array<int> nonZeroRows;
+  for(int i = 0; i < nconstraints; i++)
+  {
+    ei(i) = 1.0;
+    J->MultTranspose(ei, JTei);
+    // nullify contributions from Dirichlet constrined dofs
+    for(int j = 0; j < DirichletDofs.Size(); j++)
+    {
+      JTei(DirichletDofs[j]) = 0.0;
+    }
+    normJTei = sqrt(InnerProduct(JTei, JTei));
+    if (normJTei > 1.e-12)
+    {
+      reduced_nconstraints += 1;
+      nonZeroRows.Append(i);
+    }
+    ei(i) = 0.0;
+  }
+  cout << "number of linearized constraints = " << reduced_nconstraints << endl; // 9 constraints 
+  
+  // remove zero rows of the gap function Jacobian and corresponding gap function entries
+  SparseMatrix * Jreduced = new SparseMatrix(reduced_nconstraints, ndofs);
+  Vector g0reduced(reduced_nconstraints); g0reduced = 0.0; 
+  
+  
+  for(int i = 0; i < reduced_nconstraints; i++)
+  {
+    Array<int> col_tmp;
+    Vector v_tmp; v_tmp = 0.0;
+    J->GetRow(nonZeroRows[i], col_tmp, v_tmp);
+    
+    /* obtain subset of columns of the given nonZero Jacobian row that are not Dirichlet constrained */
+    bool freeDof;
+    Array<int> loc_indicies;
+    for(int j = 0; j < col_tmp.Size(); j++)
+    {
+      freeDof = true;
+      for(int k = 0; k < DirichletDofs.Size(); k++)
+      {
+        if(col_tmp[j] == DirichletDofs[k])
+        {
+          freeDof = false;
+        }
+      }
+      if(freeDof)
+      {
+        loc_indicies.Append(j);
+      }
+    }
+
+    Array<int> col_tmp_reduced(loc_indicies.Size());
+    Vector v_tmp_reduced(loc_indicies.Size());
+    for(int j = 0; j < loc_indicies.Size(); j++)
+    {
+      col_tmp_reduced[j] = col_tmp[loc_indicies[j]];
+      v_tmp_reduced(j)   = v_tmp(loc_indicies[j]);
+    }
+
+    Jreduced->SetRow(i, col_tmp_reduced, v_tmp_reduced);
+    g0reduced(i) = g0(nonZeroRows[i]);
+  }
+
+
+  QPContactProblem *QPContact = new QPContactProblem(*K, *Jreduced, f, g0reduced);
+  
+  Mesh * mesh1 = new Mesh("meshes/block1.mesh", 1, 1);
+  Mesh * mesh2 = new Mesh("meshes/rotatedblock2.mesh", 1, 1);
+  for(int i = 0; i < ref_levels; i++)
+  {
+     mesh1->UniformRefinement();
+     mesh2->UniformRefinement(); 
+  }
+  
+  int numMeshes = 2;
+  Mesh *meshArray[numMeshes];
+  meshArray[0] = mesh1;
+  meshArray[1] = mesh2;
+  Mesh mesh(meshArray, numMeshes);
+  
+  ParMesh pmesh(MPI_COMM_WORLD, mesh);  
+  H1_FECollection fec(1, mesh.Dimension());
+  ParFiniteElementSpace fespace(&pmesh, &fec, mesh.Dimension(), Ordering::byVDIM);
+
+  InteriorPointSolver * QPContactOptimizer = new InteriorPointSolver(QPContact, &fespace);
+  QPContactOptimizer->SetTol(1.e-6);
+  QPContactOptimizer->SetLinearSolver(linSolver);
+  QPContactOptimizer->SetMaxIter(50);
+  Vector x0(ndofs); x0 = 0.0;
+  for(int i = 0; i < DirichletDofs.Size(); i++)
+  {
+    x0(DirichletDofs[i]) = DirichletVals[i];
+  }
+  Vector xf(ndofs); xf = 0.0;
+  QPContactOptimizer->Mult(x0, xf);
+  
+  double Einitial = QPContact->E(x0);
+  double Efinal = QPContact->E(xf);
+  cout << "Energy objective at initial point = " << Einitial << endl;
+  cout << "Energy objective at QP optimizer = " << Efinal << endl;
+  QPContactOptimizer->GetCGIterNumbers().Print(mfem::out, 20);
+  MFEM_VERIFY(QPContactOptimizer->GetConverged(), "Interior point solver did not converge.");
+  
+  
+  //Mesh * mesh1 = new Mesh("meshes/block1.mesh", 1, 1);
+  //Mesh * mesh2 = new Mesh("meshes/rotatedblock2.mesh", 1, 1);
+  //for(int i = 0; i < ref_levels; i++)
+  //{
+  //   mesh1->UniformRefinement();
+  //   mesh2->UniformRefinement(); 
+  //}
+  //int gdim = mesh1->Dimension();
+  //FiniteElementCollection * fec = new H1_FECollection(1, gdim);
+  //FiniteElementSpace * fespace1 = new FiniteElementSpace(mesh1, fec, gdim, Ordering::byVDIM);
+  //FiniteElementSpace * fespace2 = new FiniteElementSpace(mesh2, fec, gdim, Ordering::byVDIM);
+  // 
+  //GridFunction x1_gf(fespace1);
+  //GridFunction x2_gf(fespace2);
+
+  //int ndof1 = fespace1->GetTrueVSize();
+  //int ndof2 = fespace2->GetTrueVSize();
+  //int ndof  = ndof1 + ndof2;
+  //for(int i = 0; i < ndof1; i++)
+  //{
+  //  x1_gf(i) = xf(i);
+  //}
+  //for(int i = ndof1; i < ndof; i++)
+  //{
+  //  x2_gf(i - ndof1) = xf(i);
+  //}
+
+  //mesh1->SetNodalFESpace(fespace1);
+  //mesh2->SetNodalFESpace(fespace2);
+  //GridFunction *nodes1 = mesh1->GetNodes();
+  //GridFunction *nodes2 = mesh2->GetNodes();
+
+  //{
+  //  *nodes1 += x1_gf;
+  //  *nodes2 += x2_gf;
+  //}
+  //
+
+  //ParaViewDataCollection paraview_dc1("QPContactBody1", mesh1);
+  //paraview_dc1.SetPrefixPath("ParaView");
+  //paraview_dc1.SetLevelsOfDetail(1);
+  //paraview_dc1.SetDataFormat(VTKFormat::BINARY);
+  //paraview_dc1.SetHighOrderOutput(true);
+  //paraview_dc1.SetCycle(0);
+  //paraview_dc1.SetTime(0.0);
+  //paraview_dc1.RegisterField("Body1", &x1_gf);
+  //paraview_dc1.Save();
+  //
+  //ParaViewDataCollection paraview_dc2("QPContactBody2", mesh2);
+  //paraview_dc2.SetPrefixPath("ParaView");
+  //paraview_dc2.SetLevelsOfDetail(1);
+  //paraview_dc2.SetDataFormat(VTKFormat::BINARY);
+  //paraview_dc2.SetHighOrderOutput(true);
+  //paraview_dc2.SetCycle(0);
+  //paraview_dc2.SetTime(0.0);
+  //paraview_dc2.RegisterField("Body2", &x2_gf);
+  //paraview_dc2.Save();
+
+  //delete fespace1;
+  //delete fespace2;
+  //delete fec;
+  //delete mesh1;
+  //delete mesh2;
+  
+  delete QPContact;
+  delete QPContactOptimizer;
+  
+  delete K;
+  delete J;
+  delete Jreduced;
+  delete contact;
+  return 0;
+}

examples/contact/makefile

@@ -0,0 +1,36 @@
+# Use the MFEM build directory
+MFEM_DIR ?= ../..
+MFEM_BUILD_DIR ?= ../..
+SRC = ./
+
+CONFIG_MK = $(MFEM_BUILD_DIR)/config/config.mk
+
+MFEM_LIB_FILE = mfem_is_not_built
+-include $(CONFIG_MK)
+
+# Remove built-in rule
+#%: %.cpp
+
+exQPContactBlockTL: exQPContactBlockTL.o problems.o IPsolver.o $(MFEM_LIB_FILE)
+	$(MFEM_CXX) $(MFEM_FLAGS) exQPContactBlockTL.o problems.o IPsolver.o -o $@ $(MFEM_LIBS)
+
+
+
+exQPContactBlockTL.o: exQPContactBlockTL.cpp $(CONFIG_MK)
+	$(MFEM_CXX) $(MFEM_FLAGS) -c $<
+
+problems.o: problems.cpp $(CONFIG_MK)
+	$(MFEM_CXX) $(MFEM_FLAGS) -c $< 
+
+IPsolver.o: IPsolver.cpp $(CONFIG_MK)
+	$(MFEM_CXX) $(MFEM_FLAGS) -c $<
+
+# Generate an error message if the MFEM library is not built and exit
+$(MFEM_LIB_FILE):
+	$(error The MFEM library is not built)
+
+.PHONY: clean
+clean:
+	rm -f *.o exQPContactBlockTL
+
+

examples/contact/meshes/block1.mesh

@@ -0,0 +1,103 @@
+MFEM mesh v1.0
+
+#
+# MFEM Geometry Types (see mesh/geom.hpp):
+#
+# POINT       = 0
+# SEGMENT     = 1
+# TRIANGLE    = 2
+# SQUARE      = 3
+# TETRAHEDRON = 4
+# CUBE        = 5
+# PRISM       = 6
+#
+
+dimension
+3
+
+elements
+9
+1   5     0      1     3     2     8    9     11    10
+1   5     2      3     5     4    10    11    13    12
+1   5     4      5     7     6    12    13    15    14
+1   5     8      9    11    10    16    17    19    18
+1   5     10    11    13    12    18    19    21    20
+1   5     12    13    15    14    20    21    23    22
+1   5     16    17    19    18    24    25    27    26
+1   5     18    19    21    20    26    27    29    28
+1   5     20    21    23    22    28    29    31    30
+
+
+
+# 0 nothing
+# 1 dirichlet bc
+# 2 contact
+boundary
+30
+1   3    1     0     2     3
+1   3    3     2     4     5
+1   3    5     4     6     7
+1   3    24    25    27    26
+1   3    26    27    29    28
+1   3    28    29    31    30
+2   3    2     0     8    10
+2   3    4     2    10    12
+2   3    6     4    12    14
+2  3    10    8    16    18
+2  3    12    10    18    20
+2  3    14    12    20    22
+2  3    18    16    24    26
+2  3    20    18    26    28
+2  3    22    20    28    30
+3  3    1     3    11    9
+3  3     3     5    13    11
+3  3     5     7    15    13
+3  3    9    11    19    17
+3  3    11    13    21    19
+3  3    13    15    23    21
+3  3    17    19    27    25
+3  3    19    21    29    27
+3  3    21    23    31    29
+1  3     8     0     1    9
+1  3    16     8    9    17
+1  3    24    16    17    25
+1  3     6    14    15     7
+1  3    14    22    23    15
+1  3    22    30    31    23
+
+
+vertices
+32
+3
+   -1.0000         0         0
+         0         0         0
+   -1.0000    0.3000         0
+         0    0.3000         0
+   -1.0000    0.6500         0
+         0    0.6500         0
+   -1.0000    1.0000         0
+         0    1.0000         0
+   -1.0000         0    0.3000
+         0         0    0.3000
+   -1.0000    0.3000    0.3500
+         0    0.3000    0.3500
+   -1.0000    0.6500    0.3000
+         0    0.6500    0.3000
+   -1.0000    1.0000    0.3000
+         0    1.0000    0.3000
+   -1.0000         0    0.6500
+         0         0    0.6500
+   -1.0000    0.3000    0.6500
+         0    0.3000    0.6500
+   -1.0000    0.6500    0.6500
+         0    0.6500    0.6500
+   -1.0000    1.0000    0.6500
+         0    1.0000    0.6500
+   -1.0000         0    1.0000
+         0         0    1.0000
+   -1.0000    0.3000    1.0000
+         0    0.3000    1.0000
+   -1.0000    0.6500    1.0000
+         0    0.6500    1.0000
+   -1.0000    1.0000    1.0000
+         0    1.0000    1.0000

examples/contact/meshes/rotatedblock2.mesh

@@ -0,0 +1,70 @@
+
+MFEM mesh v1.0
+
+#
+# MFEM Geometry Types (see mesh/geom.hpp):
+#
+# POINT       = 0
+# SEGMENT     = 1
+# TRIANGLE    = 2
+# SQUARE      = 3
+# TETRAHEDRON = 4
+# CUBE        = 5
+# PRISM       = 6
+#
+
+dimension
+3
+
+# 1 nothing
+elements
+4
+1   5     0     1     3     2     6     7    9     8
+1   5     2     3     5     4     8     9    11    10
+1   5     6     7     9     8    12    13    15    14
+1   5     8     9    11    10    14    15    17    16
+
+# 0 nothing
+# 1 dirichlet bc
+# 2 contact
+boundary
+16
+1   3     1     0     2     3
+1   3     3     2     4     5
+1   3    12    13    15    14
+1   3    14    15    17    16
+3   3     2     0     6     8
+3   3     4     2     8    10
+3   3     8     6    12    14
+3   3    10     8    14    16
+2   3     1     3     9     7
+2   3     3     5    11    9
+2   3     7    9    15    13
+2   3     9    11    17    15
+1   3     6     0     1     7
+1   3    12     6     7    13
+1   3     4    10    11     5
+1   3    10    16    17    11
+
+vertices
+18
+3
+
+0.000000000000 0.145770950245 0.443895630208
+0.507100000000 0.145770950245 0.443895630208
+0.000000000000 0.350937660019 0.294833290227
+0.507100000000 0.350937660019 0.294833290227
+0.000000000000 0.556104369792 0.145770950245
+0.507100000000 0.556104369792 0.145770950245
+0.000000000000 0.294833290227 0.649062339981
+0.507100000000 0.294833290227 0.649062339981
+0.000000000000 0.500000000000 0.500000000000
+0.507100000000 0.500000000000 0.500000000000
+0.000000000000 0.705166709773 0.350937660019
+0.507100000000 0.705166709773 0.350937660019
+0.000000000000 0.443895630208 0.854229049755
+0.507100000000 0.443895630208 0.854229049755
+0.000000000000 0.649062339981 0.705166709773
+0.507100000000 0.649062339981 0.705166709773
+0.000000000000 0.854229049755 0.556104369792
+0.507100000000 0.854229049755 0.556104369792

examples/contact/nodepair.hpp

@@ -0,0 +1,897 @@
+
+using namespace std;
+using namespace mfem;
+
+void BasisEval(const Vector xi, Vector &N, DenseMatrix &dNdxi) // dNdxi is 2*4
+{
+   N[0] = 0.25*(1-xi[0])*(1-xi[1]);
+   N[1] = 0.25*(1+xi[0])*(1-xi[1]);
+   N[2] = 0.25*(1+xi[0])*(1+xi[1]);
+   N[3] = 0.25*(1-xi[0])*(1+xi[1]);
+
+   dNdxi(0,0) = 0.25*(-1+xi[1]);
+   dNdxi(0,1) = 0.25*(1-xi[1]);
+   dNdxi(0,2) = 0.25*(1+xi[1]);
+   dNdxi(0,3) = 0.25*(-1-xi[1]);
+   dNdxi(1,0) = 0.25*(-1+xi[0]);
+   dNdxi(1,1) = 0.25*(-1-xi[0]);
+   dNdxi(1,2) = 0.25*(1+xi[0]);
+   dNdxi(1,3) = 0.25*(1-xi[0]);
+}
+
+
+void BasisEvalDerivs(const Vector xi, Vector& N, DenseMatrix& dNdxi,
+                     DenseMatrix& dN2dxi)
+{
+   N[0] = 0.25*(1-xi[0])*(1-xi[1]);
+   N[1] = 0.25*(1+xi[0])*(1-xi[1]);
+   N[2] = 0.25*(1+xi[0])*(1+xi[1]);
+   N[3] = 0.25*(1-xi[0])*(1+xi[1]);
+
+   dNdxi.SetSize(2,4); dNdxi = 0.0;
+   dN2dxi.SetSize(3,4);
+   dN2dxi = 0.0;  // first row dxi2, second detadxi, third deta2
+
+   dNdxi(0,0) = 0.25*(-1+xi[1]); dNdxi(0,1) = 0.25*(1-xi[1]);
+   dNdxi(0,2) = 0.25*(1+xi[1]);  dNdxi(0,3) = 0.25*(-1-xi[1]);
+   dNdxi(1,0) = 0.25*(-1+xi[0]); dNdxi(1,1) = 0.25*(-1-xi[0]);
+   dNdxi(1,2) = 0.25*(1+xi[0]);  dNdxi(1,3) = 0.25*(1-xi[0]);
+
+   dN2dxi(1,0) = 0.25; dN2dxi(1,1) = -0.25; dN2dxi(1,2) = 0.25;
+   dN2dxi(1,3) = -0.25;
+}
+
+// returns the vector and matrix form of the shape functions and its derivative
+void BasisVectorDerivs(const Vector xi, DenseMatrix& N, DenseMatrix& dNdxi,
+                       DenseMatrix& ddNdxi)
+{
+   N.SetSize(3,12); N = 0.0;
+   N(0,0) = 0.25*(1-xi[0])*(1-xi[1]); N(0,3) = 0.25*(1+xi[0])*(1-xi[1]);
+   N(0,6) = 0.25*(1+xi[0])*(1+xi[1]); N(0,9) = 0.25*(1-xi[0])*(1+xi[1]);
+
+   N(1,1) = 0.25*(1-xi[0])*(1-xi[1]); N(1,4) = 0.25*(1+xi[0])*(1-xi[1]);
+   N(1,7) = 0.25*(1+xi[0])*(1+xi[1]); N(1,10) = 0.25*(1-xi[0])*(1+xi[1]);
+
+   N(2,2) = 0.25*(1-xi[0])*(1-xi[1]); N(2,5) = 0.25*(1+xi[0])*(1-xi[1]);
+   N(2,8) = 0.25*(1+xi[0])*(1+xi[1]); N(2,11) = 0.25*(1-xi[0])*(1+xi[1]);
+
+   dNdxi.SetSize(3*2, 3*4);  dNdxi = 0.0;
+   dNdxi(0,0) = 0.25*(-1+xi[1]);  dNdxi(0,3) = 0.25*(1-xi[1]);
+   dNdxi(0,6) = 0.25*(1+xi[1]);  dNdxi(0,9) = 0.25*(-1-xi[1]);
+   dNdxi(1,1) = 0.25*(-1+xi[1]);  dNdxi(1,4) = 0.25*(1-xi[1]);
+   dNdxi(1,7) = 0.25*(1+xi[1]);  dNdxi(1,10) = 0.25*(-1-xi[1]);
+   dNdxi(2,2) = 0.25*(-1+xi[1]);  dNdxi(2,5) = 0.25*(1-xi[1]);
+   dNdxi(2,8) = 0.25*(1+xi[1]);  dNdxi(2,11) = 0.25*(-1-xi[1]);
+
+   dNdxi(3,0) = 0.25*(-1+xi[0]);  dNdxi(3,3) = 0.25*(-1-xi[0]);
+   dNdxi(3,6) = 0.25*(1+xi[0]);  dNdxi(3,9) = 0.25*(1-xi[0]);
+   dNdxi(4,1) = 0.25*(-1+xi[0]);  dNdxi(4,4) = 0.25*(-1-xi[0]);
+   dNdxi(4,7) = 0.25*(1+xi[0]);  dNdxi(4,10) = 0.25*(1-xi[0]);
+   dNdxi(5,2) = 0.25*(-1+xi[0]);  dNdxi(5,5) = 0.25*(-1-xi[0]);
+   dNdxi(5,8) = 0.25*(1+xi[0]);  dNdxi(5,11) = 0.25*(1-xi[0]);
+
+   ddNdxi.SetSize(3*4, 3*4); ddNdxi = 0.0;
+   ddNdxi(3,0) = 0.25; ddNdxi(3,3) = -0.25;
+   ddNdxi(3,6) = 0.25; ddNdxi(3,9) = -0.25;
+   ddNdxi(4,1) = 0.25; ddNdxi(4,4) = -0.25;
+   ddNdxi(4,7) = 0.25; ddNdxi(4,10) = -0.25;
+   ddNdxi(5,2) = 0.25; ddNdxi(5,5) = -0.25;
+   ddNdxi(5,8) = 0.25; ddNdxi(5,11) = -0.25;
+
+   ddNdxi(6,0) = 0.25; ddNdxi(6,3) = -0.25;
+   ddNdxi(6,6) = 0.25; ddNdxi(6,9) = -0.25;
+   ddNdxi(7,1) = 0.25; ddNdxi(7,4) = -0.25;
+   ddNdxi(7,7) = 0.25; ddNdxi(7,10) = -0.25;
+   ddNdxi(8,2) = 0.25; ddNdxi(8,5) = -0.25;
+   ddNdxi(8,8) = 0.25; ddNdxi(8,11) = -0.25;
+}
+
+
+void cross(const Vector a, const Vector b, Vector& c)
+{
+   assert(a.Size()==3);
+   c.SetSize(3);
+   c[0] = a[1]*b[2] - a[2]*b[1];
+   c[1] = -a[0]*b[2] + b[0]*a[2];
+   c[2] = a[0]*b[1] - a[1]*b[0];
+
+}
+// a outer b
+void outer(const Vector a, const Vector b, DenseMatrix& c)
+{
+   int m = a.Size();
+   int n = b.Size();
+   assert(c.Height()==m);
+   assert(c.Width() ==n);
+   for (int i=0; i<m; i++)
+   {
+      for (int j=0; j<n; j++)
+      {
+         c(i,j) = a[i]*b[j];
+      }
+   }
+}
+// dphidxi 2*4
+// coords 4*3
+void ComputeNormal(const DenseMatrix& dphidxi, const DenseMatrix& coords,
+                   Vector& normal, double& nnorm)
+{
+
+   DenseMatrix dxdxi(2,3);
+   Mult(dphidxi, coords, dxdxi);
+   Vector dxdxi1(3);
+   Vector dxdxi2(3);
+
+   dxdxi.GetRow(0,dxdxi1);
+   dxdxi.GetRow(1,dxdxi2);
+
+   cross(dxdxi1, dxdxi2, normal);  // is there a cross product?  no
+   // VectorCrossProductCoefficient::Eval has hard-coded cross product
+   nnorm = normal.Norml2( );
+   normal /=  nnorm;
+}
+
+void SlaveToMaster(const DenseMatrix& m_coords, const Vector& s_x, Vector& xi)
+{
+   bool converged = false;
+   bool pt_on_elem = false;
+   int dim = 3;
+   xi.SetSize(dim-1);
+   xi = 0.0;
+   int max_iter = 15;
+   double off_el_xi = 1e-2;
+   double proj_newton_tol = 1e-13;
+   double proj_max_gap = 0.5;
+   Vector gap_v(dim);
+   // warm start from linear solution
+
+   for (int it=0; it<max_iter; it++)
+   {
+      //cout<<it<<endl;
+      Vector m_N(4);
+      m_N = 0.;
+      DenseMatrix m_dN(2,4);
+      m_dN = 0.;
+      DenseMatrix m_dN2(3,4);
+      m_dN2 = 0.;
+      BasisEvalDerivs(xi, m_N, m_dN, m_dN2);
+
+      Vector x_c(dim);
+      m_coords.MultTranspose(m_N, x_c);
+
+      gap_v = s_x;
+      gap_v -= x_c;
+
+      DenseMatrix m_dx(2,3);
+      m_dx = 0.;
+      Mult(m_dN, m_coords, m_dx);
+
+      Vector r(dim-1);
+      r = 0.0;
+      m_dx.Mult(gap_v, r);
+
+      if (r.Normlinf() < proj_newton_tol)
+      {
+         converged = true;
+         break;
+      }
+
+      DenseMatrix drdxi(dim-1,dim-1);
+      drdxi = 0.;
+      MultABt(m_dx, m_dx, drdxi); // m_dx * m_dx.T
+      drdxi *= -1.0;
+
+      DenseMatrix m_dx2(3,3); m_dx2 = 0.0;
+      Mult(m_dN2,m_coords, m_dx2);
+
+      //m_d2x = m_dN(:,:,2) * m_elem_coords(1:4,:);  //m_dN(:,:,2) is 3*4
+      for (int d=0; d<3; d++)
+      {
+         DenseMatrix Mtemp(2,2); Mtemp = 0.0;
+         Mtemp(0,0) = m_dx2(0,d); Mtemp(0,1) = m_dx2(1,d);
+         Mtemp(1,0) = m_dx2(1,d); Mtemp(1,1) = m_dx2(2,d);
+
+         drdxi.Add(gap_v[d], Mtemp);
+      }
+
+      //cond_num = rcond(drdxi);  condition number?
+      //drdxi.TestInversion();
+      DenseMatrixInverse drdxi_inv(drdxi);
+      Vector xi_tmp(dim-1);
+
+      drdxi_inv.Mult(r,xi_tmp);
+      xi -= xi_tmp;
+   }
+   if (!converged)
+   {
+      xi = 0.0;
+   }
+   off_el_xi += 1 ; // tolerance of offset of xi outside [-1,1]
+
+   //cout<<gap_v.Norml2()<<" " <<xi.Normlinf()<<endl;
+   //
+   // Discuss with Frank... what is happening here
+   if (gap_v.Norml2() < proj_max_gap && xi.Normlinf() <= off_el_xi)
+   {
+      pt_on_elem = true;
+   }
+
+   if (pt_on_elem)
+   {
+     //cout << "convergence of node to segment projection? " << converged << endl;
+     //for(int i = 0; i < 2; i++)
+     //{
+     //  cout << "xi_" << i << " = " << xi(i) << endl;
+     //}
+   }
+   MFEM_VERIFY(pt_on_elem == true, "xi went out of bounds");
+   MFEM_VERIFY(converged == true, "projection didn't converge");
+}
+
+
+
+// m_coords is expected to be 4 * 3
+void  ComputeGapJacobian(const Vector x_s, const Vector xi,
+                         const DenseMatrix m_coords,
+                         double& gap, Vector& normal, Vector& dgdxm, Vector& dgdxs)
+{
+   Vector m_N(4);
+   DenseMatrix m_dN(2,4);
+   DenseMatrix m_dN2(3,4);
+   BasisEvalDerivs(xi, m_N, m_dN, m_dN2);
+
+   Vector x_c(3);
+   m_coords.MultTranspose(m_N, x_c);
+
+   Vector gap_v(3); gap_v = 0.0;
+   gap_v  = x_s;
+   gap_v -= x_c;
+
+   DenseMatrix m_dx(2,3);
+   Mult(m_dN, m_coords, m_dx);
+
+   double nnorm = 0;
+   ComputeNormal(m_dN, m_coords, normal, nnorm);
+
+   gap = gap_v * normal; // gap function value, dot product between vectors
+
+   //dr_dx = zeros(2,4,3); % nsegment, nodes in quad, ndim
+
+   DenseMatrix dr_dx_res1(4,3); dr_dx_res1 = 0.;
+   DenseMatrix dr_dx_res2(4,3); dr_dx_res2 = 0.;
+
+   Vector m_dxrow1(3);
+   m_dx.GetRow(0, m_dxrow1);
+   outer(m_N, m_dxrow1, dr_dx_res1);// 4*1 times 1*3
+   dr_dx_res1 *= -1.0;
+
+   Vector m_dxrow2(3);
+   m_dx.GetRow(1, m_dxrow2);
+   outer(m_N, m_dxrow2, dr_dx_res2);// 4*1 times 1*3
+   dr_dx_res2 *= -1.0;
+
+   Vector m_dNrow1(4); m_dN.GetRow(0, m_dNrow1);
+   Vector m_dNrow2(4); m_dN.GetRow(1, m_dNrow2);
+
+   DenseMatrix dr_dx_res1_tmp(4,3); dr_dx_res1_tmp = 0.;
+   DenseMatrix dr_dx_res2_tmp(4,3); dr_dx_res2_tmp = 0.;
+   outer(m_dNrow1, gap_v, dr_dx_res1_tmp);// 4*1 times 1*3
+   outer(m_dNrow2, gap_v, dr_dx_res2_tmp);// 4*1 times 1*3
+
+   dr_dx_res1 += dr_dx_res1_tmp; // outer product in vector?
+   dr_dx_res2 += dr_dx_res2_tmp;
+
+
+   DenseMatrix K_dxidx1(2,2); // 2*2
+   K_dxidx1 = 0.;
+   MultABt(m_dx, m_dx, K_dxidx1); // m_dx * m_dx.T
+
+   Vector v_dxidx2(4);
+   m_coords.Mult(gap_v, v_dxidx2); // m_coords * gap_v;  // 4*3 * 3 = 4
+
+   DenseMatrix K_dxidx2(2,2); K_dxidx2 = 0.0;
+
+   Vector m_dN2row1(4); m_dN2.GetRow(0, m_dN2row1);
+   Vector m_dN2row2(4); m_dN2.GetRow(1, m_dN2row2);
+   Vector m_dN2row3(4); m_dN2.GetRow(2, m_dN2row3);
+   // how to get 2nd order? multidimensional matrix?
+   K_dxidx2(0,0) = m_dN2row1 * v_dxidx2; // how would 4*1 * 1*4 be computed?
+   K_dxidx2(0,1) = m_dN2row2 * v_dxidx2;
+   K_dxidx2(1,0) = m_dN2row2 * v_dxidx2;
+   K_dxidx2(1,1) = m_dN2row3 * v_dxidx2;
+
+   DenseMatrix K_dxidx(2,2);
+   K_dxidx -= K_dxidx1;
+   K_dxidx += K_dxidx2;
+
+   // resize the vectors and matrices
+   Vector dxidx(24); dxidx = 0.0;
+   Vector drdx_r(24); drdx_r = 0.0;
+
+   for (int i=0; i<4; i++)
+   {
+      for (int j=0; j<3; j++)
+      {
+         drdx_r[4*j+i] = dr_dx_res1(i,j);
+         drdx_r[4*j+i+12] = dr_dx_res2(i,j);
+
+      }
+   }
+   //drdx_r(1:4*3,1) = reshape(dr_dx_res(:,:,1),4*3,1);
+   //drdx_r(4*3+1:2*4*3,1) = reshape(dr_dx_res(:,:,2),4*3,1);
+   DenseMatrix drdx_K(24,24); drdx_K = 0.;
+   for (int i =0; i<12; i++)
+   {
+      drdx_K(i,i) = K_dxidx(0,0);
+      drdx_K(i,12+i) = K_dxidx(0,1);
+      drdx_K(12+i,i) = K_dxidx(1,0);
+      drdx_K(12+i,12+i) = K_dxidx(1,1);
+   }
+
+   DenseMatrixInverse drdxK_inv(drdx_K);
+   drdxK_inv.Mult(drdx_r,dxidx);
+   // LinearSolve (drdx_K,drdx_r, dxidx) ; //???
+   dxidx *= -1.0;
+
+
+
+   Vector drdxs_r(6);
+   drdxs_r[0] = m_dx(0,0); drdxs_r[1] = m_dx(0,1); drdxs_r[2] = m_dx(0,2);
+   drdxs_r[3] = m_dx(1,0); drdxs_r[4] = m_dx(1,1); drdxs_r[5] = m_dx(1,2);
+
+   DenseMatrix drdxs_K(6,6); drdxs_K = 0.;
+   for (int i=0; i<3; i++)
+   {
+      drdxs_K(i,i) = K_dxidx(0,0);
+      drdxs_K(i,3+i) = K_dxidx(0,1);
+      drdxs_K(i+3,i) = K_dxidx(1,0);
+      drdxs_K(i+3,i+3) = K_dxidx(1,1);
+   }
+
+   Vector dxidxs(6); dxidxs = 0.0;
+   DenseMatrixInverse drdxsK_inv(drdxs_K);
+   drdxsK_inv.Mult(drdxs_r,dxidxs);
+   dxidxs *= -1.0;
+   //dxidxs = -drdxs_K\drdxs_r;
+
+   //dxidx = reshape(dxidx, 4,3,2); dxidxs = reshape(dxidxs, 1,3,2);
+
+   dgdxm.SetSize(12); dgdxm = 0.;
+   DenseMatrix dgdxm_tmp(4,3);
+   outer(m_N, normal,dgdxm_tmp);
+   for (int i=0; i<4; i++)
+   {
+      for (int j=0; j<3; j++)
+      {
+         dgdxm[3*i+j] = -dgdxm_tmp(i,j);
+      }
+   }
+   //dxidx_M = -m_dN(1:2,:,1) * (m_coords(1:4,:)*normal'); % this turns out to be 0
+
+   dgdxs.SetSize(3);
+   dgdxs += normal;
+   //dgdxs = dgdxs + dxidx_M(1) * dxidxs(:,:,1) + dxidx_M(2) * dxidxs(:,:,2);
+};
+
+void ComputeGapHessian(const Vector x_s, const Vector xi,
+                       const DenseMatrix m_coords,
+                       DenseMatrix& dg2dx)
+{
+   Vector m_N(4);
+   DenseMatrix m_dN(2,4);
+   DenseMatrix m_dN2(3,4);
+   BasisEvalDerivs(xi, m_N, m_dN, m_dN2);
+
+   int dim = 3;
+   int num_dofs1 = dim;
+   int num_dofs2 = 4*dim;
+   int num_dofs = num_dofs1 + num_dofs2;
+   dg2dx.SetSize(num_dofs,num_dofs); dg2dx = 0.0;
+
+   Vector x_c(3);
+   m_coords.MultTranspose(m_N,x_c);
+
+   Vector gap_v(3); gap_v = 0.0;
+   gap_v  = x_s;
+   gap_v -= x_c;
+
+   DenseMatrix m_dx(2,3);
+   Mult(m_dN, m_coords, m_dx);
+
+   DenseMatrix m_dx2(3,3); m_dx2 = 0.0;
+   Mult(m_dN2,m_coords, m_dx2);
+   double nnorm = 0.0;
+   Vector normal(3); normal = 0.0;
+   ComputeNormal(m_dN, m_coords, normal, nnorm);
+
+   double gap = gap_v * normal; // gap function value, dot product between vectors
+
+   DenseMatrix M(2,2); M = 0.0;
+   MultABt(m_dx, m_dx, M);
+
+   DenseMatrix f(2, num_dofs2); f = 0.0;
+
+   for (int d=0; d<3; d++)
+   {
+      DenseMatrix Mtemp(2,2); Mtemp = 0.0;
+      Mtemp(0,0) = m_dx2(0,d); Mtemp(0,1) = m_dx2(1,d);
+      Mtemp(1,0) = m_dx2(1,d); Mtemp(1,1) = m_dx2(2,d);
+
+      M.Add(-gap_v[d], Mtemp);
+
+      Vector m_dxcol(2); m_dx.GetColumn(d, m_dxcol);
+      DenseMatrix ftmp(2,4);
+      outer(m_dxcol, m_N, ftmp);
+      ftmp *= -1;
+      ftmp.Add( gap_v[d], m_dN);   // 2*4
+
+      for (int j=0; j<4; j++)
+      {
+         assert(d+3*j<num_dofs2);
+         f(0,d+j*3) = ftmp(0,j);
+         f(1,d+j*3) = ftmp(1,j);
+      }
+   }
+   //fprintf('hess dxidxm\n');
+   DenseMatrixInverse Minv(M);
+   DenseMatrix dxidxm(2,num_dofs2); dxidxm = 0.0;
+   Minv.Mult(f, dxidxm);
+   //LinearSolve??
+   //dxidxm = M\f;
+
+   DenseMatrix nde2(2,2); nde2 = 0.0;
+   DenseMatrix Nndx2(2,num_dofs2); Nndx2 = 0.0;
+
+   for (int d=0; d<3; d++)
+   {
+      DenseMatrix ndetmp(2,2); ndetmp = 0.0;
+      ndetmp(0,0) = normal(d)*m_dx2(0,d); ndetmp(0,1) = normal(d)*m_dx2(1,d);
+      ndetmp(1,0) = normal(d)*m_dx2(1,d); ndetmp(1,1) = normal(d)*m_dx2(2,d);
+
+      nde2 += ndetmp;
+
+      for (int j=0; j<4; j++)
+      {
+         assert(d+3*j<num_dofs2);
+         Nndx2(0,d+j*3) = normal[d]*m_dN(0,j);
+         Nndx2(1,d+j*3) = normal[d]*m_dN(1,j);
+      }
+
+   }
+
+   DenseMatrix Ndn(2,num_dofs2); Ndn = 0.0;
+   Ndn += Nndx2;
+   AddMult(nde2, dxidxm, Ndn);
+
+
+   DenseMatrix M2(2,2); M2 = 0.0;
+   MultABt(m_dx, m_dx, M2);
+   DenseMatrixInverse M2inv(M2);
+   DenseMatrix diag2(2,2); diag2(0,0) = 1.0; diag2(1,1) = 1.0;
+   DenseMatrix m_con(2,2); m_con = 0.0;
+
+   M2inv.Mult(diag2, m_con);
+
+   DenseMatrix dg2dxm(num_dofs2, num_dofs2); dg2dxm = 0.0;
+
+   DenseMatrix dg2dxm_tmp(num_dofs2,2); dg2dxm_tmp = 0.0;
+   MultAtB(Ndn, m_con, dg2dxm_tmp);
+   Mult(dg2dxm_tmp, Ndn, dg2dxm);
+   dg2dxm *= gap;
+
+   DenseMatrix dg2dxm_tmp2(num_dofs2,num_dofs2); dg2dxm_tmp2 = 0.0;
+   MultAtB(Nndx2, dxidxm, dg2dxm_tmp2);
+   dg2dxm.Add(-1.0, dg2dxm_tmp2);
+
+   dg2dxm_tmp = 0.0;
+   MultAtB(dxidxm, nde2, dg2dxm_tmp);
+
+   AddMult_a(-1.0, dg2dxm_tmp, dxidxm, dg2dxm);
+
+   dg2dxm_tmp2 = 0.0;
+   MultAtB(dxidxm, Nndx2, dg2dxm_tmp2);
+   dg2dxm.Add(-1.0, dg2dxm_tmp2);
+
+   Vector v_dxidx2(4);
+   m_coords.Mult(gap_v, v_dxidx2); // m_coords * gap_v;  // 4*3 * 3 = 4
+
+   DenseMatrix K_dxidx2(2,2); K_dxidx2 = 0.0;
+
+   Vector m_dN2row1(4); m_dN2.GetRow(0, m_dN2row1);
+   Vector m_dN2row2(4); m_dN2.GetRow(1, m_dN2row2);
+   Vector m_dN2row3(4); m_dN2.GetRow(2, m_dN2row3);
+   K_dxidx2(0,0) = m_dN2row1 * v_dxidx2; // how would 4*1 * 1*4 be computed?
+   K_dxidx2(0,1) = m_dN2row2 * v_dxidx2;
+   K_dxidx2(1,0) = m_dN2row2 * v_dxidx2;
+   K_dxidx2(1,1) = m_dN2row3 * v_dxidx2;
+
+   DenseMatrix K_dxidx(2,2);
+   K_dxidx -= M2;
+   K_dxidx += K_dxidx2;
+
+   Vector drdxs_r(6);
+   drdxs_r[0] = m_dx(0,0); drdxs_r[1] = m_dx(0,1); drdxs_r[2] = m_dx(0,2);
+   drdxs_r[3] = m_dx(1,0); drdxs_r[4] = m_dx(1,1); drdxs_r[5] = m_dx(1,2);
+
+   DenseMatrix drdxs_K(6,6); drdxs_K = 0.;
+   for (int i=0; i<3; i++)
+   {
+      drdxs_K(i,i) = K_dxidx(0,0);
+      drdxs_K(i,3+i) = K_dxidx(0,1);
+      drdxs_K(i+3,i) = K_dxidx(1,0);
+      drdxs_K(i+3,i+3) = K_dxidx(1,1);
+   }
+   Vector dxidxs(6);
+
+   DenseMatrixInverse drdxsK_inv(drdxs_K);
+   drdxsK_inv.Mult(drdxs_r,dxidxs);
+   dxidxs *= -1.0;
+   //dxidxs = -drdxs_K\drdxs_r;
+
+   DenseMatrix dxidxs_m(2,3); dxidxs_m = 0.0;
+   dxidxs_m(0,0) = dxidxs[0]; dxidxs_m(0,1) = dxidxs[1]; dxidxs_m(0,2) = dxidxs[2];
+   dxidxs_m(1,0) = dxidxs[3]; dxidxs_m(1,1) = dxidxs[4]; dxidxs_m(1,2) = dxidxs[5];
+
+   DenseMatrix dtao1dxs(3,3); dtao1dxs = 0.0;
+   DenseMatrix dtao2dxs(3,3); dtao2dxs = 0.0;
+
+   Vector dxidxs_row1(3); dxidxs_row1 = 0.0; Vector dxidxs_row2(3);
+   dxidxs_row2 = 0.0;
+   Vector mdx2_row1(3); mdx2_row1 = 0.0; Vector mdx2_row2(3); mdx2_row2 = 0.0;
+   Vector mdx2_row3(3); mdx2_row3 = 0.0;
+   dxidxs_m.GetRow(0,dxidxs_row1);
+   dxidxs_m.GetRow(1,dxidxs_row2);
+   m_dx2.GetRow(0,mdx2_row1);
+   m_dx2.GetRow(1,mdx2_row2);
+   m_dx2.GetRow(2,mdx2_row3);
+
+   DenseMatrix dtaotmp(3,3); dtaotmp = 0.0;
+   outer(mdx2_row1, dxidxs_row1,dtaotmp);
+   dtao1dxs += dtaotmp; dtaotmp = 0.0;
+   outer(mdx2_row2, dxidxs_row1,dtaotmp);
+   dtao1dxs += dtaotmp; dtaotmp = 0.0;
+
+   outer(mdx2_row2, dxidxs_row2, dtaotmp);
+   dtao2dxs += dtaotmp; dtaotmp = 0.0;
+   outer(mdx2_row3, dxidxs_row2, dtaotmp);
+   dtao2dxs += dtaotmp; dtaotmp = 0.0;
+
+   DenseMatrix dtaodxs(3,3); dtaodxs = 0.0; //tao = tao1 cross tao2
+
+   for (int d=0; d<3; d++)
+   {
+      Vector dtao1dxs_tmp(3); dtao1dxs_tmp = 0.0;
+      dtao1dxs.GetColumn(d,dtao1dxs_tmp);
+      Vector m_dxrow(3); m_dx.GetRow(1, m_dxrow);
+
+      Vector dtaodxs_tmp(3); dtaodxs_tmp = 0.0;
+      cross(dtao1dxs_tmp, m_dxrow, dtaodxs_tmp);
+
+      Vector dtaodxs_tmp2(3); dtaodxs_tmp2 = 0.0;
+      m_dx.GetRow(0, m_dxrow);
+      dtao1dxs_tmp = 0.0;  // reuse the same vector for dtao2
+      dtao2dxs.GetColumn(d,dtao1dxs_tmp);
+      cross(m_dxrow, dtao1dxs_tmp, dtaodxs_tmp2);
+
+      dtaodxs_tmp2 += dtaodxs_tmp;
+      dtaodxs.SetCol(d, dtaodxs_tmp2);
+   }
+
+   DenseMatrix dndxs(3,3); dndxs = 0.0; dndxs += dtaodxs; dndxs *= 1.0/nnorm;
+   DenseMatrix dndxs_tmp(3,3); dndxs_tmp = 0.0;
+   outer(normal, normal, dndxs_tmp);
+   AddMult_a(-1/nnorm, dndxs_tmp, dtaodxs, dndxs);
+
+   DenseMatrix dgvdxs(3,3); dgvdxs = 0.0;
+   MultAtB(m_dx, dxidxs_m, dgvdxs);
+   dgvdxs *= -1;
+   for (int d=0; d<3; d++)
+   {
+      dgvdxs(d,d) += 1.0;
+   }
+   //dxidxs: 2*3
+
+   DenseMatrix dg2dxs(3,3); dg2dxs = 0.0;
+   DenseMatrix dg2dxs_tmp(3,2); dg2dxs_tmp = 0.0;
+   MultAtB(dxidxs_m, nde2, dg2dxs_tmp);
+   AddMult_a(-1.0, dg2dxs_tmp, dxidxs_m, dg2dxs);
+   DenseMatrix dg2dxs_tmp2(3,3); dg2dxs_tmp2 = 0.0;
+   MultAtB(dgvdxs, dndxs, dg2dxs_tmp2);
+   dg2dxs += dg2dxs_tmp2;
+   dg2dxs_tmp2 = 0.0;
+   MultAtB(dndxs, dndxs_tmp, dg2dxs_tmp2);
+   AddMult(dg2dxs_tmp2, dgvdxs, dg2dxs);
+
+   DenseMatrix Ne(3,12), Be(6,12), dBe(12,12);
+   BasisVectorDerivs(xi, Ne, Be, dBe);
+
+   DenseMatrix dtao1dxm(3,12); dtao1dxm.CopyRows(Be, 0, 2);
+   DenseMatrix dtao2dxm(3,12); dtao2dxm.CopyRows(Be, 3, 5);
+
+   Vector m_coords_v(12);
+   for (int i=0; i<4; i++)
+   {
+      for (int j=0; j<3; j++)
+      {
+         m_coords_v[i*3+j] = m_coords(i,j);
+      }
+   }
+
+   for (int i=0; i<2; i++)
+   {
+      Vector dxidxm_tmp(num_dofs2); dxidxm_tmp = 0.0;
+      dxidxm.GetRow(i,dxidxm_tmp);
+
+      DenseMatrix dBe_tmp(3,12);
+      dBe_tmp.CopyRows(dBe,i*3,(i+1)*3-1);
+
+      DenseMatrix dtaodxm_tmp(12,12); dtaodxm_tmp = 0.0;
+      outer(m_coords_v, dxidxm_tmp, dtaodxm_tmp);
+      AddMult(dBe_tmp, dtaodxm_tmp, dtao1dxm);
+
+      //dtao1dxm += dBe(:,:,i)*reshape(m_coords(1:4,:)',12,1)*reshape(dxidxm(i,:),1,12); % 3*12
+      dBe_tmp = 0.0;
+      dBe_tmp.CopyRows(dBe,(i+2)*3,(i+3)*3-1);
+      AddMult(dBe_tmp, dtaodxm_tmp, dtao2dxm);
+
+   }
+
+   DenseMatrix dtaodxm(3,12); dtaodxm = 0.0;//tao = tao1 cross tao2
+
+   for (int d=0; d<12; d++)
+   {
+      Vector dtaodxm_tmp(3); dtaodxm_tmp = 0.0;
+      Vector dtaodxm_tmp2(3); dtaodxm_tmp2 = 0.0;
+      Vector tmp1(3); tmp1 = 0.0;  dtao1dxm.GetColumn(d,tmp1);
+      Vector m_dxrow2(3); m_dx.GetRow(1, m_dxrow2);
+      Vector m_dxrow1(3); m_dx.GetRow(0, m_dxrow1);
+      Vector tmp2(3); tmp2 = 0.0;  dtao2dxm.GetColumn(d,tmp2);
+
+      cross(tmp1, m_dxrow2, dtaodxm_tmp);
+      cross(m_dxrow1,tmp2, dtaodxm_tmp2);
+      dtaodxm_tmp += dtaodxm_tmp2;
+
+      dtaodxm.SetCol(d, dtaodxm_tmp);
+   }
+
+   DenseMatrix dndxm(3,12); dndxm = 0.0;
+   dndxm += dtaodxm;
+   dndxm *= 1.0/nnorm;
+   AddMult_a(-1/nnorm, dndxs_tmp, dtaodxm, dndxm); //dndxs_tmp = normal'*normal
+
+   DenseMatrix dgvdxm(3,12); dgvdxm = 0.0;
+   dgvdxm -= Ne;
+
+   for (int i=0; i<2; i++)
+   {
+      Vector dxidxm_tmp(num_dofs2); dxidxm_tmp = 0.0;
+      dxidxm.GetRow(i,dxidxm_tmp);
+
+      DenseMatrix Be_tmp(3,12);
+      Be_tmp.CopyRows(Be,i*3,(i+1)*3-1);
+
+      DenseMatrix dgvdxm_tmp(12,12); dgvdxm_tmp = 0.0;
+      outer(m_coords_v, dxidxm_tmp, dgvdxm_tmp);
+      AddMult_a(-1.0, Be_tmp, dgvdxm_tmp, dgvdxm);
+
+   }
+
+   DenseMatrix dg2dxsxm(3,12); dg2dxsxm = 0.0;
+   DenseMatrix dg2dxsxm_tmp(3,3); dg2dxsxm_tmp = 0.0;
+   MultAtB(dgvdxs, dndxm, dg2dxsxm);
+
+   MultAtB(dndxs, dndxs_tmp, dg2dxsxm_tmp);
+   AddMult(dg2dxsxm_tmp, dgvdxm, dg2dxsxm); // += dndxs'*normal'*normal*dgvdxm;
+
+   DenseMatrix dgvdxsxmn(3,12); dgvdxsxmn = 0.0;
+   DenseMatrix dgvdxsxmn_tmp(3,2); dgvdxsxmn_tmp = 0.0;
+   MultAtB(dxidxs_m, nde2, dgvdxsxmn_tmp);   //dxidxs_m: 2*3
+
+   AddMult_a(-1.0, dgvdxsxmn_tmp, dxidxm, dgvdxsxmn);
+
+
+   for (int i =0; i<2; i++)
+   {
+      DenseMatrix Be_tmp(3,12);
+      Be_tmp.CopyRows(Be,i*3,(i+1)*3-1);
+
+      Vector dxidxs_row(3); dxidxs_row = 0.0; dxidxs_m.GetRow(i,dxidxs_row);
+      DenseMatrix dgvdxsxmn_tmp2(3,3); dgvdxsxmn_tmp2 = 0.0;
+      outer(dxidxs_row, normal, dgvdxsxmn_tmp2);
+      AddMult_a(-1.0, dgvdxsxmn_tmp2, Be_tmp, dgvdxsxmn);
+   }
+
+   dg2dxsxm += dgvdxsxmn;
+
+   DenseMatrix dg2dxmxs(12,3); dg2dxmxs = 0.0;
+   DenseMatrix dg2dxmxs_tmp(12,3); dg2dxmxs_tmp = 0.0;
+   MultAtB(dgvdxm, dndxs, dg2dxmxs);
+   MultAtB(dndxm, dndxs_tmp, dg2dxmxs_tmp);
+   AddMult(dg2dxmxs_tmp, dgvdxs, dg2dxmxs);
+
+   DenseMatrix dgvdxmxsn(12,3); dgvdxmxsn = 0.0;
+   DenseMatrix dgvdxmxsn_tmp(12,2); dgvdxmxsn_tmp = 0.0;
+
+   MultAtB(dxidxm, nde2, dgvdxmxsn_tmp);
+   dgvdxmxsn_tmp *= -1.0;
+   AddMult(dgvdxmxsn_tmp, dxidxs_m, dgvdxmxsn);
+
+   for (int i =0; i<2; i++)
+   {
+      DenseMatrix Be_tmp(3,12);
+      Be_tmp.CopyRows(Be,i*3,(i+1)*3-1);
+      Be_tmp.Transpose(); // Be is now 12*3
+
+      Vector dxidxs_row(3); dxidxs_row = 0.0; dxidxs_m.GetRow(i,dxidxs_row);
+      DenseMatrix dgvdxmxsn_tmp2(3,3); dgvdxmxsn_tmp2 = 0.0;
+      outer(normal, dxidxs_row, dgvdxmxsn_tmp2);
+      AddMult_a(-1.0, Be_tmp, dgvdxmxsn_tmp2, dgvdxmxsn);
+
+   }
+
+   dg2dxmxs += dgvdxmxsn;
+
+   dg2dx.CopyMN(dg2dxs, 0, 0);
+   dg2dx.CopyMN(dg2dxm, 3, 3);
+   dg2dx.CopyMN(dg2dxsxm, 0, 3);
+   dg2dx.CopyMN(dg2dxmxs, 3, 0);
+
+};
+
+
+
+void NodeSegConPairs(const Vector x1, const Vector xi2,
+                     const DenseMatrix coords2,
+                     double& node_g, Vector& node_dg, DenseMatrix& node_dg2)
+{
+   double gap = 0.0;
+   Vector normal(3); normal = 0.0;
+   Vector dgdxm(12); dgdxm = 0.0;
+   Vector dgdxs(3); dgdxs = 0.0;
+
+   ComputeGapJacobian(x1, xi2, coords2, gap, normal, dgdxm, dgdxs);
+   node_g = gap;
+
+   node_dg.SetSize(12+3);
+   for (int i=0; i<3; i++) { node_dg[i] = dgdxs[i]; }
+   for (int i=0; i<12; i++) { node_dg[i+3] = dgdxm[i]; }
+
+   DenseMatrix dg2dx(15,15); dg2dx = 0.0;
+   DenseMatrix dgvdxmxsn(12,3); dgvdxmxsn = 0.0;
+   ComputeGapHessian(x1, xi2, coords2, dg2dx);
+
+   node_dg2.SetSize(15,15);
+   node_dg2 = dg2dx;
+
+   /*
+   if(obj.space1.conns{e1}(i)==150)  % for debugging purpose
+
+   v1 = 1:3;
+   v2 = 1:12;
+   %v1 = ones(1,3)
+   %v2 = ones(1,12)
+   v2 = reshape(v2,4,3);
+   x1n1 = x1 + 0.01*v1;
+   coords2n1 = coords2 + 0.001*v2;
+      [xi2n1, gapv1, ~, ~] = SlaveToMaster(obj, coords2n1, x1n1);
+      [gapn1, n1,dgdxmn1, dgdxsn1] = ComputeGapJacobian(obj, x1n1, xi2n1, coords2n1);
+   x1n2 = x1 - 0.01*v1;
+   coords2n2 = coords2 - 0.001*v2;
+      [xi2n2, gapv2, ~, ~] = SlaveToMaster(obj, coords2n2, x1n2);
+      [gapn2, n2,dgdxmn2, dgdxsn2] = ComputeGapJacobian(obj, x1n2, xi2n2, coords2n2);
+   fprintf('fd\n');
+      %gapv1-gapv2
+   [dgdxsn1(:)',dgdxmn1(:)'] - [dgdxsn2(:)',dgdxmn2(:)']
+
+   %dgdxsn1-dgdxsn2
+   fprintf('code\n');
+   v2n = v2';
+   %dg2dx(1:3,1:3)*0.04*ones(3,1)
+   temp = zeros(12,3);
+   for i = 1:4
+   temp1 = dg2dx(3+(i-1)*3+1:3+i*3,1:3);
+   temp((i-1)*3+1:i*3,:) = temp1';
+   end
+   temp2 = zeros(3,12);
+   for i = 1:4
+   temp3 = dg2dx(1:3,3+(i-1)*3+1:3+i*3);
+   temp2(:,(i-1)*3+1:i*3) = temp3';
+   end
+   %dg2dx
+   %dg2dx(4:end,1:3) = temp;
+   %dg2dx(1:3,4:end) = temp2;
+   %dgvdxm * 0.002*v2n(:)
+   (dg2dx*[0.02*v1(:)',0.002*v2n(:)']')'
+   %dg2dx(4:end,1:3)
+   end*/
+
+};
+
+
+// coordsm : (npoints*4, 3) use what class?
+// m_conn: (npoints*4)
+void Assemble_Contact(const int m, const int npoints, const int ndofs,
+                      const Vector x_s,
+                      const Vector xi, const DenseMatrix coordsm, const Array<int> s_conn,
+                      const Array<int> m_conn, Vector& g, SparseMatrix& M,
+                      std::vector<SparseMatrix>& dM)
+{
+   int ndim = 3;
+
+   g.SetSize(m);
+   g = 0.0;
+
+   //SparseMatrix M(m, n);  // M needs to be the correct size
+
+   //dM.resize(m); // needs to clear?
+
+   double g_tmp = 0.;
+   Vector dg(4*ndim+ndim);
+   dg = 0.;
+   DenseMatrix dg2(4*ndim+ndim,4*ndim+ndim);
+   dg2 = 0.;
+
+   for (int i=0; i<npoints; i++)
+   {
+      Vector x1(ndim);
+      x1[0] = x_s[i*ndim];
+      x1[1] = x_s[i*ndim+1];
+      x1[2] = x_s[i*ndim+2];
+
+      Vector xi2(ndim-1);
+      xi2[0] = xi[i*(ndim-1)];
+      xi2[1] = xi[i*(ndim-1)+1];
+
+      DenseMatrix coords2(4,3);
+      coords2.CopyRows(coordsm, i*4,(i+1)*4-1);
+
+      //how to get coords2?
+      dg = 0.0;
+      dg2 = 0.;
+      NodeSegConPairs(x1, xi2, coords2, g_tmp, dg, dg2);
+      g[s_conn[i]] = g_tmp; // should be unique
+      Array<int> m_conn_i(4);
+      m_conn.GetSubArray(4*i, 4, m_conn_i);
+
+      Array<int> node_conn(5);
+      node_conn[0] = s_conn[i];
+      for (int j=0; j<4; j++)
+      {
+         node_conn[j+1] = m_conn_i[j];
+      }
+
+      Array<int>  M_i_tmp(1);
+      M_i_tmp[0] = s_conn[i];
+
+      //j_idx = (node_conn-1)*obj.disp_field.num_components +repmat((1:obj.disp_field.num_components)', 1, length(node_conn{i}));
+      Array<int> j_idx(5*ndim); j_idx = 0;
+      for (int j=0; j< 5; j++)
+      {
+         for (int k=0; k<ndim; k++)
+         {
+            j_idx[j*ndim+k] = node_conn[j]*ndim+k;
+         }
+      }
+      DenseMatrix M_v_tmp(1, ndim*(4+1));   // SetData now?
+      M_v_tmp.SetRow(0, dg);
+
+      M.AddSubMatrix(M_i_tmp, j_idx, M_v_tmp);
+
+      Array<int> dM_i(ndim*(4+1));
+      Array<int> dM_j(ndim*(4+1));
+
+      for (int j=0; j< ndim*(4+1); j++)
+      {
+         dM_i[j] = j_idx[j];
+         dM_j[j] = j_idx[j];
+      }
+      dM[s_conn[i]].AddSubMatrix(dM_i,dM_j, dg2);
+      dM[s_conn[i]].Finalize();
+      dM[s_conn[i]].Threshold(0.0);
+      dM[s_conn[i]].SortColumnIndices();
+   }
+   M.Finalize();
+   M.Threshold(0.0);
+   M.SortColumnIndices();
+};
+

examples/contact/problems.hpp

@@ -0,0 +1,396 @@
+#include "mfem.hpp"
+#include <fstream>
+#include <iostream>
+#include <set>
+using namespace std;
+using namespace mfem;
+
+#ifndef PROBLEM_DEFS
+#define PROBLEM_DEFS
+
+
+
+// abstract OptProblem class
+// of the form
+// min_(u,m) f(u,m) s.t. c(u,m)=0 and m>=ml
+// the primal variable (u, m) is represented as a BlockVector
+
+class OptProblem
+{
+protected:
+    int dimU, dimM, dimC;
+    Array<int> block_offsetsx;
+    Vector ml;
+public:
+    OptProblem();
+    virtual double CalcObjective(const BlockVector &) const = 0;
+    virtual void Duf(const BlockVector &, Vector &) const = 0;
+    virtual void Dmf(const BlockVector &, Vector &) const = 0;
+    void CalcObjectiveGrad(const BlockVector &, BlockVector &) const;
+    virtual SparseMatrix* Duuf(const BlockVector &) = 0;
+    virtual SparseMatrix* Dumf(const BlockVector &) = 0;
+    virtual SparseMatrix* Dmuf(const BlockVector &) = 0;
+    virtual SparseMatrix* Dmmf(const BlockVector &) = 0;
+    virtual void c(const BlockVector &, Vector &) const = 0;
+    virtual SparseMatrix* Duc(const BlockVector &) = 0;
+    virtual SparseMatrix* Dmc(const BlockVector &) = 0;
+    // TO DO: include Hessian terms of constraint c
+    // TO DO: include log-barrier lumped-mass and pass that
+    // to the optimizer
+    //virtual SparseMatrix* GetLogBarrierLumpedMass() = 0;
+    int GetDimU() const { return dimU; };
+    int GetDimM() const { return dimM; }; 
+    int GetDimC() const { return dimC; };
+    Vector Getml() const { return ml; };
+    ~OptProblem();
+};
+
+
+// abstract ContactProblem class
+// of the form
+// min_d e(d) s.t. g(d) >= 0
+// TO DO: add functionality for gap function Hessian apply 
+class ContactProblem : public OptProblem
+{
+protected:
+    int dimD;
+    int dimS;
+    Array<int> block_offsetsx;
+public:
+    //ContactProblem(int, int);        // constructor
+    ContactProblem();
+    void InitializeParentData(int, int);
+    double CalcObjective(const BlockVector &) const; // objective e
+    void Duf(const BlockVector &, Vector &) const;
+    void Dmf(const BlockVector &, Vector &) const;
+    SparseMatrix* Duuf(const BlockVector &);
+    SparseMatrix* Dumf(const BlockVector &);
+    SparseMatrix* Dmuf(const BlockVector &);
+    SparseMatrix* Dmmf(const BlockVector &);
+    void c(const BlockVector &, Vector &) const;
+    SparseMatrix* Duc(const BlockVector &);
+    SparseMatrix* Dmc(const BlockVector &);
+    virtual double E(const Vector &) const = 0;           // objective e(d) (energy function)
+    virtual void DdE(const Vector &, Vector &) const = 0; // gradient of objective De / Dd
+    virtual SparseMatrix* DddE(const Vector &) = 0;       // Hessian of objective D^2 e / D d^2
+    virtual void g(const Vector &, Vector &) const = 0;   // inequality constraint g(d) >= 0 (gap function)
+    virtual SparseMatrix* Ddg(const Vector &) = 0;        // Jacobian of inequality constraint Dg / Dd
+    int GetDimD() const { return dimD; };
+    int GetDimS() const { return dimS; };
+    virtual ~ContactProblem();
+};
+
+
+class ObstacleProblem : public ContactProblem
+{
+protected:
+   // data to define energy objective function e(d) = 0.5 d^T K d - f^T d, g(d) = d >= 0
+   // stiffness matrix used to define objective
+   BilinearForm *Kform;
+   LinearForm   *fform;
+   Array<int> empty_tdof_list; // needed for calls to FormSystemMatrix
+   SparseMatrix  K;
+   SparseMatrix  *J;
+   FiniteElementSpace *Vh;
+   Vector f;
+public : 
+   ObstacleProblem(FiniteElementSpace* , double (*fSource)(const Vector &));
+   double E(const Vector &) const;
+   void DdE(const Vector &, Vector &) const;
+   SparseMatrix* DddE(const Vector &);
+   void g(const Vector &, Vector &) const;
+   SparseMatrix* Ddg(const Vector &);
+   // TO DO: include lumped-mass for the log-barrier term
+   //SparseMatrix* GetLogBarrierLumpedMass();
+   virtual ~ObstacleProblem();
+};
+
+class DirichletObstacleProblem : public ContactProblem
+{
+protected:
+   // data to define energy objective function e(d) = 0.5 d^T K d - f^T d, g(d) = d + \psi >= 0
+   // stiffness matrix used to define objective
+   BilinearForm *Kform;
+   LinearForm   *fform;
+   Array<int> ess_tdof_list; // needed for calls to FormSystemMatrix
+   SparseMatrix  *K;
+   SparseMatrix  *J;
+   FiniteElementSpace *Vh;
+   Vector f;
+   Vector psi;
+   Vector xDC;
+public : 
+   DirichletObstacleProblem(FiniteElementSpace*, Vector&,  double (*fSource)(const Vector &), double (*obstacleSource)(const Vector &), Array<int> tdof_list, bool);
+   double E(const Vector &) const;
+   void DdE(const Vector &, Vector &) const;
+   SparseMatrix* DddE(const Vector &);
+   void g(const Vector &, Vector &) const;
+   SparseMatrix* Ddg(const Vector &);
+   virtual ~DirichletObstacleProblem();
+};
+
+
+// abstract out technology for removing null rows of the Jacobian from an existing contact problem
+class ReducedContactProblem : public ContactProblem
+{
+protected:
+   Array<int> activeConstraints;
+   Array<int> fixedDofs;
+   ContactProblem * contact;
+   int dimSin;
+public:
+   ReducedContactProblem(ContactProblem * contact, Array<int> activeConstraints, Array<int> fixedDofs);
+   double E(const Vector &) const;
+   void DdE(const Vector &, Vector &) const;
+   SparseMatrix* DddE(const Vector &);
+   void g(const Vector &, Vector &) const;
+   SparseMatrix* Ddg(const Vector &);
+   virtual ~ReducedContactProblem();
+};
+
+
+class QPContactProblem : public ContactProblem
+{
+protected:
+  SparseMatrix *K;
+  SparseMatrix *J;
+  Vector f;
+  Vector g0;
+public:
+  QPContactProblem(const SparseMatrix, const SparseMatrix, const Vector, const Vector);
+  double E(const Vector &) const;
+  void DdE(const Vector &, Vector &) const;
+  SparseMatrix* DddE(const Vector &);
+  void g(const Vector &, Vector &) const;
+  SparseMatrix* Ddg(const Vector &);
+  virtual ~QPContactProblem();
+};
+
+
+typedef int Index;
+typedef double Number;
+
+class ExContactBlockTL : public ContactProblem
+{
+public:
+   double E(const Vector &) const;
+   void DdE(const Vector &, Vector &) const;
+   SparseMatrix* DddE(const Vector &);
+   void g(const Vector &, Vector &) const;
+   SparseMatrix* Ddg(const Vector &);
+   FiniteElementSpace GetVh1();
+   FiniteElementSpace GetVh2();
+
+public:
+   /** default constructor */
+   ExContactBlockTL(int );
+   
+
+   /** default destructor */
+   virtual ~ExContactBlockTL();
+
+   ///**@name Overloaded from TNLP */
+   ///** Method to return some info about the nlp */
+   //virtual bool get_nlp_info(
+   //   Index&          n,
+   //   Index&          m,
+   //   Index&          nnz_jac_g,
+   //   Index&          nnz_h_lag,
+   //   IndexStyleEnum& index_style
+   //);
+
+   ///** Method to return the bounds for my problem */
+   //virtual bool get_bounds_info(
+   //   Index   n,
+   //   Number* x_l,
+   //   Number* x_u,
+   //   Index   m,
+   //   Number* g_l,
+   //   Number* g_u
+   //);
+
+   ///** Method to return the starting point for the algorithm */
+   //virtual bool get_starting_point(
+   //   Index   n,
+   //   bool    init_x,
+   //   Number* x,
+   //   bool    init_z,
+   //   Number* z_L,
+   //   Number* z_U,
+   //   Index   m,
+   //   bool    init_lambda,
+   //   Number* lambda
+   //);
+
+   /* Method to return the objective value */
+   virtual bool eval_f(
+      Index         n,
+      const Number* x,
+      bool          new_x,
+      Number&       obj_value
+   ) const;
+
+   /* Method to return the gradient of the objective */
+   virtual bool eval_grad_f(
+      Index         n,
+      const Number* x,
+      bool          new_x,
+      Number*       grad_f
+   ) const;
+
+   /* Method to return the constraint residuals */
+   virtual bool eval_g(
+      Index         n,
+      const Number* x,
+      bool          new_x,
+      Index         m,
+      Number*       cons
+   ) const;
+
+   /* Method to return:
+      1) The structure of the Jacobian (if "values" is NULL)
+      2) The values of the Jacobian (if "values" is not NULL)
+   */
+   virtual bool eval_jac_g(
+      Index         n,
+      const Number* x,
+      bool          new_x,
+      Index         m,
+      Index         nele_jac,
+      Index*        iRow,
+      Index*        jCol,
+      Number*       values
+   ) const;
+
+   /* Method to return:
+    *   1) The structure of the Hessian of the Lagrangian (if "values" is NULL)
+    *   2) The values of the Hessian of the Lagrangian (if "values" is not NULL)
+   */
+   virtual bool eval_h(
+      Index         n,
+      const Number* x,
+      bool          new_x,
+      Number        obj_factor,
+      Index         m,
+      const Number* lambda,
+      bool          new_lambda,
+      Index         nele_hess,
+      Index*        iRow,
+      Index*        jCol,
+      Number*       values
+   );
+
+   ///** This method is called when the algorithm is complete so the TNLP can store/write the solution */
+   //virtual void finalize_solution(
+   //   SolverReturn               status,
+   //   Index                      n,
+   //   const Number*              x,
+   //   const Number*              z_L,
+   //   const Number*              z_U,
+   //   Index                      m,
+   //   const Number*              g,
+   //   const Number*              lambda,
+   //   Number                     obj_value,
+   //   const IpoptData*           ip_data,
+   //   IpoptCalculatedQuantities* ip_cq
+   //);
+
+private:
+   void update_g() const;
+   void update_jac();
+   void update_hess();
+
+private:
+   /**@name Methods to block default compiler methods.
+    *
+    * The compiler automatically generates the following three methods.
+    *  Since the default compiler implementation is generally not what
+    *  you want (for all but the most simple classes), we usually
+    *  put the declarations of these methods in the private section
+    *  and never implement them. This prevents the compiler from
+    *  implementing an incorrect "default" behavior without us
+    *  knowing. (See Scott Meyers book, "Effective C++")
+    */
+   ExContactBlockTL(
+      const ExContactBlockTL&
+   );
+
+   ExContactBlockTL& operator=(
+      const ExContactBlockTL&
+   );
+
+   Array<int> attr;
+   Array<int> m_attr;
+   Array<int> s_conn; // connectivity of the second/slave mesh
+   std::string mesh_file1;
+   std::string mesh_file2;
+   Mesh* mesh1;
+   Mesh* mesh2;
+   FiniteElementCollection* fec1;
+   FiniteElementCollection* fec2;
+   FiniteElementSpace* fespace1;
+   FiniteElementSpace* fespace2;
+   Array<int> ess_tdof_list1;
+   Array<int> ess_tdof_list2;
+   GridFunction nodes0;
+   GridFunction* nodes1;
+   GridFunction* nodes2;
+   mutable GridFunction* x1;
+   mutable GridFunction* x2;
+   LinearForm* b1;
+   LinearForm* b2;
+   PWConstCoefficient* lambda1_func;
+   PWConstCoefficient* lambda2_func;
+   PWConstCoefficient* mu1_func;
+   PWConstCoefficient* mu2_func;
+   BilinearForm* a1;
+   BilinearForm* a2;
+
+   mfem::Vector lambda1;
+   mfem::Vector lambda2;
+   mfem::Vector mu1;
+   mfem::Vector mu2;
+   mutable mfem::Vector xyz;
+
+   std::set<int> bdryVerts2;
+
+   int dim;
+   // degrees of freedom of both meshes
+   int ndof_1;
+   int ndof_2;
+   int ndofs;
+   // number of nodes for each mesh
+   int nnd_1;
+   int nnd_2;
+   int nnd;
+
+   int npoints;
+
+   SparseMatrix A1;
+   mfem::Vector B1, X1;
+   SparseMatrix A2;
+   mfem::Vector B2, X2;
+
+   SparseMatrix* K;
+   mutable mfem::Vector gapv;
+   mutable mfem::Vector m_xi;
+   mutable mfem::Vector xs;
+
+   mutable Array<int> m_conn; // only works for linear elements that have 4 vertices!
+   mutable DenseMatrix* coordsm;
+   mutable SparseMatrix* M;
+
+   mutable std::vector<SparseMatrix>* dM;
+
+   Array<int> Dirichlet_dof;
+   Array<double> Dirichlet_val;
+
+public: 
+   Mesh * GetMesh1() {return mesh1;}
+   Mesh * GetMesh2() {return mesh2;}
+   Array<int> GetDirichletDofs() {return Dirichlet_dof;}
+   Array<double> GetDirichletVals() {return Dirichlet_val;}
+
+};
+
+#endif

fem/gslib.cpp

@@ -1308,6 +1308,298 @@ void OversetFindPointsGSLIB::Interpolate(const Vector &point_pos,
    Interpolate(field_in, field_out);
 }
 
+#ifdef MFEM_USE_MPI
+
+GSLIBCommunicator::GSLIBCommunicator(MPI_Comm comm_)
+   : cr(NULL), gsl_comm(NULL)
+{
+   gsl_comm = new gslib::comm;
+   cr      = new gslib::crystal;
+   comm_init(gsl_comm, comm_);
+   crystal_init(cr, gsl_comm);
+}
+
+void GSLIBCommunicator::SendData(int dim, const Array<unsigned int> & gsl_proc,
+                                 const Array<unsigned int> & elem_send,
+                                 const Vector &ref_send,
+                                 const Vector &coords_send,
+                                 const Array<int> &s_conn_send,
+                                 Array<unsigned int> & proc_recv,
+                                 Array<unsigned int> & index_recv,
+                                 Array<unsigned int> & elem_recv,
+                                 Vector &ref_recv,
+                                 Vector &coords_recv,
+                                 Array<int> &s_conn_recv)
+{
+   int nptsend = gsl_proc.Size();
+   int nptElem   = elem_send.Size();
+   int nptRST    = ref_send.Size();
+
+   MFEM_VERIFY(nptElem == nptsend,
+               "Incompatible Elem size.");
+   MFEM_VERIFY(nptsend*dim == nptRST,
+               "Incompatible nptRST size.");
+   MFEM_VERIFY(dim <= 3,
+               "Incompatible dimension.");
+
+   // Pack data to send via crystal router
+   struct gslib::array *outpt = new gslib::array;
+
+   struct out_pt { double rst[3], coords[3]; int s_conn; uint index, elem, proc; };
+   struct out_pt *pt;
+   array_init(struct out_pt, outpt, nptsend);
+   outpt->n=nptsend;
+   pt = (struct out_pt *)outpt->ptr;
+   for (int index = 0; index < nptsend; index++)
+   {
+      pt->index = index;
+      pt->elem = elem_send[index];
+      pt->proc  = gsl_proc[index];
+      pt->s_conn = s_conn_send[index];
+      for (int d = 0; d < dim; ++d)
+      {
+         pt->rst[d]= ref_send(index*dim + d);
+         pt->coords[d]= coords_send(index + d*nptsend);
+      }
+      ++pt;
+   }
+
+   // Transfer data to target MPI ranks
+   sarray_transfer(struct out_pt, outpt, proc, 1, cr);
+
+   // unpack
+   int npt = outpt->n;
+   proc_recv.SetSize(npt);
+   elem_recv.SetSize(npt);
+   index_recv.SetSize(npt);
+   ref_recv.SetSize(npt*dim);
+   coords_recv.SetSize(npt*dim);
+   s_conn_recv.SetSize(npt);
+
+   pt = (struct out_pt *)outpt->ptr;
+   for (int index = 0; index < npt; index++)
+   {
+      index_recv[index] = pt->index;
+      elem_recv[index] = pt->elem;
+      proc_recv[index] = pt->proc;
+      s_conn_recv[index] = pt->s_conn;
+      for (int d = 0; d < dim; ++d)
+      {
+         ref_recv(index*dim + d)= pt->rst[d]; // by VDIM
+         coords_recv(index + d*npt)= pt->coords[d]; // by NODES
+      }
+      ++pt;
+   }
+
+   array_free(outpt);
+   delete outpt;
+}
+
+void GSLIBCommunicator::SendData2(int dim,
+                                  const Array<unsigned int> & gsl_proc,
+                                  const Vector &xyz_send,
+                                  const Vector &xi_send,
+                                  const Array<int> &s_conn_send,
+                                  const Array<int> &conn_send,
+                                  const DenseMatrix &coords_send,
+                                  Vector &xyz_recv,
+                                  Vector &xi_recv,
+                                  Array<int> &s_conn_recv,
+                                  Array<int> &conn_recv,
+                                  DenseMatrix &coords_recv)
+{
+   int nptsend = gsl_proc.Size();
+
+   struct gslib::array *outpt = new gslib::array;
+   struct out_pt {double xyz[3], xi[2], coords[12]; int s_conn; int conn[4]; uint proc;};
+   struct out_pt *pt;
+   array_init(struct out_pt, outpt, nptsend);
+   outpt->n=nptsend;
+   pt = (struct out_pt *)outpt->ptr;
+   for (int index = 0; index < nptsend; index++)
+   {
+      pt->proc  = gsl_proc[index];
+      pt->s_conn  = s_conn_send[index];
+      for (int d = 0; d < dim-1; ++d)
+      {
+         pt->xi[d]= xi_send(index*(dim-1) + d);
+      }
+      for (int d = 0; d < dim; ++d)
+      {
+         pt->xyz[d]= xyz_send(index + d*nptsend);
+      }
+      for (int j = 0; j<4; j++)
+      {
+         pt->conn[j] = conn_send[index*4+j];
+         for (int d = 0; d < dim; ++d)
+         {
+            pt->coords[j*dim+d]= coords_send(index*4+j,d);
+         }
+      }
+      ++pt;
+   }
+
+   // Transfer data to target MPI ranks
+   sarray_transfer(struct out_pt, outpt, proc, 1, cr);
+   // unpack
+   int npt = outpt->n;
+   xi_recv.SetSize(npt*(dim-1));
+   xyz_recv.SetSize(npt*dim);
+   s_conn_recv.SetSize(npt);
+   conn_recv.SetSize(npt*4);
+   coords_recv.SetSize(npt*4,dim);
+
+   pt = (struct out_pt *)outpt->ptr;
+   for (int index = 0; index < npt; index++)
+   {
+      s_conn_recv[index] = pt->s_conn;
+      for (int d = 0; d < dim-1; ++d)
+      {
+         xi_recv(index*(dim-1) + d) = pt->xi[d];
+      }
+      for (int d = 0; d < dim; ++d)
+      {
+         xyz_recv(index + d*npt)= pt->xyz[d]; // by NODES
+      }
+      for (int j = 0; j<4; j++)
+      {
+         conn_recv[index*4+j] = pt->conn[j];
+         for (int d = 0; d < dim; ++d)
+         {
+            coords_recv(index*4+j,d) = pt->coords[j*dim+d];
+         }
+      }
+      ++pt;
+   }
+   array_free(outpt);
+   delete outpt;
+}
+
+
+void GSLIBCommunicator::ExchangeNormal(Mesh & mesh,
+                                       const Array<unsigned int> &gsl_proc,
+                                       const Array<unsigned int> &gsl_mfem_elem,
+                                       const Vector &gsl_mfem_ref,
+                                       Vector &recv_normals)
+{
+   int dim = mesh.Dimension();
+   int nptsend = gsl_proc.Size();
+   int nptElem   = gsl_mfem_elem.Size();
+   int nptRST    = gsl_mfem_ref.Size();
+
+   recv_normals.SetSize(nptRST);
+   int nptNormal    = recv_normals.Size();
+
+   MFEM_VERIFY(nptElem == nptsend,
+               "Incompatible Elem size.");
+   MFEM_VERIFY(nptsend*dim == nptRST,
+               "Incompatible nptRST size.");
+   MFEM_VERIFY(dim <= 3,
+               "Incompatible dimension.");
+
+   // Pack data to send via crystal router
+   struct gslib::array *outpt = new gslib::array;
+
+   struct out_pt { double rst[3]; uint index, elem, proc; };
+   struct out_pt *pt;
+   array_init(struct out_pt, outpt, nptsend);
+   outpt->n=nptsend;
+   pt = (struct out_pt *)outpt->ptr;
+   for (int index = 0; index < nptsend; index++)
+   {
+      pt->index = index;
+      pt->elem = gsl_mfem_elem[index];
+      pt->proc  = gsl_proc[index];
+      for (int d = 0; d < dim; ++d)
+      {
+         pt->rst[d]= gsl_mfem_ref(index*dim + d);
+      }
+      ++pt;
+   }
+
+   // Transfer data to target MPI ranks
+   sarray_transfer(struct out_pt, outpt, proc, 1, cr);
+
+   // Get normal vector
+   int npt = outpt->n;
+   pt = (struct out_pt *)outpt->ptr;
+   Vector normal(npt*dim);
+   for (int index = 0; index < npt; index++)
+   {
+      IntegrationPoint ip;
+      ip.Set3(&pt->rst[0]);
+      Vector localval(normal.GetData()+index*dim, dim);
+      // get the normal at this integration point here
+      // for now I just put back this proc's rank + the input rst coordinates
+      for (int d = 0; d < dim; d++)
+      {
+         localval(d) = gsl_comm->id + pt->rst[d];
+      }
+      ++pt;
+   }
+
+   // Save index and proc data in a struct
+   struct gslib::array *savpt = new gslib::array;
+   struct sav_pt { uint index, proc; };
+   struct sav_pt *spt;
+   array_init(struct sav_pt, savpt, npt);
+   savpt->n=npt;
+   spt = (struct sav_pt *)savpt->ptr;
+   pt  = (struct out_pt *)outpt->ptr;
+   for (int index = 0; index < npt; index++)
+   {
+      spt->index = pt->index;
+      spt->proc  = pt->proc;
+      ++pt; ++spt;
+   }
+
+   array_free(outpt);
+   delete outpt;
+
+   // Copy data from save struct to send struct and send component wise
+   struct gslib::array *sendpt = new gslib::array;
+   struct send_pt { double ival; uint index, proc; };
+   struct send_pt *sdpt;
+   for (int j = 0; j < dim; j++)
+   {
+      array_init(struct send_pt, sendpt, npt);
+      sendpt->n=npt;
+      spt  = (struct sav_pt *)savpt->ptr;
+      sdpt = (struct send_pt *)sendpt->ptr;
+      for (int index = 0; index < npt; index++)
+      {
+         sdpt->index = spt->index;
+         sdpt->proc  = spt->proc;
+         sdpt->ival  = normal(j + index*dim);
+         ++sdpt; ++spt;
+      }
+
+      sarray_transfer(struct send_pt, sendpt, proc, 1, cr);
+      sdpt = (struct send_pt *)sendpt->ptr;
+      for (int index = 0; index < static_cast<int>(sendpt->n); index++)
+      {
+         int idx = sdpt->index*dim + j;
+         recv_normals(idx) = sdpt->ival;
+         ++sdpt;
+      }
+      array_free(sendpt);
+   }
+   array_free(savpt);
+   delete sendpt;
+   delete savpt;
+}
+
+void GSLIBCommunicator::FreeData()
+{
+   crystal_free(cr);
+}
+
+GSLIBCommunicator::~GSLIBCommunicator()
+{
+   delete gsl_comm;
+   delete cr;
+}
+#endif
 
 } // namespace mfem
 

fem/gslib.hpp

@@ -290,6 +290,55 @@ public:
    using FindPointsGSLIB::Interpolate;
 };
 
+#ifdef MFEM_USE_MPI
+// Use to send info to certain processes
+class GSLIBCommunicator
+{
+protected:
+   struct gslib::crystal *cr;             // gslib's internal data
+   struct gslib::comm *gsl_comm;          // gslib's internal data
+
+public:
+   GSLIBCommunicator(MPI_Comm comm_);
+
+   virtual ~GSLIBCommunicator();
+
+   void ExchangeNormal(Mesh& mesh,
+                       const Array<unsigned int> &gsl_proc,
+                       const Array<unsigned int> &gsl_mfem_elem,
+                       const Vector &gsl_mfem_ref,
+                       Vector &recv_normals); //npt*dim
+
+   void SendData(int dim,
+                 const Array<unsigned int> & gsl_proc,
+                 const Array<unsigned int> & elem_send,
+                 const Vector &ref_send,
+                 const Vector &coords_send,
+                 const Array<int> &s_conn_send,
+                 Array<unsigned int> & proc_recv,
+                 Array<unsigned int> & index_recv,
+                 Array<unsigned int> & elem_recv,
+                 Vector &ref_recv,
+                 Vector &coords_recv,
+                 Array<int> & s_conn_recv);
+
+   void SendData2(int dim,
+                  const Array<unsigned int> & gsl_proc,
+                  const Vector &xyz_send,
+                  const Vector &xi_send,
+                  const Array<int> &s_conn_send,
+                  const Array<int> &conn_send,
+                  const DenseMatrix &coords_send,
+                  Vector &xyz_recv,
+                  Vector &ref_recv,
+                  Array<int> &s_conn_recv,
+                  Array<int> &conn_recv,
+                  DenseMatrix &coords_recv);
+
+   virtual void FreeData();
+};
+#endif
+
 } // namespace mfem
 
 #endif // MFEM_USE_GSLIB

makefile

@@ -125,7 +125,7 @@ EXAMPLE_TEST_DIRS := examples
 
 MINIAPP_SUBDIRS = common electromagnetics meshing navier performance tools \
  toys nurbs gslib adjoint solvers shifted mtop parelag autodiff hooke \
- multidomain dpg hdiv-linear-solver spde
+ multidomain dpg hdiv-linear-solver spde contact
 MINIAPP_DIRS := $(addprefix miniapps/,$(MINIAPP_SUBDIRS))
 MINIAPP_TEST_DIRS := $(filter-out %/common,$(MINIAPP_DIRS))
 MINIAPP_USE_COMMON := $(addprefix miniapps/,electromagnetics meshing tools \

miniapps/contact/contact_driver.cpp

@@ -0,0 +1,151 @@
+//                                Contact example
+//
+// Compile with: make contact
+//
+// Sample runs:  ./contact -m1 block1.mesh -m2 block2.mesh -at "5 6 7 8"
+// Sample runs:  ./contact -m1 block1_d.mesh -m2 block2_d.mesh -at "5 6 7 8"
+
+#include "mfem.hpp"
+#include <fstream>
+#include <iostream>
+#include "ipsolver/IPsolver.hpp"
+
+using namespace std;
+using namespace mfem;
+
+int main(int argc, char *argv[])
+{
+   // 1. Parse command-line options.
+   const char *mesh_file1 = "meshes/block1.mesh";
+   const char *mesh_file2 = "meshes/rotatedblock2.mesh";
+   int order = 1;
+   int ref = 0;
+   Array<int> attr;
+   Array<int> m_attr;
+   int linSolver = 2;
+   bool paraview = false;
+   bool visualization = true;
+
+   OptionsParser args(argc, argv);
+   args.AddOption(&mesh_file1, "-m1", "--mesh1",
+                  "First mesh file to use.");
+   args.AddOption(&mesh_file2, "-m2", "--mesh2",
+                  "Second mesh file to use.");
+   args.AddOption(&attr, "-at", "--attributes-surf",
+                  "Attributes of boundary faces on contact surface for mesh 2.");
+   args.AddOption(&ref, "-r", "--refinements",
+                  "Number of uniform refinements.");
+   args.AddOption(&paraview, "-paraview", "--paraview", "-no-paraview",
+                  "--no-paraview",
+                  "Enable or disable ParaView visualization.");
+   args.AddOption(&visualization, "-vis", "--visualization", "-no-vis",
+                  "--no-visualization",
+                  "Enable or disable GLVis visualization.");
+   args.Parse();
+   if (!args.Good())
+   {
+      args.PrintUsage(cout);
+      return 1;
+   }
+   args.PrintOptions(cout);
+
+   ElasticityProblem prob1(mesh_file1,ref,order);
+   ElasticityProblem prob2(mesh_file2,ref,order);
+
+   ContactProblem contact(&prob1, &prob2);
+   QPOptContactProblem qpopt(&contact);
+   int numconstr = contact.GetNumConstraints();
+
+   InteriorPointSolver optimizer(&qpopt);
+   optimizer.SetTol(1e-6);
+   optimizer.SetMaxIter(50);
+   optimizer.SetLinearSolver(linSolver);
+   optimizer.SetLinearSolveTol(1e-10);
+
+   GridFunction x1 = prob1.GetDisplacementGridFunction();
+   GridFunction x2 = prob2.GetDisplacementGridFunction();
+
+   int ndofs1 = prob1.GetNumDofs();
+   int ndofs2 = prob2.GetNumDofs();
+   int ndofs = ndofs1 + ndofs2;
+
+   Vector x0(ndofs); x0 = 0.0;
+   x0.SetVector(x1,0);
+   x0.SetVector(x2,x1.Size());
+
+   Vector xf(ndofs); xf = 0.0;
+   optimizer.Mult(x0, xf);
+   Array<int> & CGiterations = optimizer.GetCGIterNumbers();
+
+   double Einitial = contact.E(x0);
+   double Efinal = contact.E(xf);
+
+   mfem::out << endl;
+   mfem::out << " Initial Energy objective     = " << Einitial << endl;
+   mfem::out << " Final Energy objective       = " << Efinal << endl;
+   mfem::out << " Global number of dofs        = " << ndofs1 + ndofs2 << endl;
+   mfem::out << " Global number of constraints = " << numconstr << endl;
+   mfem::out << " CG iteration numbers         = " ;
+   CGiterations.Print(mfem::out, CGiterations.Size());
+
+   MFEM_VERIFY(optimizer.GetConverged(),
+               "Interior point solver did not converge.");
+
+   if (visualization || paraview)
+   {
+      FiniteElementSpace * fes1 = prob1.GetFESpace();
+      FiniteElementSpace * fes2 = prob2.GetFESpace();
+
+      Mesh * mesh1 = fes1->GetMesh();
+      Mesh * mesh2 = fes2->GetMesh();
+
+      GridFunction x1_gf(fes1,xf.GetData());
+      GridFunction x2_gf(fes2,&xf.GetData()[fes1->GetTrueVSize()]);
+
+      mesh1->MoveNodes(x1_gf);
+      mesh2->MoveNodes(x2_gf);
+
+      if (paraview)
+      {
+         ParaViewDataCollection paraview_dc1("QPContactBody1", mesh1);
+         paraview_dc1.SetPrefixPath("ParaView");
+         paraview_dc1.SetLevelsOfDetail(1);
+         paraview_dc1.SetDataFormat(VTKFormat::BINARY);
+         paraview_dc1.SetHighOrderOutput(true);
+         paraview_dc1.SetCycle(0);
+         paraview_dc1.SetTime(0.0);
+         paraview_dc1.RegisterField("Body1", &x1_gf);
+         paraview_dc1.Save();
+
+         ParaViewDataCollection paraview_dc2("QPContactBody2", mesh2);
+         paraview_dc2.SetPrefixPath("ParaView");
+         paraview_dc2.SetLevelsOfDetail(1);
+         paraview_dc2.SetDataFormat(VTKFormat::BINARY);
+         paraview_dc2.SetHighOrderOutput(true);
+         paraview_dc2.SetCycle(0);
+         paraview_dc2.SetTime(0.0);
+         paraview_dc2.RegisterField("Body2", &x2_gf);
+         paraview_dc2.Save();
+      }
+
+      if (visualization)
+      {
+         char vishost[] = "localhost";
+         int visport = 19916;
+         {
+            socketstream sol_sock(vishost, visport);
+            sol_sock.precision(8);
+            sol_sock << "parallel " << 2 << " " << 0 << "\n"
+                     << "solution\n" << *mesh1 << x1_gf << flush;
+         }
+         {
+            socketstream sol_sock(vishost, visport);
+            sol_sock.precision(8);
+            sol_sock << "parallel " << 2 << " " << 1 << "\n"
+                     << "solution\n" << *mesh2 << x2_gf << flush;
+         }
+      }
+   }
+
+   return 0;
+}

miniapps/contact/ipsolver/IPsolver.cpp

@@ -0,0 +1,818 @@
+#include "mfem.hpp"
+#include "IPsolver.hpp"
+#include <fstream>
+#include <iostream>
+#include <cstdlib>
+
+using namespace std;
+using namespace mfem;
+
+
+
+InteriorPointSolver::InteriorPointSolver(QPOptContactProblem * Problem) 
+: optProblem(Problem), block_offsetsumlz(5), block_offsetsuml(4), block_offsetsx(3),
+saveLogBarrierIterates(false)
+{
+   rel_tol  = 1.e-2;
+   max_iter = 20;
+   mu_k     = 1.0;
+
+   sMax     = 1.e2;
+   kSig     = 1.e10;   // control deviation from primal Hessian
+   tauMin   = 0.8;     // control rate at which iterates can approach the boundary
+   eta      = 1.e-4;   // backtracking constant
+   thetaMin = 1.e-4;   // allowed violation of the equality constraints
+
+   // constants in line-step A-5.4
+   delta    = 1.0;
+   sTheta   = 1.1;
+   sPhi     = 2.3;
+
+   // control the rate at which the penalty parameter is decreased
+   kMu     = 0.2;
+   thetaMu = 1.5;
+
+   // TO DO -- include the filter
+
+   thetaMax = 1.e6; // maximum constraint violation
+   // data for the second order correction
+   kSoc     = 0.99;
+
+   // equation (18)
+   gTheta = 1.e-5;
+   gPhi   = 1.e-5;
+
+   kEps   = 1.e1;
+
+   dimU = optProblem->GetDimU();
+   dimM = optProblem->GetDimM();
+   dimC = optProblem->GetDimC();
+   ckSoc.SetSize(dimC);
+   
+   block_offsetsumlz[0] = 0;
+   block_offsetsumlz[1] = dimU; // u
+   block_offsetsumlz[2] = dimM; // m
+   block_offsetsumlz[3] = dimC; // lambda
+   block_offsetsumlz[4] = dimM; // zl
+   block_offsetsumlz.PartialSum();
+
+   for(int i = 0; i < block_offsetsuml.Size(); i++)  { block_offsetsuml[i] = block_offsetsumlz[i]; }
+   for(int i = 0; i < block_offsetsx.Size(); i++)    { block_offsetsx[i]   = block_offsetsuml[i] ; }
+
+   // lower-bound for the inequality constraint m >= ml
+   ml = optProblem->Getml();
+   
+   lk.SetSize(dimC);  lk  = 0.0;
+   zlk.SetSize(dimM); zlk = 0.0;
+
+   linSolver = 0;
+   MyRank = 0;
+   iAmRoot = MyRank == 0 ? true : false;
+}
+
+double InteriorPointSolver::MaxStepSize(Vector &x, Vector &xl, Vector &xhat, double tau)
+{
+   double alphaMaxloc = 1.0;
+   double alphaTmp;
+   for(int i = 0; i < x.Size(); i++)
+   {
+      if( xhat(i) < 0. )
+      {
+         alphaTmp = -1. * tau * (x(i) - xl(i)) / xhat(i);
+         alphaMaxloc = min(alphaMaxloc, alphaTmp);
+      } 
+   }
+
+   // alphaMaxloc is the local maximum step size which is
+   // distinct on each MPI process. Need to compute
+   // the global maximum step size 
+   double alphaMaxglb;
+   alphaMaxglb = alphaMaxloc;
+   return alphaMaxglb;
+}
+
+double InteriorPointSolver::MaxStepSize(Vector &x, Vector &xhat, double tau)
+{
+   Vector zero(x.Size()); zero = 0.0;
+   return MaxStepSize(x, zero, xhat, tau);
+}
+
+
+void InteriorPointSolver::Mult(const Vector &x0, Vector &xf)
+{
+   BlockVector x0block(block_offsetsx); x0block = 0.0;
+   x0block.GetBlock(0).Set(1.0, x0);
+   // To do: give options for user specificiation of initialization m0
+   x0block.GetBlock(1) = 1.0;
+   x0block.GetBlock(1).Add(1.0, ml);
+   BlockVector xfblock(block_offsetsx); xfblock = 0.0;
+   Mult(x0block, xfblock);
+   xf.Set(1.0, xfblock.GetBlock(0));
+}
+
+void InteriorPointSolver::Mult(const BlockVector &x0, BlockVector &xf)
+{
+   converged = false;
+   
+   BlockVector xk(block_offsetsx), xhat(block_offsetsx); xk = 0; xhat = 0.0;
+   BlockVector Xk(block_offsetsumlz), Xhat(block_offsetsumlz); Xk = 0.0; Xhat = 0.0;
+   BlockVector Xhatuml(block_offsetsuml); Xhatuml = 0.0;
+   Vector zlhat(dimM); zlhat = 0.0;
+
+   xk.GetBlock(0).Set(1.0, x0.GetBlock(0));
+   xk.GetBlock(1).Set(1.0, x0.GetBlock(1));
+   // running estimate of the final values of the Lagrange multipliers
+   lk  = 0.0;
+   zlk = 0.0;
+
+   for(int i = 0; i < dimM; i++)
+   {
+      zlk(i) = 1.e1 * mu_k / (xk(i+dimU) - ml(i));
+   }
+
+   Xk.GetBlock(0).Set(1.0, xk.GetBlock(0));
+   Xk.GetBlock(1).Set(1.0, xk.GetBlock(1));
+   Xk.GetBlock(2).Set(1.0, lk);
+   Xk.GetBlock(3).Set(1.0, zlk);
+
+   /* set theta0 = theta(x0)
+      *     thetaMin
+      *     thetaMax
+      * when theta(xk) < thetaMin and the switching condition holds
+      * then we ask for the Armijo sufficient decrease of the barrier
+      * objective to be satisfied, in order to accept the trial step length alphakl
+      * 
+      * thetaMax controls how the filter is initialized for each log-barrier subproblem
+      * F0 = {(th, phi) s.t. th > thetaMax}
+      * that is the filter does not allow for iterates where the constraint violation
+      * is larger than that of thetaMax
+      */
+   double theta0 = theta(xk);
+   thetaMin = 1.e-4 * max(1.0, theta0);
+   thetaMax = 1.e8  * thetaMin;
+
+   double Eeval, maxBarrierSolves, Eevalmu0;
+   bool printOptimalityError; // control optimality error print to console for log-barrier subproblems
+   
+   maxBarrierSolves = 10;
+
+   for(jOpt = 0; jOpt < max_iter; jOpt++)
+   {
+      mfem::out << "interior-point solve step " << jOpt << endl;
+      // A-2. Check convergence of overall optimization problem
+      printOptimalityError = false;
+      Eevalmu0 = E(xk, lk, zlk, printOptimalityError);
+      if(Eevalmu0 < rel_tol)
+      {
+         converged = true;
+         mfem::out << "solved optimization problem :)\n";
+         break;
+      }
+      
+      if(jOpt > 0) { maxBarrierSolves = 1; }
+      
+      for(int i = 0; i < maxBarrierSolves; i++)
+      {
+         // A-3. Check convergence of the barrier subproblem
+         printOptimalityError = true;
+         Eeval = E(xk, lk, zlk, mu_k, printOptimalityError);
+         if(Eeval < kEps * mu_k)
+         {
+            mfem::out << "solved barrier subproblem, for mu = " << mu_k << endl;
+            // A-3.1. Recompute the barrier parameter
+            mu_k  = max(rel_tol / 10., min(kMu * mu_k, pow(mu_k, thetaMu)));
+            // A-3.2. Re-initialize the filter
+            F1.DeleteAll();
+            F2.DeleteAll();
+         }
+         else
+         {
+            break;
+         }
+      }
+      // A-4. Compute the search direction
+      // solve for (uhat, mhat, lhat)
+      mfem::out << "\n** A-4. IP-Newton solve **\n";
+      zlhat = 0.0; Xhatuml = 0.0;
+      // why do we have Xhatuml ....???
+      // TO DO: remove Xhatuml in favor of passing Xhat
+      IPNewtonSolve(xk, lk, zlk, zlhat, Xhatuml, mu_k, false); 
+
+      // assign data stack, X = (u, m, l, zl)
+      Xk = 0.0;
+      Xk.GetBlock(0).Set(1.0, xk.GetBlock(0));
+      Xk.GetBlock(1).Set(1.0, xk.GetBlock(1));
+      Xk.GetBlock(2).Set(1.0, lk);
+      Xk.GetBlock(3).Set(1.0, zlk);
+
+      // assign data stack, Xhat = (uhat, mhat, lhat, zlhat)
+      Xhat = 0.0;
+      for(int i = 0; i < 3; i++)
+      {
+         Xhat.GetBlock(i).Set(1.0, Xhatuml.GetBlock(i));
+      }
+      Xhat.GetBlock(3).Set(1.0, zlhat);
+
+      // A-5. Backtracking line search.
+      mfem::out << "\n** A-5. Linesearch **\n";
+      mfem::out << "mu = " << mu_k << endl;
+      
+      lineSearch(Xk, Xhat, mu_k);
+      if(lineSearchSuccess)
+      {
+         if(!switchCondition || !sufficientDecrease)
+         {
+            F1.Append( (1. - gTheta) * thx0);
+            F2.Append( phx0 - gPhi * thx0);
+         }
+         // ----- A-6: Accept the trial point
+         // print info regarding zl...
+         xk.GetBlock(0).Add(alpha, Xhat.GetBlock(0));
+         xk.GetBlock(1).Add(alpha, Xhat.GetBlock(1));
+         lk.Add(alpha,   Xhat.GetBlock(2));
+         zlk.Add(alphaz, Xhat.GetBlock(3));
+         projectZ(xk, zlk, mu_k);
+      }
+      else
+      {
+         mfem::out << "lineSearch not successful :(\n";
+         mfem::out << "attempting feasibility restoration with theta = " << thx0 << endl;
+         mfem::out << "no feasibility restoration implemented, exiting now \n";
+         break;
+      }
+      //
+      if(jOpt + 1 == max_iter) 
+      { 
+         mfem::out << "maximum optimization iterations :(\n";
+      }
+   }
+   // done with optimization routine, just reassign data to xf reference so
+   // that the application code has access to the optimal point
+   xf = 0.0;
+   xf.GetBlock(0).Set(1.0, xk.GetBlock(0));
+   xf.GetBlock(1).Set(1.0, xk.GetBlock(1));
+}
+
+void InteriorPointSolver::FormIPNewtonMat(BlockVector & x, Vector & l, Vector &zl, BlockOperator &Ak)
+{
+   // WARNING: Huu, Hum, Hmu, Hmm should all be Hessian terms of the Lagrangian, currently we 
+   //          them by Hessian terms of the objective function and neglect the Hessian of l^T c
+
+   Huu = optProblem->Duuf(x);
+   Hum = optProblem->Dumf(x);
+   Hmu = optProblem->Dmuf(x);
+   Hmm = optProblem->Dmmf(x);
+
+   Vector DiagLogBar(dimM); DiagLogBar = 0.0;
+   for(int ii = 0; ii < dimM; ii++)
+   {
+      DiagLogBar(ii) = zl(ii) / (x(ii+dimU) - ml(ii));
+   }
+
+   if(saveLogBarrierIterates)
+   {
+      std::ofstream diagStream;
+      char diagString[100];
+      snprintf(diagString, 100, "logBarrierHessiandata/D%d.dat", jOpt);
+      diagStream.open(diagString, ios::out | ios::trunc);
+      for(int ii = 0; ii < dimM; ii++)
+      {
+         diagStream << setprecision(30) << DiagLogBar(ii) << endl;
+      }
+      diagStream.close();
+   } 
+
+   delete Wmm;
+   if(Hmm != nullptr)
+   {
+      SparseMatrix * D = new SparseMatrix(DiagLogBar);
+      Wmm = Add(*Hmm, *D);
+      delete D;
+   }
+   else
+   {
+      Wmm = new SparseMatrix(DiagLogBar);
+   }
+
+   delete JuT;
+   delete JmT;
+   Ju = optProblem->Duc(x); JuT = Transpose(*Ju);
+   Jm = optProblem->Dmc(x); JmT = Transpose(*Jm);
+   
+   Huucl = optProblem->lDuuc(x, l);
+   if(Huucl != nullptr)
+   {
+      delete HLuucl;
+      HLuucl = Add(*Huucl, *Huu);
+      Ak.SetBlock(0, 0, HLuucl);
+   } 
+   else
+   {
+      Ak.SetBlock(0, 0, Huu);
+   }
+
+   //         IP-Newton system matrix
+   //    Ak = [[H_(u,u)  H_(u,m)   J_u^T]
+   //          [H_(m,u)  W_(m,m)   J_m^T]
+   //          [ J_u      J_m       0  ]]
+
+   Ak.SetBlock(0, 0, Huu);
+   Ak.SetBlock(0, 2, JuT);
+   Ak.SetBlock(1, 1, Wmm);   
+   Ak.SetBlock(1, 2, JmT);
+   Ak.SetBlock(2, 0, Ju); 
+   Ak.SetBlock(2, 1, Jm); 
+
+   if(Hum != nullptr) { Ak.SetBlock(0, 1, Hum); Ak.SetBlock(1, 0, Hmu); }
+}
+
+// perturbed KKT system solve
+// determine the search direction
+void InteriorPointSolver::IPNewtonSolve(BlockVector &x, Vector &l, Vector &zl, Vector &zlhat, BlockVector &Xhat, double mu, bool socSolve)
+{
+   // solve A x = b, where A is the IP-Newton matrix
+   BlockOperator A(block_offsetsuml, block_offsetsuml); BlockVector b(block_offsetsuml); b = 0.0;
+   FormIPNewtonMat(x, l, zl, A);
+
+   //       [grad_u phi + Ju^T l]
+   // b = - [grad_m phi + Jm^T l]
+   //       [          c        ]
+   BlockVector gradphi(block_offsetsx); gradphi = 0.0;
+   BlockVector JTl(block_offsetsx); JTl = 0.0;
+   Dxphi(x, mu, gradphi);
+  
+   (A.GetBlock(0,2)).Mult(l, JTl.GetBlock(0));
+   (A.GetBlock(1,2)).Mult(l, JTl.GetBlock(1));
+
+   
+   for(int ii = 0; ii < 2; ii++)
+   {
+      b.GetBlock(ii).Set(1.0, gradphi.GetBlock(ii));
+      b.GetBlock(ii).Add(1.0, JTl.GetBlock(ii));
+   }
+   if(!socSolve) 
+   {
+      optProblem->c(x, b.GetBlock(2));
+   }
+   else
+   {
+      b.GetBlock(2).Set(1.0, ckSoc);
+   }
+   b *= -1.0; 
+   Xhat = 0.0;
+
+
+#ifdef MFEM_USE_SUITESPARSE
+   // Direct solve for IP-Newton saddle-point system
+   // A = [ [ Huu   0    Ju^T]
+   //       [  0    D     -I ]
+   //       [ Ju   -I      0 ]]
+   // if(linSolver == 0)
+   // {
+   //    BlockMatrix ABlockMatrix(block_offsetsuml, block_offsetsuml);
+   //    for(int ii = 0; ii < 3; ii++)
+   //    {
+   //       for(int jj = 0; jj < 3; jj++)
+   //       {
+   //          if(!A.IsZeroBlock(ii, jj))
+   //          {
+   //             ABlockMatrix.SetBlock(ii, jj, dynamic_cast<SparseMatrix *>(&(A.GetBlock(ii, jj))));
+   //          }
+   //       }
+   //    }
+   //    /* direct solve of the 3x3 IP-Newton linear system */
+   //    UMFPackSolver ASolver;
+   //    SparseMatrix *ASparse = ABlockMatrix.CreateMonolithic();
+   //    ASolver.SetOperator(*ASparse);
+   //    ASolver.Mult(b, Xhat);
+
+   //    Vector residual(Xhat.Size());
+   //    ASparse->Mult(Xhat, residual);
+   //    residual.Add(-1.0, b);
+   //    delete ASparse;
+   // }
+   // else if(linSolver == 1)
+   // {
+   //    // Direct solve for 0,0 Schur complement of IP-Newton system, Huu + Ju^T Wmm Ju,
+   //    // where Wmm = D for contact problems
+   //    SparseMatrix * Huuloc = dynamic_cast<SparseMatrix *>(&(A.GetBlock(0, 0)));
+   //    SparseMatrix * Wmmloc = dynamic_cast<SparseMatrix *>(&(A.GetBlock(1, 1)));
+   //    SparseMatrix * Juloc  = dynamic_cast<SparseMatrix *>(&(A.GetBlock(2, 0)));
+   //    SparseMatrix * JuTloc = dynamic_cast<SparseMatrix *>(&(A.GetBlock(0, 2)));
+   //    Vector DVec(dimM); DVec = 0.0;
+   //    Vector one(dimM); one = 1.0;
+   //    D->Mult(one, DVec);
+   //    SparseMatrix *JuTDJu = Mult_AtDA(*Juloc, DVec);     // Ju^T D Ju
+   //    SparseMatrix *Areduced = Add(*Huuloc, *JuTDJu);  // Huu + Ju^T D Ju
+      
+   //    /* prepare the reduced rhs */
+   //    // breduced = bu + Ju^T (bm + Wmm bl)
+   //    Vector breduced(dimU); breduced = 0.0;
+   //    Vector tempVec(dimM); tempVec = 0.0;
+   //    Wmmloc->Mult(b.GetBlock(2), tempVec);
+   //    tempVec.Add(1.0, b.GetBlock(1));
+   //    JuTloc->Mult(tempVec, breduced);
+   //    breduced.Add(1.0, b.GetBlock(0));
+
+   //    // solve the reduced linear system
+   //    UMFPackSolver AreducedSolver;
+   //    AreducedSolver.SetOperator(*Areduced);
+   //    AreducedSolver.Mult(breduced, Xhat.GetBlock(0));
+
+   //    // now propagate solved uhat to obtain mhat and lhat
+   //    // xm = Ju xu - bl
+   //    Juloc->Mult(Xhat.GetBlock(0), Xhat.GetBlock(1));
+   //    Xhat.GetBlock(1).Add(-1.0, b.GetBlock(2));
+
+   //    // xl = Wmm xm - bm
+   //    Wmmloc->Mult(Xhat.GetBlock(1), Xhat.GetBlock(2));
+   //    Xhat.GetBlock(2).Add(-1.0, b.GetBlock(1));
+
+   //    delete JuTDJu;
+   //    delete Areduced;
+   // }
+#else
+   MFEM_VERIFY(linSolver > 1, "linSolver = 0, 1 require MFEM_USE_SUITESPARSE=YES");
+#endif
+   // if(linSolver ==2)
+   { 
+      // Iterative solve for 0,0 Schur complement of IP-Newton system, Huu + Ju^T Wmm Ju,
+      // where Wmm = D for contact problems
+      // here the iterative solver is a Jacobi-preconditioned CG-solve
+      SparseMatrix * Huuloc = dynamic_cast<SparseMatrix *>(&(A.GetBlock(0, 0)));
+      SparseMatrix * Wmmloc = dynamic_cast<SparseMatrix *>(&(A.GetBlock(1, 1)));
+      SparseMatrix * Juloc  = dynamic_cast<SparseMatrix *>(&(A.GetBlock(2, 0)));
+      SparseMatrix * JuTloc = dynamic_cast<SparseMatrix *>(&(A.GetBlock(0, 2)));
+
+      SparseMatrix *JuTDJu   = RAP(*Juloc,*Wmmloc,*Juloc);     // Ju^T D Ju
+      SparseMatrix *Areduced = Add(*Huuloc, *JuTDJu);  // Huu + Ju^T D Ju
+
+      /* prepare the reduced rhs */
+      // breduced = bu + Ju^T (bm + Wmm bl)
+      Vector breduced(dimU); breduced = 0.0;
+      Vector tempVec(dimM); tempVec = 0.0;
+      Wmmloc->SortColumnIndices();
+
+      Wmmloc->Mult(b.GetBlock(2), tempVec);
+      tempVec.Add(1.0, b.GetBlock(1));
+      JuTloc->Mult(tempVec, breduced);
+
+
+      breduced.Add(1.0, b.GetBlock(0));
+      int globalNumRows = dimU;
+      HYPRE_BigInt rowStarts[2];
+      rowStarts[0] = 0;
+      rowStarts[1] = dimU;
+
+      HypreParMatrix Ahypre(MPI_COMM_WORLD, globalNumRows, rowStarts, Areduced);
+      HypreBoomerAMG Aprec(Ahypre);
+      Aprec.SetPrintLevel(0);
+      Aprec.SetSystemsOptions(3,false);
+      HyprePCG AreducedSolver(MPI_COMM_WORLD);
+      AreducedSolver.SetOperator(Ahypre);
+      // AreducedSolver.SetRelTol(linSolveTol);
+      // AreducedSolver.SetRelTol(1e-6);
+      AreducedSolver.SetTol(1e-6);
+      AreducedSolver.SetMaxIter(1000);
+      AreducedSolver.SetPreconditioner(Aprec);
+      // AreducedSolver.SetResidualConvergenceOptions();
+      AreducedSolver.SetPrintLevel(2);
+
+      AreducedSolver.Mult(breduced, Xhat.GetBlock(0));
+      int num_iterations;
+      AreducedSolver.GetNumIterations(num_iterations);
+      cgnum_iterations.Append(num_iterations);
+    
+      // now propagate solved uhat to obtain mhat and lhat
+      // xm = Ju xu - bl
+      Juloc->Mult(Xhat.GetBlock(0), Xhat.GetBlock(1));
+      Xhat.GetBlock(1).Add(-1.0, b.GetBlock(2));
+      // xl = Wmm xm - bm
+      Wmmloc->Mult(Xhat.GetBlock(1), Xhat.GetBlock(2));
+      Xhat.GetBlock(2).Add(-1.0, b.GetBlock(1));
+
+      delete JuTDJu;
+      delete Areduced;
+   }
+    
+   /* backsolve to determine zlhat */
+   for(int ii = 0; ii < dimM; ii++)
+   {
+      zlhat(ii) = -1.*(zl(ii) + (zl(ii) * Xhat(ii + dimU) - mu) / (x(ii + dimU) - ml(ii)) );
+   }
+}
+
+// here Xhat, X will be BlockVectors w.r.t. the 4 partitioning X = (u, m, l, zl)
+
+void InteriorPointSolver::lineSearch(BlockVector& X0, BlockVector& Xhat, double mu)
+{
+  double tau  = max(tauMin, 1.0 - mu);
+  Vector u0   = X0.GetBlock(0);
+  Vector m0   = X0.GetBlock(1);
+  Vector l0   = X0.GetBlock(2);
+  Vector z0   = X0.GetBlock(3);
+  Vector uhat = Xhat.GetBlock(0);
+  Vector mhat = Xhat.GetBlock(1);
+  Vector lhat = Xhat.GetBlock(2);
+  Vector zhat = Xhat.GetBlock(3);
+  double alphaMax  = MaxStepSize(m0, ml, mhat, tau);
+  double alphaMaxz = MaxStepSize(z0, zhat, tau);
+  alphaz = alphaMaxz;
+
+
+  BlockVector x0(block_offsetsx); x0 = 0.0;
+  x0.GetBlock(0).Set(1.0, u0);
+  x0.GetBlock(1).Set(1.0, m0);
+  
+  BlockVector xhat(block_offsetsx); xhat = 0.0;
+  xhat.GetBlock(0).Set(1.0, uhat);
+  xhat.GetBlock(1).Set(1.0, mhat);
+  
+  BlockVector xtrial(block_offsetsx); xtrial = 0.0;
+  BlockVector Dxphi0(block_offsetsx); Dxphi0 = 0.0;
+  int maxBacktrack = 20;
+  alpha = alphaMax;
+
+
+  Vector ck0(dimC); ck0 = 0.0;
+  Vector zhatsoc(dimM); zhatsoc = 0.0;
+  BlockVector Xhatumlsoc(block_offsetsuml); Xhatumlsoc = 0.0;
+  BlockVector xhatsoc(block_offsetsx); xhatsoc = 0.0;
+  Vector uhatsoc(dimU); uhatsoc = 0.0;
+  Vector mhatsoc(dimM); mhatsoc = 0.0;
+
+  Dxphi(x0, mu, Dxphi0);
+  Dxphi0_xhat = InnerProduct(Dxphi0, xhat);
+  descentDirection = Dxphi0_xhat < 0. ? true : false;
+  if(descentDirection)
+  {
+    mfem::out << "is a descent direction for the log-barrier objective\n";
+  }
+  else
+  {
+    mfem::out << "is not a descent direction for the log-barrier objective\n";
+  }
+  mfem::out << "Dxphi^T xhat / (|| Dxphi||_2 * || xhat ||_2) = " << Dxphi0_xhat / (xhat.Norml2() * Dxphi0.Norml2()) << endl;
+  thx0 = theta(x0);
+  phx0 = phi(x0, mu);
+
+  lineSearchSuccess = false;
+  for(int i = 0; i < maxBacktrack; i++)
+  {
+    mfem::out << "\n--------- alpha = " << alpha << " ---------\n";
+
+    // ----- A-5.2. Compute trial point: xtrial = x0 + alpha_i xhat
+    xtrial.Set(1.0, x0);
+    xtrial.Add(alpha, xhat);
+
+    // ------ A-5.3. if not in filter region go to A.5.4 otherwise go to A-5.5.
+    thxtrial = theta(xtrial);
+    phxtrial = phi(xtrial, mu);
+
+    filterCheck(thxtrial, phxtrial);    
+    if(!inFilterRegion)
+    {
+      mfem::out << "not in filter region :)\n";
+      // ------ A.5.4: Check sufficient decrease
+      if(!descentDirection)
+      {
+        switchCondition = false;
+      }
+      else
+      {
+        switchCondition = (alpha * pow(abs(Dxphi0_xhat), sPhi) > delta * pow(thx0, sTheta)) ? true : false;
+      }
+      mfem::out << "theta(x0) = "     << thx0     << ", thetaMin = "                  << thetaMin             << endl;
+      mfem::out << "theta(xtrial) = " << thxtrial << ", (1-gTheta) *theta(x0) = "     << (1. - gTheta) * thx0 << endl;
+      mfem::out << "phi(xtrial) = "   << phxtrial << ", phi(x0) - gPhi *theta(x0) = " << phx0 - gPhi * thx0   << endl;
+      
+      // Case I      
+      if(thx0 <= thetaMin && switchCondition)
+      {
+        sufficientDecrease = phxtrial <= phx0 + eta * alpha * Dxphi0_xhat ? true : false;
+        if(sufficientDecrease)
+        {
+          mfem::out << "Accepted step length -- sufficient decrease in log-barrier objective.\n"; 
+          // accept the trial step
+          lineSearchSuccess = true;
+          break;
+        }
+      }
+      else
+      {
+        if(thxtrial <= (1. - gTheta) * thx0 || phxtrial <= phx0 - gPhi * thx0)
+        {
+          mfem::out << "Accepted step length -- decrease in either constraint violation or log-barrier objective.\n"; 
+          // accept the trial step
+          lineSearchSuccess = true;
+          break;
+        }
+      }
+      // A-5.5: Initialize the second-order correction
+      if((!(thx0 < thxtrial)) && i == 0)
+      {
+        mfem::out << "second order correction\n";
+        optProblem->c(xtrial, ckSoc);
+        optProblem->c(x0, ck0);
+        ckSoc.Add(alphaMax, ck0);
+        // A-5.6 Compute the second-order correction.
+        IPNewtonSolve(x0, l0, z0, zhatsoc, Xhatumlsoc, mu, true);
+        mhatsoc.Set(1.0, Xhatumlsoc.GetBlock(1));
+        // alphasoc = MaxStepSize(m0, ml, mhatsoc, tau);
+        //WARNING: not complete but currently solver isn't entering this region
+      }
+    }
+    else
+    {
+      mfem::out << "in filter region\n";
+    }
+
+    // include more if needed
+    alpha *= 0.5;
+
+  } 
+}
+
+
+void InteriorPointSolver::projectZ(const Vector &x, Vector &z, double mu)
+{
+  double zi;
+  double mudivmml;
+  for(int i = 0; i < dimM; i++)
+  {
+    zi = z(i);
+    mudivmml = mu / (x(i + dimU) - ml(i));
+    z(i) = max(min(zi, kSig * mudivmml), mudivmml / kSig);
+  }
+}
+
+void InteriorPointSolver::filterCheck(double th, double ph)
+{
+  inFilterRegion = false;
+  if(th > thetaMax)
+  {
+    inFilterRegion = true;
+  }
+  else
+  {
+    for(int i = 0; i < F1.Size(); i++)
+    {
+      if(th >= F1[i] && ph >= F2[i])
+      {
+        inFilterRegion = true;
+        break;
+      }
+    }
+  }
+}
+
+double InteriorPointSolver::E(const BlockVector &x, const Vector &l, const Vector &zl, double mu, bool print)
+{
+  double E1, E2, E3;
+  double sc, sd;
+  BlockVector gradL(block_offsetsx); gradL = 0.0; // stationarity grad L = grad f + J^T l - z
+  Vector cx(dimC); cx = 0.0;     // feasibility c = c(x)
+  Vector comp(dimM); comp = 0.0; // complementarity M Z - mu 1
+
+  DxL(x, l, zl, gradL);
+  E1 = gradL.Normlinf();
+
+  optProblem->c(x, cx);
+  E2 = cx.Normlinf();
+
+  for(int ii = 0; ii < dimM; ii++) 
+  { 
+    comp(ii) = x(dimU + ii) * zl(ii) - mu;
+  }
+  E3 = comp.Normlinf();
+
+  double ll1, zl1;
+  zl1 = zl.Norml1() / double(dimC + dimM);
+  ll1 =  l.Norml1();
+  sc = max(sMax, zl1 / (double(dimM)) ) / sMax;
+  sd = max(sMax, (ll1 + zl1) / (double(dimC + dimM))) / sMax;
+  if(print)
+  {
+    mfem::out << "evaluating optimality error for mu = " << mu << endl;
+    mfem::out << "stationarity measure = "    << E1 / sd << endl;
+    mfem::out << "feasibility measure  = "    << E2      << endl;
+    mfem::out << "complimentarity measure = " << E3 / sc << endl;
+  }
+  return max(max(E1 / sd, E2), E3 / sc);
+}
+
+double InteriorPointSolver::E(const BlockVector &x, const Vector &l, const Vector &zl, bool print)
+{
+  return E(x, l, zl, 0.0, print);
+}
+
+double InteriorPointSolver::theta(const BlockVector &x)
+{
+  Vector cx(dimC); cx = 0.0;
+  optProblem->c(x, cx);
+  return cx.Norml2();
+}
+
+// log-barrier objective
+double InteriorPointSolver::phi(const BlockVector &x, double mu)
+{
+  double fx = optProblem->CalcObjective(x); 
+  double logBarrierLoc = 0.0;
+  for(int i = 0; i < dimM; i++) 
+  { 
+    logBarrierLoc += log(x(dimU+i)-ml(i));
+  }
+  double logBarrierGlb = 0.0;
+  logBarrierGlb = logBarrierLoc;
+  return fx - mu * logBarrierGlb;
+}
+
+// gradient of log-barrier objective with respect to x = (u, m)
+void InteriorPointSolver::Dxphi(const BlockVector &x, double mu, BlockVector &y)
+{
+  optProblem->CalcObjectiveGrad(x, y);
+  for(int i = 0; i < dimM; i++) 
+  { 
+    y(dimU + i) -= mu / (x(dimU + i));
+  } 
+}
+
+// Lagrangian function evaluation
+// L(x, l, zl) = f(x) + l^T c(x) - zl^T m
+double InteriorPointSolver::L(const BlockVector &x, const Vector &l, const Vector &zl)
+{
+  double fx = optProblem->CalcObjective(x);
+  Vector cx(dimC); optProblem->c(x, cx);
+  return (fx + InnerProduct(cx, l) - InnerProduct(x.GetBlock(1), zl));
+}
+
+void InteriorPointSolver::DxL(const BlockVector &x, const Vector &l, const Vector &zl, BlockVector &y)
+{
+  // evaluate the gradient of the objective with respect to the primal variables x = (u, m)
+  BlockVector gradxf(block_offsetsx); gradxf = 0.0;
+  optProblem->CalcObjectiveGrad(x, gradxf);
+  
+  SparseMatrix *Jacu, *Jacm, *JacuT, *JacmT;
+  Jacu = optProblem->Duc(x); Jacm = optProblem->Dmc(x);
+  JacuT = Transpose(*Jacu);
+  JacmT = Transpose(*Jacm);
+  JacuT->Mult(l, y.GetBlock(0));
+  JacmT->Mult(l, y.GetBlock(1));
+  delete JacuT; 
+  delete JacmT;
+  y.Add(1.0, gradxf);
+  (y.GetBlock(1)).Add(-1.0, zl);
+}
+
+
+bool InteriorPointSolver::GetConverged() const
+{
+  return converged;
+}
+
+void InteriorPointSolver::SetTol(double Tol)
+{
+  rel_tol = Tol;
+}
+
+void InteriorPointSolver::SetMaxIter(int max_it)
+{
+  max_iter = max_it;
+}
+
+void InteriorPointSolver::SetBarrierParameter(double mu_0)
+{
+  mu_k = mu_0;
+}
+
+void InteriorPointSolver::SaveLogBarrierHessianIterates(bool save)
+{
+   MFEM_ASSERT(MyRank == 0 || save == false, "currently can only save logbarrier hessian in serial codes");
+   saveLogBarrierIterates = save;
+}
+
+void InteriorPointSolver::SetLinearSolver(int LinSolver)
+{
+   linSolver = LinSolver;
+}
+
+void InteriorPointSolver::SetLinearSolveTol(double Tol)
+{
+   linSolveTol = Tol;
+}
+
+
+InteriorPointSolver::~InteriorPointSolver() 
+{
+   delete HLuucl;
+   delete JuT;
+   delete JmT; 
+   delete Wmm;
+
+   F1.DeleteAll();
+   F2.DeleteAll();
+   block_offsetsx.DeleteAll();
+   block_offsetsumlz.DeleteAll();
+   block_offsetsuml.DeleteAll();
+   ml.SetSize(0);
+}

miniapps/contact/ipsolver/IPsolver.hpp

@@ -0,0 +1,94 @@
+#include "mfem.hpp"
+#include "../problems/problems.hpp"
+#include <fstream>
+#include <iostream>
+
+using namespace std;
+using namespace mfem;
+
+
+#ifndef IPSOLVER 
+#define IPSOLVER
+
+class InteriorPointSolver
+{
+protected:
+    QPOptContactProblem * optProblem;
+    double rel_tol;
+    int  max_iter;
+    double mu_k; // \mu_k
+    Vector lk, zlk;
+
+    double sMax, kSig, tauMin, eta, thetaMin, delta, sTheta, sPhi, kMu, thetaMu;
+    double thetaMax, kSoc, gTheta, gPhi, kEps;
+	
+    // filter
+    Array<double> F1, F2;
+	
+    // quantities computed in lineSearch
+    double alpha, alphaz;
+    double thx0, thxtrial;
+    double phx0, phxtrial;
+    bool descentDirection, switchCondition, sufficientDecrease, lineSearchSuccess, inFilterRegion;
+    double Dxphi0_xhat;
+
+    int dimU, dimM, dimC;
+    Array<int> block_offsetsumlz, block_offsetsuml, block_offsetsx;
+    Vector ml;
+
+    Vector ckSoc;
+    SparseMatrix * Huu = nullptr; 
+    SparseMatrix * Hum = nullptr; 
+    SparseMatrix * Hmu = nullptr; 
+    SparseMatrix * Hmm = nullptr; 
+    SparseMatrix * Wmm = nullptr; 
+    SparseMatrix * Ju  = nullptr; 
+    SparseMatrix * Jm  = nullptr; 
+    SparseMatrix * JmT  = nullptr; 
+    SparseMatrix * JuT = nullptr; 
+    SparseMatrix * Huucl = nullptr;
+    SparseMatrix * HLuucl = nullptr;
+
+    int jOpt;
+    bool converged;
+    
+    int MyRank;
+    bool iAmRoot;
+
+    bool saveLogBarrierIterates;
+
+    int linSolver;
+    double linSolveTol;
+    Array<int> cgnum_iterations;
+
+public:
+    InteriorPointSolver(QPOptContactProblem*);
+    void Mult(const BlockVector& , BlockVector&); // used when the user wants to be aware of bound-constrained variable m >= ml
+    void Mult(const Vector&, Vector &); // useful when the user doesn't need to know about bound-constrained variable   m >= ml, e.g., when m is a slack variable
+    double MaxStepSize(Vector& , Vector& , Vector& , double);
+    double MaxStepSize(Vector& , Vector& , double);
+    void FormIPNewtonMat(BlockVector& , Vector& , Vector& , BlockOperator &);
+    void IPNewtonSolve(BlockVector& , Vector& , Vector& , Vector&, BlockVector& , double, bool);
+    void lineSearch(BlockVector& , BlockVector& , double);
+    void projectZ(const Vector & , Vector &, double);
+    void filterCheck(double, double);
+    double E(const BlockVector &, const Vector &, const Vector &, double, bool);
+    double E(const BlockVector &, const Vector &, const Vector &, bool);
+    Array<int> & GetCGIterNumbers() {return cgnum_iterations;}
+    bool GetConverged() const;
+    // TO DO: include Hessian of Lagrangian
+    double theta(const BlockVector &);
+    double phi(const BlockVector &, double);
+    void Dxphi(const BlockVector &, double, BlockVector &);
+    double L(const BlockVector &, const Vector &, const Vector &);
+    void DxL(const BlockVector &, const Vector &, const Vector &, BlockVector &);
+    void SetTol(double);
+    void SetMaxIter(int);
+    void SetBarrierParameter(double);    
+    void SaveLogBarrierHessianIterates(bool);
+    void SetLinearSolver(int);
+    void SetLinearSolveTol(double);
+    virtual ~InteriorPointSolver();
+};
+
+#endif

miniapps/contact/ipsolver/ParIPsolver.cpp

@@ -0,0 +1,864 @@
+#include "mfem.hpp"
+#include "ParIPsolver.hpp"
+#include <fstream>
+#include <iostream>
+#include <cstdlib>
+
+using namespace std;
+using namespace mfem;
+
+
+ParInteriorPointSolver::ParInteriorPointSolver(QPOptParContactProblem * problem_) 
+                     : problem(problem_)
+{
+   OptTol  = 1.e-2;
+   max_iter = 20;
+   mu_k     = 1.0;
+
+   sMax     = 1.e2;
+   kSig     = 1.e10;   // control deviation from primal Hessian
+   tauMin   = 0.8;     // control rate at which iterates can approach the boundary
+   eta      = 1.e-4;   // backtracking constant
+   thetaMin = 1.e-4;   // allowed violation of the equality constraints
+
+   // constants in line-step A-5.4
+   delta    = 1.0;
+   sTheta   = 1.1;
+   sPhi     = 2.3;
+
+   // control the rate at which the penalty parameter is decreased
+   kMu     = 0.2;
+   thetaMu = 1.5;
+
+   thetaMax = 1.e6; // maximum constraint violation
+   // data for the second order correction
+   kSoc     = 0.99;
+
+   // equation (18)
+   gTheta = 1.e-5;
+   gPhi   = 1.e-5;
+
+   kEps   = 1.e1;
+
+   dimU = problem->GetDimU();
+   dimM = problem->GetDimM();
+   dimC = problem->GetDimC();
+
+   MPI_Allreduce(&dimU,&gdimU,1,MPI_INT,MPI_SUM,problem->GetComm());
+   MPI_Allreduce(&dimM,&gdimM,1,MPI_INT,MPI_SUM,problem->GetComm());
+   MPI_Allreduce(&dimC,&gdimC,1,MPI_INT,MPI_SUM,problem->GetComm());
+
+   ckSoc.SetSize(dimC);
+
+   block_offsetsumlz.SetSize(5);
+   block_offsetsuml.SetSize(4);
+   block_offsetsx.SetSize(3);
+  
+   block_offsetsumlz[0] = 0;
+   block_offsetsumlz[1] = dimU; // u
+   block_offsetsumlz[2] = dimM; // m
+   block_offsetsumlz[3] = dimC; // lambda
+   block_offsetsumlz[4] = dimM; // zl
+   block_offsetsumlz.PartialSum();
+
+   for(int i = 0; i < block_offsetsuml.Size(); i++)  
+   { 
+      block_offsetsuml[i] = block_offsetsumlz[i]; 
+   }
+   for(int i = 0; i < block_offsetsx.Size(); i++)    
+   { 
+      block_offsetsx[i] = block_offsetsuml[i] ; 
+   }
+
+   ml = problem->Getml();
+  
+   lk.SetSize(dimC);  lk  = 0.0;
+   zlk.SetSize(dimM); zlk = 0.0;
+
+   linSolver = 0;
+   linSolveTol = 1.e-8;
+   MyRank = Mpi::WorldRank();
+   iAmRoot = MyRank == 0 ? true : false;
+}
+
+double ParInteriorPointSolver::MaxStepSize(Vector &x, Vector &xl, Vector &xhat, double tau)
+{
+   double alphaMaxloc = 1.0;
+   double alphaTmp;
+   for(int i = 0; i < x.Size(); i++)
+   {   
+      if( xhat(i) < 0. )
+      {
+         alphaTmp = -1. * tau * (x(i) - xl(i)) / xhat(i);
+         alphaMaxloc = min(alphaMaxloc, alphaTmp);
+      } 
+   }
+
+   // alphaMaxloc is the local maximum step size which is
+   // distinct on each MPI process. Need to compute
+   // the global maximum step size 
+   double alphaMaxglb;
+   MPI_Allreduce(&alphaMaxloc, &alphaMaxglb, 1, MPI_DOUBLE, MPI_MIN, MPI_COMM_WORLD);
+   return alphaMaxglb;
+}
+
+double ParInteriorPointSolver::MaxStepSize(Vector &x, Vector &xhat, double tau)
+{
+   Vector zero(x.Size()); zero = 0.0;
+   return MaxStepSize(x, zero, xhat, tau);
+}
+
+
+void ParInteriorPointSolver::Mult(const Vector &x0, Vector &xf)
+{
+   BlockVector x0block(block_offsetsx); x0block = 0.0;
+   x0block.GetBlock(0).Set(1.0, x0);
+   x0block.GetBlock(1) = 1.0;
+   x0block.GetBlock(1).Add(1.0, ml);
+   BlockVector xfblock(block_offsetsx); xfblock = 0.0;
+
+   Mult(x0block, xfblock);
+   xf.Set(1.0, xfblock.GetBlock(0));
+}
+
+
+void ParInteriorPointSolver::Mult(const BlockVector &x0, BlockVector &xf)
+{
+   converged = false;
+   
+   BlockVector xk(block_offsetsx), xhat(block_offsetsx); xk = 0; xhat = 0.0;
+   BlockVector Xk(block_offsetsumlz), Xhat(block_offsetsumlz); Xk = 0.0; Xhat = 0.0;
+   BlockVector Xhatuml(block_offsetsuml); Xhatuml = 0.0;
+   Vector zlhat(dimM); zlhat = 0.0;
+
+   xk.GetBlock(0).Set(1.0, x0.GetBlock(0));
+   xk.GetBlock(1).Set(1.0, x0.GetBlock(1));
+   // running estimate of the final values of the Lagrange multipliers
+   lk  = 0.0;
+   zlk = 0.0;
+
+   for(int i = 0; i < dimM; i++)
+   {
+      zlk(i) = 1.e1 * mu_k / (xk(i+dimU) - ml(i));
+   }
+
+   Xk.GetBlock(0).Set(1.0, xk.GetBlock(0));
+   Xk.GetBlock(1).Set(1.0, xk.GetBlock(1));
+   Xk.GetBlock(2).Set(1.0, lk);
+   Xk.GetBlock(3).Set(1.0, zlk);
+
+  /* set theta0 = theta(x0)
+   *     thetaMin
+   *     thetaMax
+   * when theta(xk) < thetaMin and the switching condition holds
+   * then we ask for the Armijo sufficient decrease of the barrier
+   * objective to be satisfied, in order to accept the trial step length alphakl
+   * 
+   * thetaMax controls how the filter is initialized for each log-barrier subproblem
+   * F0 = {(th, phi) s.t. th > thetaMax}
+   * that is the filter does not allow for iterates where the constraint violation
+   * is larger than that of thetaMax
+   */
+   double theta0 = theta(xk);
+   thetaMin = 1.e-4 * max(1.0, theta0);
+   thetaMax = 1.e8  * thetaMin; // 1.e4 * max(1.0, theta0)
+
+   double Eeval, maxBarrierSolves, Eevalmu0;
+   bool printOptimalityError; // control optimality error print to console for log-barrier subproblems
+   
+   maxBarrierSolves = 10;
+
+   for(jOpt = 0; jOpt < max_iter; jOpt++)
+   {
+      if(iAmRoot)
+      {
+         cout << "interior-point solve step " << jOpt << endl;
+      }
+      // A-2. Check convergence of overall optimization problem
+      printOptimalityError = false;
+      Eevalmu0 = E(xk, lk, zlk, printOptimalityError);
+      if(Eevalmu0 < OptTol)
+      {
+         converged = true;
+         if(iAmRoot)
+         {
+            cout << "solved optimization problem :)\n";
+         }
+         break;
+      }
+      
+      if(jOpt > 0) { maxBarrierSolves = 1; }
+      
+      for(int i = 0; i < maxBarrierSolves; i++)
+      {
+         // A-3. Check convergence of the barrier subproblem
+         printOptimalityError = true;
+         Eeval = E(xk, lk, zlk, mu_k, printOptimalityError);
+         if(iAmRoot)
+         {
+            cout << "E = " << Eeval << endl;
+         }
+         if(Eeval < kEps * mu_k)
+         {
+            if(iAmRoot)
+            {
+               cout << "solved barrier subproblem :), for mu = " << mu_k << endl;
+            }
+            // A-3.1. Recompute the barrier parameter
+            mu_k  = max(OptTol / 10., min(kMu * mu_k, pow(mu_k, thetaMu)));
+            // A-3.2. Re-initialize the filter
+            F1.DeleteAll();
+            F2.DeleteAll();
+         }
+         else
+         {
+            break;
+         }
+      }
+    
+      // A-4. Compute the search direction
+      // solve for (uhat, mhat, lhat)
+      if(iAmRoot)
+      {
+         cout << "\n** A-4. IP-Newton solve **\n";
+      }
+      zlhat = 0.0; Xhatuml = 0.0;
+      // why do we have Xhatuml ....???
+      // TO DO: remove Xhatuml in favor of passing Xhat
+      IPNewtonSolve(xk, lk, zlk, zlhat, Xhatuml, mu_k, false); 
+
+      // assign data stack, X = (u, m, l, zl)
+      Xk = 0.0;
+      Xk.GetBlock(0).Set(1.0, xk.GetBlock(0));
+      Xk.GetBlock(1).Set(1.0, xk.GetBlock(1));
+      Xk.GetBlock(2).Set(1.0, lk);
+      Xk.GetBlock(3).Set(1.0, zlk);
+
+      // assign data stack, Xhat = (uhat, mhat, lhat, zlhat)
+      Xhat = 0.0;
+      for(int i = 0; i < 3; i++)
+      {
+         Xhat.GetBlock(i).Set(1.0, Xhatuml.GetBlock(i));
+      }
+      Xhat.GetBlock(3).Set(1.0, zlhat);
+
+      // A-5. Backtracking line search.
+      if(iAmRoot)
+      {
+         cout << "\n** A-5. Linesearch **\n";
+         cout << "mu = " << mu_k << endl;
+      }
+      lineSearch(Xk, Xhat, mu_k);
+
+      if(lineSearchSuccess)
+      {
+         if(iAmRoot)
+         {
+            cout << "lineSearch successful :)\n";
+         }
+         if(!switchCondition || !sufficientDecrease)
+         {
+            F1.Append( (1. - gTheta) * thx0);
+            F2.Append( phx0 - gPhi * thx0);
+         }
+         // ----- A-6: Accept the trial point
+         // print info regarding zl...
+         xk.GetBlock(0).Add(alpha, Xhat.GetBlock(0));
+         xk.GetBlock(1).Add(alpha, Xhat.GetBlock(1));
+         lk.Add(alpha,   Xhat.GetBlock(2));
+         zlk.Add(alphaz, Xhat.GetBlock(3));
+         projectZ(xk, zlk, mu_k);
+      }
+      else
+      {
+         if(iAmRoot)
+         {
+            cout << "lineSearch not successful :(\n";
+            cout << "attempting feasibility restoration with theta = " << thx0 << endl;
+            cout << "no feasibility restoration implemented, exiting now \n";
+         }
+         break;
+      }
+      if(jOpt + 1 == max_iter && iAmRoot) 
+      {  
+         cout << "maximum optimization iterations :(\n";
+      }
+   }
+   // done with optimization routine, just reassign data to xf reference so
+   // that the application code has access to the optimal point
+   xf = 0.0;
+   xf.GetBlock(0).Set(1.0, xk.GetBlock(0));
+   xf.GetBlock(1).Set(1.0, xk.GetBlock(1));
+}
+
+void ParInteriorPointSolver::FormIPNewtonMat(BlockVector & x, Vector & l, Vector &zl, 
+                                             BlockOperator &Ak)
+{
+   // WARNING: Huu, Hum, Hmu, Hmm should all be Hessian terms of the Lagrangian, currently we 
+   //          them by Hessian terms of the objective function and neglect the Hessian of l^T c
+
+   Huu = problem->Duuf(x); 
+   Hum = problem->Dumf(x);
+   Hmu = problem->Dmuf(x);
+   Hmm = problem->Dmmf(x);
+
+   Vector DiagLogBar(dimM); DiagLogBar = 0.0;
+   for(int ii = 0; ii < dimM; ii++)
+   {
+      DiagLogBar(ii) = zl(ii) / (x(ii+dimU) - ml(ii));
+   }
+   if(saveLogBarrierIterates)
+   {
+      std::ofstream diagStream;
+      char diagString[100];
+      snprintf(diagString, 100, "logBarrierHessiandata/D%d.dat", jOpt);
+      diagStream.open(diagString, ios::out | ios::trunc);
+      for(int ii = 0; ii < dimM; ii++)
+      {
+         diagStream << setprecision(30) << DiagLogBar(ii) << endl;
+         // mfem::out << DiagLogBar(ii) << endl;
+      }
+      diagStream.close();
+   } 
+
+   
+   int gsize = problem->GetGlobalNumConstraints();
+   int * rows = problem->GetConstraintsStarts();
+
+   delete Wmm;
+   if(Hmm != nullptr)
+   {
+      SparseMatrix * Ds = new SparseMatrix(DiagLogBar);
+      HypreParMatrix * D = new HypreParMatrix(problem->GetComm(), gsize, rows, Ds);
+      HypreStealOwnership(*D,*Ds);
+      delete Ds;
+      Wmm = ParAdd(Hmm,D);
+      delete D;
+   }
+   else
+   {
+      SparseMatrix * Ds = new SparseMatrix(DiagLogBar);
+      Wmm = new HypreParMatrix(problem->GetComm(), gsize, rows, Ds);
+      HypreStealOwnership(*Wmm,*Ds);
+      delete Ds;
+   }
+
+   delete JuT;
+   delete JmT;
+   Ju = problem->Duc(x); JuT = Ju->Transpose();
+   Jm = problem->Dmc(x); JmT = Jm->Transpose();
+
+   //         IP-Newton system matrix
+   //    Ak = [[H_(u,u)  H_(u,m)   J_u^T]
+   //          [H_(m,u)  W_(m,m)   J_m^T]
+   //          [ J_u      J_m       0  ]]
+
+   Ak.SetBlock(0, 0, Huu);                         Ak.SetBlock(0, 2, JuT);
+                           Ak.SetBlock(1, 1, Wmm); Ak.SetBlock(1, 2, JmT);
+   Ak.SetBlock(2, 0,  Ju); Ak.SetBlock(2, 1,  Jm);
+   if(Hum != nullptr) { Ak.SetBlock(0, 1, Hum); Ak.SetBlock(1, 0, Hmu); }
+}
+
+// perturbed KKT system solve
+// determine the search direction
+void ParInteriorPointSolver::IPNewtonSolve(BlockVector &x, Vector &l, Vector &zl, Vector &zlhat, BlockVector &Xhat, double mu, bool socSolve)
+{
+   // solve A x = b, where A is the IP-Newton matrix
+   BlockOperator A(block_offsetsuml, block_offsetsuml); 
+   BlockVector b(block_offsetsuml); b = 0.0;
+   FormIPNewtonMat(x, l, zl, A);
+
+   //       [grad_u phi + Ju^T l]
+   // b = - [grad_m phi + Jm^T l]
+   //       [          c        ]
+   BlockVector gradphi(block_offsetsx); gradphi = 0.0;
+   BlockVector JTl(block_offsetsx); JTl = 0.0;
+   Dxphi(x, mu, gradphi);
+   
+   (A.GetBlock(0,2)).Mult(l, JTl.GetBlock(0));
+   (A.GetBlock(1,2)).Mult(l, JTl.GetBlock(1));
+
+   for(int ii = 0; ii < 2; ii++)
+   {
+      b.GetBlock(ii).Set(1.0, gradphi.GetBlock(ii));
+      b.GetBlock(ii).Add(1.0, JTl.GetBlock(ii));
+   }
+   if(!socSolve) 
+   {
+      problem->c(x, b.GetBlock(2));
+   }
+   else
+   {
+      b.GetBlock(2).Set(1.0, ckSoc);
+   }
+   b *= -1.0; 
+   Xhat = 0.0;
+
+   // Direct solver (default)
+   if(linSolver == 0)
+   {
+      Array2D<HypreParMatrix *> ABlockMatrix(3,3);
+      for(int ii = 0; ii < 3; ii++)
+      {
+         for(int jj = 0; jj < 3; jj++)
+         {
+            if(!A.IsZeroBlock(ii, jj))
+            {
+               ABlockMatrix(ii, jj) = dynamic_cast<HypreParMatrix *>(&(A.GetBlock(ii, jj)));
+	         }
+	         else
+	         {
+	            ABlockMatrix(ii, jj) = nullptr;
+	         }
+         }  
+      }
+      
+      HypreParMatrix * Ah = HypreParMatrixFromBlocks(ABlockMatrix);   
+
+      /* direct solve of the 3x3 IP-Newton linear system */
+#ifdef MFEM_USE_MUMPS
+      MUMPSSolver ASolver(*Ah);;
+      ASolver.SetPrintLevel(0);
+      ASolver.SetMatrixSymType(MUMPSSolver::MatType::UNSYMMETRIC);
+      ASolver.Mult(b, Xhat);
+#else 
+#ifdef MFEM_USE_MKL_CPARDISO
+      CPardisoSolver ASolver(MPI_COMM_WORLD);
+      ASolver.SetOperator(*Ah);
+      ASolver.Mult(b, Xhat);
+#else
+  	   MFEM_VERIFY(false, "linSolver 0 will not work unless compiled with MUMPS or MKL");
+#endif
+#endif
+
+      delete Ah;
+   }
+   else if(linSolver == 1 || linSolver == 2)
+   {
+      // form A = Huu + Ju^T D Ju, Wmm = D for contact
+      HypreParMatrix * Wmmloc = dynamic_cast<HypreParMatrix *>(&(A.GetBlock(1, 1)));
+      HypreParMatrix * Huuloc = dynamic_cast<HypreParMatrix *>(&(A.GetBlock(0, 0)));
+      HypreParMatrix * Juloc  = dynamic_cast<HypreParMatrix *>(&(A.GetBlock(2, 0)));
+      HypreParMatrix * JuTloc = dynamic_cast<HypreParMatrix *>(&(A.GetBlock(0, 2)));
+      HypreParMatrix *JuTDJu   = RAP(Wmmloc, Juloc);     // Ju^T D Ju
+      HypreParMatrix *Areduced = ParAdd(Huuloc, JuTDJu);  // Huu + Ju^T D Ju
+
+      Areduced->DropSmallEntries(1e-16);
+
+      /* prepare the reduced rhs */
+      // breduced = bu + Ju^T (bm + Wmm bl)
+      Vector breduced(dimU); breduced = 0.0;
+      Vector tempVec(dimM); tempVec = 0.0;
+      Wmmloc->Mult(b.GetBlock(2), tempVec);
+      tempVec.Add(1.0, b.GetBlock(1));
+      JuTloc->Mult(tempVec, breduced);
+      breduced.Add(1.0, b.GetBlock(0));
+      
+      if(linSolver == 1)
+      {
+         // setup the solver for the reduced linear system
+#ifdef MFEM_USE_MUMPS
+	      MUMPSSolver AreducedSolver(*Areduced);   
+         AreducedSolver.SetPrintLevel(0);
+         AreducedSolver.SetMatrixSymType(MUMPSSolver::MatType::SYMMETRIC_INDEFINITE);
+	      AreducedSolver.Mult(breduced, Xhat.GetBlock(0));
+#else 
+#ifdef MFEM_USE_MKL_CPARDISO
+         CPardisoSolver AreducedSolver(MPI_COMM_WORLD);
+	      AreducedSolver.SetOperator(*Areduced);
+	      AreducedSolver.Mult(breduced, Xhat.GetBlock(0));
+#else
+         MFEM_VERIFY(false, "linSolver 1 will not work unless compiled with MUMPS or MKL");
+#endif
+#endif
+      }
+      else
+      {
+         HypreBoomerAMG amg(*Areduced);
+         amg.SetPrintLevel(0);
+         if (pfes)
+         {
+            amg.SetElasticityOptions(pfes);
+         }
+         else
+         {
+            amg.SetSystemsOptions(3,false);
+         }
+         amg.SetRelaxType(relax_type);
+         int n;
+
+
+         // CGSolver AreducedSolver(MPI_COMM_WORLD);
+	      // AreducedSolver.SetOperator(*Areduced);
+         // AreducedSolver.SetRelTol(linSolveTol);
+         // AreducedSolver.SetMaxIter(1000);
+         // AreducedSolver.SetPreconditioner(amg);
+	      // AreducedSolver.SetPrintLevel(3);
+         // AreducedSolver.Mult(breduced, Xhat.GetBlock(0));
+         // n = AreducedSolver.GetNumIterations();
+
+         HyprePCG AreducedSolver(*Areduced);
+         AreducedSolver.SetTol(linSolveTol);
+         AreducedSolver.SetMaxIter(1000);
+         AreducedSolver.SetPreconditioner(amg);
+	      AreducedSolver.SetPrintLevel(2);
+         // AreducedSolver.SetResidualConvergenceOptions();
+         AreducedSolver.Mult(breduced, Xhat.GetBlock(0));
+         AreducedSolver.GetNumIterations(n);
+
+         cgnum_iterations.Append(n);
+
+
+      }
+
+      // now propagate solved uhat to obtain mhat and lhat
+      // xm = Ju xu - bl
+      Juloc->Mult(Xhat.GetBlock(0), Xhat.GetBlock(1));
+      Xhat.GetBlock(1).Add(-1.0, b.GetBlock(2));
+
+      // xl = Wmm xm - bm
+      Wmmloc->Mult(Xhat.GetBlock(1), Xhat.GetBlock(2));
+      Xhat.GetBlock(2).Add(-1.0, b.GetBlock(1));
+
+      delete JuTDJu;
+      delete Areduced;
+   }
+
+   /* backsolve to determine zlhat */
+   for(int ii = 0; ii < dimM; ii++)
+   {
+      zlhat(ii) = -1.*(zl(ii) + (zl(ii) * Xhat(ii + dimU) - mu) / (x(ii + dimU) - ml(ii)) );
+   }
+}
+
+// here Xhat, X will be BlockVectors w.r.t. the 4 partitioning X = (u, m, l, zl)
+
+void ParInteriorPointSolver::lineSearch(BlockVector& X0, BlockVector& Xhat, double mu)
+{
+   double tau  = max(tauMin, 1.0 - mu);
+   Vector u0   = X0.GetBlock(0);
+   Vector m0   = X0.GetBlock(1);
+   Vector l0   = X0.GetBlock(2);
+   Vector z0   = X0.GetBlock(3);
+   Vector uhat = Xhat.GetBlock(0);
+   Vector mhat = Xhat.GetBlock(1);
+   Vector lhat = Xhat.GetBlock(2);
+   Vector zhat = Xhat.GetBlock(3);
+   double alphaMax  = MaxStepSize(m0, ml, mhat, tau);
+   double alphaMaxz = MaxStepSize(z0, zhat, tau);
+   alphaz = alphaMaxz;
+
+   BlockVector x0(block_offsetsx); x0 = 0.0;
+   x0.GetBlock(0).Set(1.0, u0);
+   x0.GetBlock(1).Set(1.0, m0);
+   
+   BlockVector xhat(block_offsetsx); xhat = 0.0;
+   xhat.GetBlock(0).Set(1.0, uhat);
+   xhat.GetBlock(1).Set(1.0, mhat);
+   
+   BlockVector xtrial(block_offsetsx); xtrial = 0.0;
+   BlockVector Dxphi0(block_offsetsx); Dxphi0 = 0.0;
+   int maxBacktrack = 20;
+   alpha = alphaMax;
+
+   Vector ck0(dimC); ck0 = 0.0;
+   Vector zhatsoc(dimM); zhatsoc = 0.0;
+   BlockVector Xhatumlsoc(block_offsetsuml); Xhatumlsoc = 0.0;
+   BlockVector xhatsoc(block_offsetsx); xhatsoc = 0.0;
+   Vector uhatsoc(dimU); uhatsoc = 0.0;
+   Vector mhatsoc(dimM); mhatsoc = 0.0;
+
+   Dxphi(x0, mu, Dxphi0);
+
+   Dxphi0_xhat = InnerProduct(MPI_COMM_WORLD, Dxphi0, xhat);
+   descentDirection = Dxphi0_xhat < 0. ? true : false;
+   
+   
+   if (iAmRoot)
+   {
+      if(descentDirection)
+      {
+         cout << "is a descent direction for the log-barrier objective\n";
+      }
+      else
+      {
+         cout << "is not a descent direction for the log-barrier objective\n";
+      }
+   }
+
+   thx0 = theta(x0);
+   phx0 = phi(x0, mu);
+
+   lineSearchSuccess = false;
+   for(int i = 0; i < maxBacktrack; i++)
+   {
+      if (iAmRoot)
+      {
+         cout << "\n--------- alpha = " << alpha << " ---------\n";
+      }
+      // ----- A-5.2. Compute trial point: xtrial = x0 + alpha_i xhat
+      xtrial.Set(1.0, x0);
+      xtrial.Add(alpha, xhat);
+
+      // ------ A-5.3. if not in filter region go to A.5.4 otherwise go to A-5.5.
+      thxtrial = theta(xtrial);
+      phxtrial = phi(xtrial, mu);
+      filterCheck(thxtrial, phxtrial);    
+      if(!inFilterRegion)
+      {
+         if (iAmRoot)
+         {
+            cout << "not in filter region :)\n";
+         }
+         // ------ A.5.4: Check sufficient decrease
+         if(!descentDirection)
+         {
+            switchCondition = false;
+         }
+         else
+         {
+            switchCondition = (alpha * pow(abs(Dxphi0_xhat), sPhi) > delta * pow(thx0, sTheta)) ? true : false;
+         }
+         if (iAmRoot)
+         {
+            cout << "theta(x0) = "     << thx0     << ", thetaMin = "                  << thetaMin             << endl;
+            cout << "theta(xtrial) = " << thxtrial << ", (1-gTheta) *theta(x0) = "     << (1. - gTheta) * thx0 << endl;
+            cout << "phi(xtrial) = "   << phxtrial << ", phi(x0) - gPhi *theta(x0) = " << phx0 - gPhi * thx0   << endl;
+         }      
+         // Case I      
+         if(thx0 <= thetaMin && switchCondition)
+         {
+            sufficientDecrease = (phxtrial <= phx0 + eta * alpha * Dxphi0_xhat) ? true : false;
+            if(sufficientDecrease)
+            {
+               if(iAmRoot) { cout << "Line search successful: sufficient decrease in log-barrier objective.\n"; } 
+               // accept the trial step
+               lineSearchSuccess = true;
+               break;
+            }
+         }
+         else
+         {
+            if(thxtrial <= (1. - gTheta) * thx0 || phxtrial <= phx0 - gPhi * thx0)
+            {
+               if(iAmRoot) { cout << "Line search successful: infeasibility or log-barrier objective decreased.\n"; } 
+               // accept the trial step
+               lineSearchSuccess = true;
+               break;
+            }
+         }
+         // A-5.5: Initialize the second-order correction
+         if((!(thx0 < thxtrial)) && i == 0)
+         {
+            if (iAmRoot)
+            {
+               cout << "second order correction\n";
+            }
+            problem->c(xtrial, ckSoc);
+            problem->c(x0, ck0);
+            ckSoc.Add(alphaMax, ck0);
+            // A-5.6 Compute the second-order correction.
+            IPNewtonSolve(x0, l0, z0, zhatsoc, Xhatumlsoc, mu, true);
+            mhatsoc.Set(1.0, Xhatumlsoc.GetBlock(1));
+            //WARNING: not complete but currently solver isn't entering this region
+         }
+      }
+      else
+      {
+         if (iAmRoot)
+         {
+            cout << "in filter region :(\n"; 
+         }
+      }
+      // include more if needed
+      alpha *= 0.5;
+
+   } 
+}
+
+
+void ParInteriorPointSolver::projectZ(const Vector &x, Vector &z, double mu)
+{
+   double zi;
+   double mudivmml;
+   for(int i = 0; i < dimM; i++)
+   {
+      zi = z(i);
+      mudivmml = mu / (x(i + dimU) - ml(i));
+      z(i) = max(min(zi, kSig * mudivmml), mudivmml / kSig);
+   }
+}
+
+void ParInteriorPointSolver::filterCheck(double th, double ph)
+{
+   inFilterRegion = false;
+   if(th > thetaMax)
+   {
+      inFilterRegion = true;
+   }
+   else
+   {
+      for(int i = 0; i < F1.Size(); i++)
+      {
+         if(th >= F1[i] && ph >= F2[i])
+         {
+            inFilterRegion = true;
+            break;
+         }
+      }
+   }
+}
+
+double ParInteriorPointSolver::E(const BlockVector &x, const Vector &l, const Vector &zl, double mu, bool printEeval)
+{
+   double E1, E2, E3;
+   double sc, sd;
+   BlockVector gradL(block_offsetsx); gradL = 0.0; // stationarity grad L = grad f + J^T l - z
+   Vector cx(dimC); cx = 0.0;     // feasibility c = c(x)
+   Vector comp(dimM); comp = 0.0; // complementarity M Z - mu 1
+
+   DxL(x, l, zl, gradL);
+   E1 = GlobalLpNorm(infinity(), gradL.Normlinf(), MPI_COMM_WORLD); 
+
+   problem->c(x, cx);
+   E2 = GlobalLpNorm(infinity(), cx.Normlinf(), MPI_COMM_WORLD); 
+
+
+   for(int ii = 0; ii < dimM; ii++) 
+   { 
+      comp(ii) = x(dimU + ii) * zl(ii) - mu;
+   }
+   E3 = GlobalLpNorm(infinity(), comp.Normlinf(), MPI_COMM_WORLD); 
+
+   double ll1, zl1;
+
+   zl1 = GlobalLpNorm(1, zl.Norml1(), MPI_COMM_WORLD)/ double(gdimC + gdimM);; 
+   ll1 = GlobalLpNorm(1, l.Norml1(), MPI_COMM_WORLD);
+   sc = max(sMax, zl1 / (double(gdimM)) ) / sMax;
+   sd = max(sMax, (ll1 + zl1) / (double(gdimC + gdimM))) / sMax;
+   if(iAmRoot && printEeval)
+   {
+      cout << "evaluating optimality error for mu = " << mu << endl;
+      cout << "stationarity measure = "    << E1 / sd << endl;
+      cout << "feasibility measure  = "    << E2      << endl;
+      cout << "complimentarity measure = " << E3 / sc << endl;
+   }
+   return max(max(E1 / sd, E2), E3 / sc);
+}
+
+double ParInteriorPointSolver::E(const BlockVector &x, const Vector &l, const Vector &zl, bool printEeval)
+{
+  return E(x, l, zl, 0.0, printEeval);
+}
+
+double ParInteriorPointSolver::theta(const BlockVector &x)
+{
+   Vector cx(dimC);
+   problem->c(x, cx);
+   return sqrt(InnerProduct(MPI_COMM_WORLD,cx, cx));
+}
+
+// log-barrier objective
+double ParInteriorPointSolver::phi(const BlockVector &x, double mu)
+{
+   double fx = problem->CalcObjective(x); 
+   double logBarrierLoc = 0.0;
+   for(int i = 0; i < dimM; i++) 
+   { 
+     logBarrierLoc += log(x(dimU+i)-ml(i));
+   }
+   double logBarrierGlb;
+   MPI_Allreduce(&logBarrierLoc, &logBarrierGlb, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+   return fx - mu * logBarrierGlb;
+}
+
+// gradient of log-barrier objective with respect to x = (u, m)
+void ParInteriorPointSolver::Dxphi(const BlockVector &x, double mu, BlockVector &y)
+{
+   problem->CalcObjectiveGrad(x, y);
+   
+   for(int i = 0; i < dimM; i++) 
+   { 
+      y(dimU + i) -= mu / (x(dimU + i));
+   } 
+}
+
+// Lagrangian function evaluation
+// L(x, l, zl) = f(x) + l^T c(x) - zl^T m
+double ParInteriorPointSolver::L(const BlockVector &x, const Vector &l, const Vector &zl)
+{
+   double fx = problem->CalcObjective(x);
+   Vector cx(dimC); problem->c(x, cx);
+   return (fx + InnerProduct(MPI_COMM_WORLD,cx, l) - InnerProduct(MPI_COMM_WORLD, x.GetBlock(1), zl));
+}
+
+void ParInteriorPointSolver::DxL(const BlockVector &x, const Vector &l, const Vector &zl, BlockVector &y)
+{
+   // evaluate the gradient of the objective with respect to the primal variables x = (u, m)
+   BlockVector gradxf(block_offsetsx); gradxf = 0.0;
+   problem->CalcObjectiveGrad(x, gradxf);
+   
+   HypreParMatrix *Jacu, *Jacm, *JacuT, *JacmT;
+   Jacu = problem->Duc(x); 
+   Jacm = problem->Dmc(x);
+   JacuT = Jacu->Transpose();
+   JacmT = Jacm->Transpose();
+   
+   JacuT->Mult(l, y.GetBlock(0));
+   JacmT->Mult(l, y.GetBlock(1));
+   
+   delete JacuT;
+   delete JacmT;
+   
+   y.Add(1.0, gradxf);
+   (y.GetBlock(1)).Add(-1.0, zl);
+}
+
+bool ParInteriorPointSolver::GetConverged() const
+{
+   return converged;
+}
+
+void ParInteriorPointSolver::SetTol(double Tol)
+{
+   OptTol = Tol;
+}
+
+void ParInteriorPointSolver::SetMaxIter(int max_it)
+{
+   max_iter = max_it;
+}
+
+void ParInteriorPointSolver::SetBarrierParameter(double mu_0)
+{
+   mu_k = mu_0;
+}
+
+void ParInteriorPointSolver::SaveLogBarrierHessianIterates(bool save)
+{
+   MFEM_ASSERT(MyRank == 0 || save == false, "currently can only save logbarrier hessian in serial codes");
+   saveLogBarrierIterates = save;
+}
+
+void ParInteriorPointSolver::SetLinearSolver(int LinSolver)
+{
+   linSolver = LinSolver;
+}
+
+void ParInteriorPointSolver::SetLinearSolveTol(double Tol)
+{
+  linSolveTol = Tol;
+}
+
+void ParInteriorPointSolver::SetLinearSolveRelaxType(int relax_type_)
+{
+   relax_type = relax_type_;
+}
+
+
+ParInteriorPointSolver::~ParInteriorPointSolver() 
+{
+   delete JuT;
+   delete JmT;
+   delete Wmm;
+}

miniapps/contact/ipsolver/ParIPsolver.hpp

@@ -0,0 +1,100 @@
+#include "mfem.hpp"
+#include "../problems/parproblems.hpp"
+#include <fstream>
+#include <iostream>
+
+using namespace std;
+using namespace mfem;
+
+
+#ifndef PARIPSOLVER 
+#define PARIPSOLVER
+
+class ParInteriorPointSolver
+{
+protected:
+    QPOptParContactProblem* problem;
+    double OptTol;
+    int  max_iter;
+    double mu_k; // \mu_k
+    Vector lk, zlk;
+
+    double sMax, kSig, tauMin, eta, thetaMin, delta, sTheta, sPhi, kMu, thetaMu;
+    double thetaMax, kSoc, gTheta, gPhi, kEps;
+	
+    // filter
+    Array<double> F1, F2;
+	
+    // quantities computed in lineSearch
+    double alpha, alphaz;
+    double thx0, thxtrial;
+    double phx0, phxtrial;
+    bool descentDirection, switchCondition, sufficientDecrease, lineSearchSuccess, inFilterRegion;
+    double Dxphi0_xhat;
+
+    int dimU, dimM, dimC;
+    int gdimU, gdimM, gdimC;
+    Array<int> block_offsetsumlz, block_offsetsuml, block_offsetsx;
+    Vector ml;
+
+    Vector ckSoc;
+    HypreParMatrix * Huu = nullptr;
+    HypreParMatrix * Hum = nullptr;
+    HypreParMatrix * Hmu = nullptr;
+    HypreParMatrix * Hmm = nullptr;
+    HypreParMatrix * Wmm = nullptr;
+    HypreParMatrix * Ju = nullptr;
+    HypreParMatrix * Jm = nullptr;
+    HypreParMatrix * JuT = nullptr;
+    HypreParMatrix * JmT = nullptr;
+
+    Array<int> cgnum_iterations;
+    ParFiniteElementSpace *pfes = nullptr;
+    
+    int jOpt;
+    bool converged;
+    
+    int MyRank;
+    bool iAmRoot;
+
+    bool saveLogBarrierIterates = false;
+
+    int linSolver;
+    double linSolveTol;
+    int relax_type = 8;
+public:
+    ParInteriorPointSolver(QPOptParContactProblem*);
+    double MaxStepSize(Vector& , Vector& , Vector& , double);
+    double MaxStepSize(Vector& , Vector& , double);
+    void Mult(const BlockVector& , BlockVector&);
+    void Mult(const Vector&, Vector &); 
+    void FormIPNewtonMat(BlockVector& , Vector& , Vector& , BlockOperator &);
+    void IPNewtonSolve(BlockVector& , Vector& , Vector& , Vector&, BlockVector& , double, bool);
+    void lineSearch(BlockVector& , BlockVector& , double);
+    void projectZ(const Vector & , Vector &, double);
+    void filterCheck(double, double);
+    double E(const BlockVector &, const Vector &, const Vector &, double, bool);
+    double E(const BlockVector &, const Vector &, const Vector &, bool);
+    bool GetConverged() const;
+    Array<int> & GetCGIterNumbers() {return cgnum_iterations;}
+    // TO DO: include Hessian of Lagrangian
+    double theta(const BlockVector &);
+    double phi(const BlockVector &, double);
+    void Dxphi(const BlockVector &, double, BlockVector &);
+    double L(const BlockVector &, const Vector &, const Vector &);
+    void DxL(const BlockVector &, const Vector &, const Vector &, BlockVector &);
+    void SetTol(double);
+    void SetMaxIter(int);
+    void SetBarrierParameter(double);    
+    void SaveLogBarrierHessianIterates(bool);
+    void SetLinearSolver(int);
+    void SetLinearSolveTol(double);
+    void SetLinearSolveRelaxType(int);
+    void SetFiniteElementSpace(ParFiniteElementSpace * pfes_)
+    {
+        pfes = pfes_;
+    }
+    virtual ~ParInteriorPointSolver();
+};
+
+#endif

miniapps/contact/makefile

@@ -0,0 +1,110 @@
+# Copyright (c) 2010-2023, Lawrence Livermore National Security, LLC. Produced
+# at the Lawrence Livermore National Laboratory. All Rights reserved. See files
+# LICENSE and NOTICE for details. LLNL-CODE-806117.
+#
+# This file is part of the MFEM library. For more information and source code
+# availability visit https://mfem.org.
+#
+# MFEM is free software; you can redistribute it and/or modify it under the
+# terms of the BSD-3 license. We welcome feedback and contributions, see file
+# CONTRIBUTING.md for details.
+
+# Use the MFEM build directory
+MFEM_DIR ?= ../..
+MFEM_BUILD_DIR ?= ../..
+SRC = $(if $(MFEM_DIR:../..=),$(MFEM_DIR)/miniapps/contact/,)
+CONFIG_MK = $(MFEM_BUILD_DIR)/config/config.mk
+
+# Include defaults.mk to get XLINKER
+DEFAULTS_MK = $(MFEM_DIR)/config/defaults.mk
+include $(DEFAULTS_MK)
+
+MFEM_LIB_FILE = mfem_is_not_built
+-include $(CONFIG_MK)
+
+CONTACT_SEQ_SRC = problems/problems.cpp problems/problems_util.cpp util/util.cpp ipsolver/IPsolver.cpp
+CONTACT_SEC_OBJ = $(CONTACT_PAR_SRC:.cpp=.o)
+CONTACT_PAR_SRC = $(CONTACT_SEQ_SRC) ipsolver/ParIPsolver.cpp problems/parproblems.cpp problems/parproblems_util.cpp util/mpicomm.cpp 
+CONTACT_PAR_OBJ = $(CONTACT_PAR_SRC:.cpp=.o)
+
+CONTACT_SRC = contact_driver.cpp $(CONTACT_SEQ_SRC)
+CONTACT_OBJ = $(CONTACT_SRC:.cpp=.o)
+
+PCONTACT_SRC = pcontact_driver.cpp $(CONTACT_PAR_SRC)
+PCONTACT_OBJ = $(PCONTACT_SRC:.cpp=.o)
+
+SEQ_MINIAPPS = contact_driver
+PAR_MINIAPPS = pcontact_driver
+
+ifeq ($(MFEM_USE_MPI),NO)
+   MINIAPPS = $(SEQ_MINIAPPS)
+else
+   MINIAPPS = $(PAR_MINIAPPS) $(SEQ_MINIAPPS)
+endif
+
+COMMON_LIB = -L$(MFEM_BUILD_DIR)/miniapps/common -lmfem-common
+
+# If MFEM_SHARED is set, add the ../common rpath
+COMMON_LIB += $(if $(MFEM_SHARED:YES=),,\
+   $(if $(MFEM_USE_CUDA:YES=),$(CXX_XLINKER),$(CUDA_XLINKER))-rpath,$(abspath\
+   $(MFEM_BUILD_DIR)/miniapps/common))
+
+.SUFFIXES:
+.SUFFIXES: .o .cpp .mk
+.PHONY: all lib-common clean clean-build clean-exec
+
+# Remove built-in rule
+%: %.cpp
+%.o: %.cpp
+
+%.o: $(SRC)%.cpp $(wildcard $(SRC)%.hpp) $(MFEM_LIB_FILE)\
+ $(CONFIG_MK) | lib-common
+	$(MFEM_CXX) $(MFEM_FLAGS) -c $< -o $@
+
+util/%.o: $(SRC)util/%.cpp $(wildcard $(SRC)util/%.hpp) $(MFEM_LIB_FILE)\
+ $(CONFIG_MK) | lib-common
+	mkdir -p $(@D)
+	$(MFEM_CXX) $(MFEM_FLAGS) -c $< -o $@
+
+problems/%.o: $(SRC)problems/%.cpp $(wildcard $(SRC)problems/%.hpp) $(MFEM_LIB_FILE)\
+ $(CONFIG_MK) | lib-common
+	mkdir -p $(@D)
+	$(MFEM_CXX) $(MFEM_FLAGS) -c $< -o $@
+
+
+all: $(MINIAPPS)
+
+contact_driver: $(CONTACT_OBJ)
+	$(MFEM_CXX) $(MFEM_LINK_FLAGS) -o $@ $(CONTACT_OBJ) $(COMMON_LIB) $(MFEM_LIBS)
+
+pcontact_driver: $(PCONTACT_OBJ)
+	$(MFEM_CXX) $(MFEM_LINK_FLAGS) -o $@ $(PCONTACT_OBJ) $(COMMON_LIB) $(MFEM_LIBS)
+
+
+# Rule for building lib-common
+lib-common:
+	$(MAKE) -C $(MFEM_BUILD_DIR)/miniapps/common
+
+MFEM_TESTS = MINIAPPS
+include $(MFEM_TEST_MK)
+
+# Testing: Specific execution options
+RUN_MPI = $(MFEM_MPIEXEC) $(MFEM_MPIEXEC_NP) $(MFEM_MPI_NP)
+contact-test-seq: diffusion
+	@$(call mfem-test,$<,, contact miniapp,)
+pcontact-test-par: pcontact
+	@$(call mfem-test,$<, $(RUN_MPI), pcontact miniapp,)
+
+# Generate an error message if the MFEM library is not built and exit
+$(MFEM_LIB_FILE):
+	$(error The MFEM library is not built)
+
+clean: clean-build clean-exec
+
+clean-build:
+	rm -f *.o *~ $(PAR_MINIAPPS) $(SEQ_MINIAPPS)
+	rm -f $(CONTACT_OBJ) $(PCONTACT_OBJ)
+	rm -rf *.dSYM *.TVD.*breakpoints
+
+clean-exec:
+	@rm -rf ParaView 

miniapps/contact/meshes/block1.mesh

@@ -0,0 +1,103 @@
+MFEM mesh v1.0
+
+#
+# MFEM Geometry Types (see mesh/geom.hpp):
+#
+# POINT       = 0
+# SEGMENT     = 1
+# TRIANGLE    = 2
+# SQUARE      = 3
+# TETRAHEDRON = 4
+# CUBE        = 5
+# PRISM       = 6
+#
+
+dimension
+3
+
+elements
+9
+1   5     0      1     3     2     8    9     11    10
+1   5     2      3     5     4    10    11    13    12
+1   5     4      5     7     6    12    13    15    14
+1   5     8      9    11    10    16    17    19    18
+1   5     10    11    13    12    18    19    21    20
+1   5     12    13    15    14    20    21    23    22
+1   5     16    17    19    18    24    25    27    26
+1   5     18    19    21    20    26    27    29    28
+1   5     20    21    23    22    28    29    31    30
+
+
+
+# 0 nothing
+# 1 dirichlet bc
+# 2 contact
+boundary
+30
+1   3    1     0     2     3
+1   3    3     2     4     5
+1   3    5     4     6     7
+1   3    24    25    27    26
+1   3    26    27    29    28
+1   3    28    29    31    30
+2   3    2     0     8    10
+2   3    4     2    10    12
+2   3    6     4    12    14
+2  3    10    8    16    18
+2  3    12    10    18    20
+2  3    14    12    20    22
+2  3    18    16    24    26
+2  3    20    18    26    28
+2  3    22    20    28    30
+3  3    1     3    11    9
+3  3     3     5    13    11
+3  3     5     7    15    13
+3  3    9    11    19    17
+3  3    11    13    21    19
+3  3    13    15    23    21
+3  3    17    19    27    25
+3  3    19    21    29    27
+3  3    21    23    31    29
+1  3     8     0     1    9
+1  3    16     8    9    17
+1  3    24    16    17    25
+1  3     6    14    15     7
+1  3    14    22    23    15
+1  3    22    30    31    23
+
+
+vertices
+32
+3
+   -1.0000         0         0
+         0         0         0
+   -1.0000    0.3000         0
+         0    0.3000         0
+   -1.0000    0.6500         0
+         0    0.6500         0
+   -1.0000    1.0000         0
+         0    1.0000         0
+   -1.0000         0    0.3000
+         0         0    0.3000
+   -1.0000    0.3000    0.3500
+         0    0.3000    0.3500
+   -1.0000    0.6500    0.3000
+         0    0.6500    0.3000
+   -1.0000    1.0000    0.3000
+         0    1.0000    0.3000
+   -1.0000         0    0.6500
+         0         0    0.6500
+   -1.0000    0.3000    0.6500
+         0    0.3000    0.6500
+   -1.0000    0.6500    0.6500
+         0    0.6500    0.6500
+   -1.0000    1.0000    0.6500
+         0    1.0000    0.6500
+   -1.0000         0    1.0000
+         0         0    1.0000
+   -1.0000    0.3000    1.0000
+         0    0.3000    1.0000
+   -1.0000    0.6500    1.0000
+         0    0.6500    1.0000
+   -1.0000    1.0000    1.0000
+         0    1.0000    1.0000

miniapps/contact/meshes/block2.mesh

@@ -0,0 +1,68 @@
+MFEM mesh v1.0
+
+#
+# MFEM Geometry Types (see mesh/geom.hpp):
+#
+# POINT       = 0
+# SEGMENT     = 1
+# TRIANGLE    = 2
+# SQUARE      = 3
+# TETRAHEDRON = 4
+# CUBE        = 5
+# PRISM       = 6
+#
+
+dimension
+3
+
+# 1 nothing
+elements
+4
+1   5     0     1     3     2     6     7    9     8
+1   5     2     3     5     4     8     9    11    10
+1   5     6     7     9     8    12    13    15    14
+1   5     8     9    11    10    14    15    17    16
+
+# 0 nothing
+# 1 dirichlet bc
+# 2 contact
+boundary
+16
+1   3     1     0     2     3
+1   3     3     2     4     5
+1   3    12    13    15    14
+1   3    14    15    17    16
+3   3     2     0     6     8
+3   3     4     2     8    10
+3   3     8     6    12    14
+3   3    10     8    14    16
+2   3     1     3     9     7
+2   3     3     5    11    9
+2   3     7    9    15    13
+2   3     9    11    17    15
+1   3     6     0     1     7
+1   3    12     6     7    13
+1   3     4    10    11     5
+1   3    10    16    17    11
+
+vertices
+18
+3
+         0    0.2464    0.2464
+    0.5071    0.2464    0.2464
+         0    0.5000    0.2464
+    0.5071    0.5000    0.2464
+         0    0.7536    0.2464
+    0.5071    0.7536    0.2464
+         0    0.2464    0.5000
+    0.5071    0.2464    0.5000
+         0    0.5000    0.5000
+    0.5071    0.5000    0.5000
+         0    0.7536    0.5000
+    0.5071    0.7536    0.5000
+         0    0.2464    0.7536
+    0.5071    0.2464    0.7536
+         0    0.5000    0.7536
+    0.5071    0.5000    0.7536
+         0    0.7536    0.7536
+    0.5071    0.7536    0.7536

miniapps/contact/meshes/rotatedblock2.mesh

@@ -0,0 +1,70 @@
+
+MFEM mesh v1.0
+
+#
+# MFEM Geometry Types (see mesh/geom.hpp):
+#
+# POINT       = 0
+# SEGMENT     = 1
+# TRIANGLE    = 2
+# SQUARE      = 3
+# TETRAHEDRON = 4
+# CUBE        = 5
+# PRISM       = 6
+#
+
+dimension
+3
+
+# 1 nothing
+elements
+4
+1   5     0     1     3     2     6     7    9     8
+1   5     2     3     5     4     8     9    11    10
+1   5     6     7     9     8    12    13    15    14
+1   5     8     9    11    10    14    15    17    16
+
+# 0 nothing
+# 1 dirichlet bc
+# 2 contact
+boundary
+16
+1   3     1     0     2     3
+1   3     3     2     4     5
+1   3    12    13    15    14
+1   3    14    15    17    16
+3   3     2     0     6     8
+3   3     4     2     8    10
+3   3     8     6    12    14
+3   3    10     8    14    16
+2   3     1     3     9     7
+2   3     3     5    11    9
+2   3     7    9    15    13
+2   3     9    11    17    15
+1   3     6     0     1     7
+1   3    12     6     7    13
+1   3     4    10    11     5
+1   3    10    16    17    11
+
+vertices
+18
+3
+
+0.000000000000 0.145770950245 0.443895630208
+0.507100000000 0.145770950245 0.443895630208
+0.000000000000 0.350937660019 0.294833290227
+0.507100000000 0.350937660019 0.294833290227
+0.000000000000 0.556104369792 0.145770950245
+0.507100000000 0.556104369792 0.145770950245
+0.000000000000 0.294833290227 0.649062339981
+0.507100000000 0.294833290227 0.649062339981
+0.000000000000 0.500000000000 0.500000000000
+0.507100000000 0.500000000000 0.500000000000
+0.000000000000 0.705166709773 0.350937660019
+0.507100000000 0.705166709773 0.350937660019
+0.000000000000 0.443895630208 0.854229049755
+0.507100000000 0.443895630208 0.854229049755
+0.000000000000 0.649062339981 0.705166709773
+0.507100000000 0.649062339981 0.705166709773
+0.000000000000 0.854229049755 0.556104369792
+0.507100000000 0.854229049755 0.556104369792

miniapps/contact/pcontact_driver.cpp

@@ -0,0 +1,254 @@
+//                               Parallel contact example
+//
+// Compile with: make pcontact_driver
+// sample run
+// mpirun -np 6 ./pcontact_driver -sr 2 -pr 2
+
+#include "mfem.hpp"
+#include <fstream>
+#include <iostream>
+#include "ipsolver/ParIPsolver.hpp"
+
+using namespace std;
+using namespace mfem;
+
+
+int main(int argc, char *argv[])
+{
+   Mpi::Init();
+   int myid = Mpi::WorldRank();
+   int num_procs = Mpi::WorldSize();
+   Hypre::Init();
+   // 1. Parse command-line options.
+   const char *mesh_file = "meshes/merged.mesh";
+   int order = 1;
+   int sref = 0;
+   int pref = 0;
+   Array<int> attr;
+   Array<int> m_attr;
+   bool visualization = true;
+   bool paraview = false;
+   double linsolvertol = 1e-6;
+   int relax_type = 8;
+   double optimizer_tol = 1e-6;
+
+   OptionsParser args(argc, argv);
+   args.AddOption(&mesh_file, "-m", "--mesh",
+                  "Mesh file to use.");
+   args.AddOption(&attr, "-at", "--attributes-surf",
+                  "Attributes of boundary faces on contact surface for mesh 2.");
+   args.AddOption(&sref, "-sr", "--serial-refinements",
+                  "Number of uniform refinements.");
+   args.AddOption(&pref, "-pr", "--parallel-refinements",
+                  "Number of uniform refinements.");
+   args.AddOption(&linsolvertol, "-stol", "--solver-tol",
+                  "Linear Solver Tolerance.");
+   args.AddOption(&optimizer_tol, "-otol", "--optimizer-tol",
+                  "Interior Point Solver Tolerance.");
+   args.AddOption(&relax_type, "-rt", "--relax-type",
+                  "Selection of Smoother for AMG");
+   args.AddOption(&visualization, "-vis", "--visualization", "-no-vis",
+                  "--no-visualization",
+                  "Enable or disable GLVis visualization.");
+   args.AddOption(&paraview, "-paraview", "--paraview", "-no-paraview",
+                  "--no-paraview",
+                  "Enable or disable ParaView visualization.");
+   args.Parse();
+   if (!args.Good())
+   {
+      if (myid == 0)
+      {
+         args.PrintUsage(cout);
+      }
+      return 1;
+   }
+   if (myid == 0)
+   {
+      args.PrintOptions(cout);
+   }
+
+   Mesh * merged_mesh = new Mesh(mesh_file,1);
+
+
+   Array<int> attr1; attr1.Append(1);
+   Array<int> attr2; attr2.Append(2);
+   Mesh * mesh1 = new Mesh(SubMesh::CreateFromDomain(*merged_mesh,attr1));
+   Mesh * mesh2 = new Mesh(SubMesh::CreateFromDomain(*merged_mesh,attr2));
+
+   for (int i = 0; i<sref; i++)
+   {
+      mesh1->UniformRefinement();
+      mesh2->UniformRefinement();
+   }
+   for (int i = 0; i<mesh1->GetNE(); i++)
+   {
+      mesh1->SetAttribute(i,1);
+   }
+   mesh1->SetAttributes();
+   for (int i = 0; i<mesh2->GetNE(); i++)
+   {
+      mesh2->SetAttribute(i,2);
+   }
+   mesh2->SetAttributes();
+
+   ParMesh * pmesh1 = new ParMesh(MPI_COMM_WORLD,*mesh1);
+   ParMesh * pmesh2 = new ParMesh(MPI_COMM_WORLD,*mesh2);
+
+   for (int i = 0; i<pref; i++)
+   {
+      pmesh1->UniformRefinement();
+      pmesh2->UniformRefinement();
+   }
+
+   MFEM_VERIFY(pmesh1->GetNE(), "Empty partition mesh1");
+   MFEM_VERIFY(pmesh2->GetNE(), "Empty partition mesh2");
+
+   ParElasticityProblem * prob1 = new ParElasticityProblem(pmesh1,order);
+   ParElasticityProblem * prob2 = new ParElasticityProblem(pmesh2,order);
+
+
+   Vector lambda1(prob1->GetMesh()->attributes.Max()); lambda1 = 57.6923076923;
+   Vector mu1(prob1->GetMesh()->attributes.Max()); mu1 = 38.4615384615;
+   Vector lambda2(prob2->GetMesh()->attributes.Max()); lambda2 = 57.6923076923;
+   Vector mu2(prob2->GetMesh()->attributes.Max()); mu2 = 38.4615384615;
+
+   prob1->SetLambda(lambda1); prob1->SetMu(mu1);
+   prob2->SetLambda(lambda2); prob2->SetMu(mu2);
+
+   ParContactProblem contact(prob1,prob2);
+   QPOptParContactProblem qpopt(&contact);
+   int numconstr = contact.GetGlobalNumConstraints();
+
+   ParInteriorPointSolver optimizer(&qpopt);
+
+   optimizer.SetTol(optimizer_tol);
+   optimizer.SetMaxIter(50);
+
+   int linsolver = 2;
+   optimizer.SetLinearSolver(linsolver);
+   optimizer.SetLinearSolveTol(linsolvertol);
+   optimizer.SetLinearSolveRelaxType(relax_type);
+
+   ParGridFunction x1 = prob1->GetDisplacementGridFunction();
+   ParGridFunction x2 = prob2->GetDisplacementGridFunction();
+
+   int ndofs1 = prob1->GetNumTDofs();
+   int ndofs2 = prob2->GetNumTDofs();
+   int gndofs1 = prob1->GetGlobalNumDofs();
+   int gndofs2 = prob2->GetGlobalNumDofs();
+   int ndofs = ndofs1 + ndofs2;
+
+   Vector X1 = x1.GetTrueVector();
+   Vector X2 = x2.GetTrueVector();
+
+   Vector x0(ndofs); x0 = 0.0;
+   x0.SetVector(X1,0);
+   x0.SetVector(X2,X1.Size());
+
+   Vector xf(ndofs); xf = 0.0;
+   optimizer.Mult(x0, xf);
+
+   double Einitial = contact.E(x0);
+   double Efinal = contact.E(xf);
+   Array<int> & CGiterations = optimizer.GetCGIterNumbers();
+   if (Mpi::Root())
+   {
+      mfem::out << endl;
+      mfem::out << " Initial Energy objective     = " << Einitial << endl;
+      mfem::out << " Final Energy objective       = " << Efinal << endl;
+      mfem::out << " Global number of dofs        = " << gndofs1 + gndofs2 << endl;
+      mfem::out << " Global number of constraints = " << numconstr << endl;
+      mfem::out << " CG iteration numbers         = " ;
+      CGiterations.Print(mfem::out, CGiterations.Size());
+   }
+
+   MFEM_VERIFY(optimizer.GetConverged(),
+               "Interior point solver did not converge.");
+
+
+   if (visualization || paraview)
+   {
+      ParFiniteElementSpace * fes1 = prob1->GetFESpace();
+      ParFiniteElementSpace * fes2 = prob2->GetFESpace();
+
+      ParMesh * pmesh_1 = fes1->GetParMesh();
+      ParMesh * pmesh_2 = fes2->GetParMesh();
+
+      Vector X1_new(xf.GetData(),fes1->GetTrueVSize());
+      Vector X2_new(&xf.GetData()[fes1->GetTrueVSize()],fes2->GetTrueVSize());
+
+      ParGridFunction x1_gf(fes1);
+      ParGridFunction x2_gf(fes2);
+
+      x1_gf.SetFromTrueDofs(X1_new);
+      x2_gf.SetFromTrueDofs(X2_new);
+
+      pmesh_1->MoveNodes(x1_gf);
+      pmesh_2->MoveNodes(x2_gf);
+
+      if (paraview)
+      {
+         ParaViewDataCollection paraview_dc1("QPContactBody1", pmesh_1);
+         paraview_dc1.SetPrefixPath("ParaView");
+         paraview_dc1.SetLevelsOfDetail(1);
+         paraview_dc1.SetDataFormat(VTKFormat::BINARY);
+         paraview_dc1.SetHighOrderOutput(true);
+         paraview_dc1.SetCycle(0);
+         paraview_dc1.SetTime(0.0);
+         paraview_dc1.RegisterField("Body1", &x1_gf);
+         paraview_dc1.Save();
+
+         ParaViewDataCollection paraview_dc2("QPContactBody2", pmesh_2);
+         paraview_dc2.SetPrefixPath("ParaView");
+         paraview_dc2.SetLevelsOfDetail(1);
+         paraview_dc2.SetDataFormat(VTKFormat::BINARY);
+         paraview_dc2.SetHighOrderOutput(true);
+         paraview_dc2.SetCycle(0);
+         paraview_dc2.SetTime(0.0);
+         paraview_dc2.RegisterField("Body2", &x2_gf);
+         paraview_dc2.Save();
+      }
+
+
+      if (visualization)
+      {
+         char vishost[] = "localhost";
+         int visport = 19916;
+
+         {
+            socketstream sol_sock1(vishost, visport);
+            sol_sock1.precision(8);
+            sol_sock1 << "parallel " << num_procs << " " << myid << "\n"
+                      << "solution\n" << *pmesh_1 << x1_gf << flush;
+         }
+         {
+            socketstream sol_sock2(vishost, visport);
+            sol_sock2.precision(8);
+            sol_sock2 << "parallel " << num_procs << " " << myid << "\n"
+                      << "solution\n" << *pmesh_2 << x2_gf << flush;
+         }
+
+         // {
+         //    socketstream sol_sock(vishost, visport);
+         //    sol_sock.precision(8);
+         //    sol_sock << "parallel " << 2*num_procs << " " << myid << "\n"
+         //             << "solution\n" << *pmesh_1 << x1_gf << flush;
+         // }
+         // {
+         //    socketstream sol_sock(vishost, visport);
+         //    sol_sock.precision(8);
+         //    sol_sock << "parallel " << 2*num_procs << " " << myid+num_procs << "\n"
+         //             << "solution\n" << *pmesh_2 << x2_gf << flush;
+         // }
+      }
+   }
+
+   delete prob2;
+   delete prob1;
+   delete pmesh2;
+   delete pmesh1;
+   // delete mesh1;
+   // delete mesh2;
+
+   return 0;
+}

miniapps/contact/problems/parproblems.cpp

@@ -0,0 +1,516 @@
+#include "parproblems.hpp"
+
+void ParElasticityProblem::Init()
+{
+   int dim = pmesh->Dimension();
+   fec = new H1_FECollection(order,dim);
+   fes = new ParFiniteElementSpace(pmesh,fec,dim,Ordering::byVDIM);
+   ndofs = fes->GetVSize();
+   ntdofs = fes->GetTrueVSize();
+   gndofs = fes->GlobalTrueVSize();
+   pmesh->SetNodalFESpace(fes);
+   if (pmesh->bdr_attributes.Size())
+   {
+      ess_bdr.SetSize(pmesh->bdr_attributes.Max());
+   }
+   ess_bdr = 0; ess_bdr[1] = 1;
+   fes->GetEssentialTrueDofs(ess_bdr,ess_tdof_list);
+   // Solution GridFunction
+   x.SetSpace(fes);  x = 0.0;
+   // RHS
+   b.Update(fes);
+
+   // Elasticity operator
+   lambda.SetSize(pmesh->attributes.Max()); lambda = 57.6923076923;
+   mu.SetSize(pmesh->attributes.Max()); mu = 38.4615384615;
+
+   lambda_cf.UpdateConstants(lambda);
+   mu_cf.UpdateConstants(mu);
+
+   a = new ParBilinearForm(fes);
+   a->AddDomainIntegrator(new ElasticityIntegrator(lambda_cf,mu_cf));
+}
+
+void ParElasticityProblem::FormLinearSystem()
+{
+   if (!formsystem) 
+   {
+      formsystem = true;
+      b.Assemble();
+      a->Assemble();
+      a->FormLinearSystem(ess_tdof_list, x, b, A, X, B);
+   }
+}
+
+void ParElasticityProblem::UpdateLinearSystem()
+{
+   if (formsystem)
+   {
+      b.Update();
+      a->Update();
+      formsystem = false;
+   }
+   FormLinearSystem();
+}
+
+ParContactProblem::ParContactProblem(ParElasticityProblem * prob1_, ParElasticityProblem * prob2_)
+: prob1(prob1_), prob2(prob2_)
+{
+   ParMesh* pmesh1 = prob1->GetMesh();
+   comm = pmesh1->GetComm();
+   MPI_Comm_rank(comm, &myid);
+   MPI_Comm_size(comm, &numprocs);
+ 
+   dim = pmesh1->Dimension();
+   nodes0.SetSpace(pmesh1->GetNodes()->FESpace());
+   nodes0 = *pmesh1->GetNodes();
+   nodes1 = pmesh1->GetNodes();
+   Vector delta1(dim);
+   delta1 = 0.0; delta1[0] = 0.1;
+   prob1->SetDisplacementDirichletData(delta1);
+   prob1->FormLinearSystem();
+
+   Vector delta2(dim);
+   delta2 = 0.0; 
+   prob2->SetDisplacementDirichletData(delta2);
+   prob2->FormLinearSystem();
+
+   int ndof1 = prob1->GetNumTDofs();
+   int ndof2 = prob2->GetNumTDofs();
+
+   tdof_offsets.SetSize(3);
+   tdof_offsets[0] = 0;
+   tdof_offsets[1] = ndof1;
+   tdof_offsets[2] = ndof2;
+   tdof_offsets.PartialSum();
+
+   Array2D<HypreParMatrix*> A(2,2);
+   A(0,0) = &prob1->GetOperator();
+   A(1,1) = &prob2->GetOperator();
+   A(1,0) = nullptr;
+   A(0,1) = nullptr;
+   K = HypreParMatrixFromBlocks(A);
+
+   B = new BlockVector(tdof_offsets);
+   B->GetBlock(0).Set(1.0, prob1->GetRHS());
+   B->GetBlock(1).Set(1.0, prob2->GetRHS());
+
+   ComputeContactVertices();
+}
+
+void ParContactProblem::ComputeContactVertices()
+{
+   if (gnpoints>0) return;
+
+   ParMesh * pmesh1 = prob1->GetMesh();
+   ParMesh * pmesh2 = prob2->GetMesh();
+   dim = pmesh1->Dimension();
+
+   vfes1 = new ParFiniteElementSpace(pmesh1, prob1->GetFECol());
+   vfes2 = new ParFiniteElementSpace(pmesh2, prob2->GetFECol());
+
+   int gnv1 = vfes1->GlobalTrueVSize();
+   int gnv2 = vfes2->GlobalTrueVSize();
+   gnv = gnv1+gnv2;
+   int nv1 = vfes1->GetTrueVSize();
+   int nv2 = vfes2->GetTrueVSize();
+   nv = nv1+nv2;
+
+   vertices1.SetSize(pmesh1->GetNV());
+   vertices2.SetSize(pmesh2->GetNV());
+
+   for (int i = 0; i<pmesh1->GetNV(); i++)
+   {
+      vertices1[i] = i;
+   }
+   pmesh1->GetGlobalVertexIndices(vertices1);
+
+   for (int i = 0; i<pmesh2->GetNV(); i++)
+   {
+      vertices2[i] = i;
+   }
+   pmesh2->GetGlobalVertexIndices(vertices2);
+
+   int voffset2 = vfes2->GetMyTDofOffset();
+
+   std::vector<int> vertex2_offsets;
+   ComputeTdofOffsets(comm,voffset2, vertex2_offsets);
+
+   Array<int> vert;
+   for (int b=0; b<pmesh2->GetNBE(); b++)
+   {
+      if (pmesh2->GetBdrAttribute(b) == 3)
+      {
+         pmesh2->GetBdrElementVertices(b, vert);
+         for (auto v : vert)
+         {
+            if (myid != get_rank(vertices2[v],vertex2_offsets)) { continue; }
+            contact_vertices.insert(v);
+         }
+      }
+   }
+
+   npoints = contact_vertices.size();
+
+   MPI_Allreduce(&npoints, &gnpoints,1,MPI_INT,MPI_SUM,pmesh1->GetComm());
+   int constrains_offset;
+   MPI_Scan(&npoints,&constrains_offset,1,MPI_INT,MPI_SUM,pmesh1->GetComm());
+
+   constrains_offset-=npoints;
+   constraints_starts.SetSize(2);
+   constraints_starts[0] = constrains_offset;
+   constraints_starts[1] = constrains_offset+npoints;
+
+   ComputeTdofOffsets(comm,constrains_offset, constraints_offsets);
+}
+
+void ParContactProblem::ComputeGapFunctionAndDerivatives(const Vector & displ1, const Vector &displ2)
+{
+   ComputeContactVertices();
+   ParMesh * pmesh1 = prob1->GetMesh();
+   ParMesh * pmesh2 = prob2->GetMesh();
+
+   ParGridFunction displ1_gf(prob1->GetFESpace());
+   ParGridFunction displ2_gf(prob2->GetFESpace());
+
+   displ1_gf.SetFromTrueDofs(displ1);
+   displ2_gf.SetFromTrueDofs(displ2);
+
+   Array<int> conn2(npoints); 
+   Vector xyz(dim * npoints);
+
+   int cnt = 0;
+   for (auto v : contact_vertices)
+   {
+      for (int d = 0; d<dim; d++)
+      {
+         xyz(cnt*dim + d) = pmesh2->GetVertex(v)[d]+displ2_gf[v*dim+d];
+      }
+      conn2[cnt] = vertices2[v];
+      cnt++;
+   }
+
+   MFEM_VERIFY(cnt == npoints, "");
+   gapv.SetSize(npoints*dim); gapv = 0.0;
+   // segment reference coordinates of the closest point
+   Vector xi1(npoints*(dim-1));
+   Array<int> conn1(npoints*4);
+   DenseMatrix coordsm(npoints*4, dim);
+   // add(nodes0, displ1_gf, *nodes1);
+   FindPointsInMesh(*pmesh1, vertices1, conn2, displ1_gf, xyz, conn1, xi1, coordsm);
+   if (M)
+   {
+      delete M;
+      for (int i = 0; i<dM.Size(); i++)
+      {
+         delete dM[i];
+      }
+      dM.SetSize(0);
+   }
+
+   int ndofs1 = prob1->GetFESpace()->GetTrueVSize();
+   int ndofs2 = prob2->GetFESpace()->GetTrueVSize();
+   int gndofs1 = prob1->GetFESpace()->GlobalTrueVSize();
+   int gndofs2 = prob2->GetFESpace()->GlobalTrueVSize();
+   
+   Array<int> npts(numprocs);
+   MPI_Allgather(&npoints,1,MPI_INT,&npts[0],1,MPI_INT,comm);
+   npts.PartialSum(); npts.Prepend(0);
+
+   SparseMatrix S1(gnpoints,gndofs1);
+   SparseMatrix S2(gnpoints,gndofs2);
+   Array<SparseMatrix *> dS11;
+   Array<SparseMatrix *> dS12;
+   Array<SparseMatrix *> dS21;
+   Array<SparseMatrix *> dS22;
+
+   // local to global map for constraints
+   Array<int> points_map(npoints);
+   cnt = 0;
+   for (int i = 0; i<gnpoints; i++)
+   {
+      if (i >= npts[myid] && i< npts[myid+1])
+      {
+         points_map[cnt++] = i;
+      }
+   }
+   if (compute_hessians)
+   {
+      dS11.SetSize(gnpoints);
+      dS12.SetSize(gnpoints);
+      dS21.SetSize(gnpoints);
+      dS22.SetSize(gnpoints);
+      for (int i = 0; i<gnpoints; i++)
+      {
+         if (i >= npts[myid] && i< npts[myid+1])
+         {
+            dS11[i] = new SparseMatrix(gndofs1,gndofs1);
+            dS12[i] = new SparseMatrix(gndofs1,gndofs2);
+            dS21[i] = new SparseMatrix(gndofs2,gndofs1);
+            dS22[i] = new SparseMatrix(gndofs2,gndofs2);
+         }
+         else
+         {
+            dS11[i] = nullptr;
+            dS12[i] = nullptr;
+            dS21[i] = nullptr;
+            dS22[i] = nullptr;
+         }
+      }
+      Assemble_Contact(xyz, xi1, coordsm, conn2, conn1, gapv, S1,S2,
+                      dS11,dS12,dS21,dS22);
+   }
+   else
+   {
+      Assemble_Contact(xyz, xi1, coordsm, conn2, conn1, gapv, S1,S2, points_map);
+   }                   
+                
+   // --------------------------------------------------------------------
+   // Redistribute the M block matrix [M1 M2]
+   // --------------------------------------------------------------------
+   int offset = constraints_offsets[myid];
+   MPICommunicator Mcomm1(comm,offset,gnpoints);
+   SparseMatrix localS1(npoints,gndofs1);
+   Mcomm1.Communicate(S1,localS1);
+   MPICommunicator Mcomm2(comm,offset,gnpoints);
+   SparseMatrix localS2(npoints,gndofs2);
+   Mcomm2.Communicate(S2,localS2);
+   
+   MFEM_VERIFY(HYPRE_AssumedPartitionCheck(), "Hypre_AssumedPartitionCheck is False");
+
+   // Construct M row and col starts to construct HypreParMatrix
+   int M1rows[2], M2rows[2]; 
+   int M1cols[2], M2cols[2];
+   M1rows[0] = constraints_starts[0];
+   M1rows[1] = constraints_starts[1];
+
+   M2rows[0] = constraints_starts[0];
+   M2rows[1] = constraints_starts[1];
+
+   M1cols[0] = prob1->GetFESpace()->GetTrueDofOffsets()[0];
+   M1cols[1] = prob1->GetFESpace()->GetTrueDofOffsets()[1];
+
+   M2cols[0] = prob2->GetFESpace()->GetTrueDofOffsets()[0];
+   M2cols[1] = prob2->GetFESpace()->GetTrueDofOffsets()[1];
+
+   Array2D<HypreParMatrix*> blockM(1,2);
+   blockM(0,0) = new HypreParMatrix(comm,npoints,gnpoints,gndofs1,
+                          localS1.GetI(), localS1.GetJ(),localS1.GetData(),
+                          M1rows,M1cols);
+
+   blockM(0,1) = new HypreParMatrix(comm,npoints,gnpoints,gndofs2,
+                          localS2.GetI(), localS2.GetJ(),localS2.GetData(),
+                          M2rows,M2cols);
+
+   M = HypreParMatrixFromBlocks(blockM);
+   delete blockM(0,0);
+   delete blockM(0,1);
+   blockM.DeleteAll();
+
+   if (compute_hessians)
+   {
+      Array<SparseMatrix*> localdS11(gnpoints);
+      Array<SparseMatrix*> localdS12(gnpoints);
+      Array<SparseMatrix*> localdS21(gnpoints);
+      Array<SparseMatrix*> localdS22(gnpoints);
+      for (int k = 0; k<gnpoints; k++)
+      {
+         localdS11[k] = new SparseMatrix(ndofs1,gndofs1); 
+         localdS12[k] = new SparseMatrix(ndofs1,gndofs2); 
+         localdS21[k] = new SparseMatrix(ndofs2,gndofs1); 
+         localdS22[k] = new SparseMatrix(ndofs2,gndofs2); 
+      }
+
+      int offset1 = prob1->GetFESpace()->GetMyTDofOffset();
+      int offset2 = prob2->GetFESpace()->GetMyTDofOffset();
+
+      MPICommunicator dmcomm11(comm, offset1, gndofs1);
+      dmcomm11.Communicate(dS11,localdS11);
+      for (int k = 0; k<gnpoints; k++) { delete dS11[k]; }
+
+      MPICommunicator dmcomm12(comm, offset1, gndofs1);
+      dmcomm12.Communicate(dS12,localdS12);
+      for (int k = 0; k<gnpoints; k++) { delete dS12[k]; }
+
+      MPICommunicator dmcomm21(comm, offset2, gndofs2);
+      dmcomm21.Communicate(dS21,localdS21);
+      for (int k = 0; k<gnpoints; k++) { delete dS21[k]; }
+
+      MPICommunicator dmcomm22(comm, offset2, gndofs2);
+      dmcomm22.Communicate(dS22,localdS22);
+      for (int k = 0; k<gnpoints; k++) { delete dS22[k]; }
+
+      // --------------------------------------------------------------------
+      // Redistribute the block dM matrices [dM11 dM12; dM21 dM22]
+      // --------------------------------------------------------------------
+
+      // Construct dMi HypreParMatrix
+      Array2D<HypreParMatrix *> dMs(2,2);
+      dM.SetSize(gnpoints);
+      int * offs1 = prob1->GetFESpace()->GetTrueDofOffsets();
+      int * offs2 = prob2->GetFESpace()->GetTrueDofOffsets();
+      for (int i = 0; i<gnpoints; i++)
+      {
+         dMs(0,0) = new HypreParMatrix(comm, ndofs1, gndofs1, gndofs1, 
+                                      localdS11[i]->GetI(), localdS11[i]->GetJ(), 
+                                      localdS11[i]->GetData(),
+                                      offs1,offs1);
+         delete localdS11[i];                                 
+         dMs(0,1) = new HypreParMatrix(comm, ndofs1, gndofs1, gndofs2, 
+                                      localdS12[i]->GetI(), localdS12[i]->GetJ(), 
+                                      localdS12[i]->GetData(),
+                                      offs1,offs2);   
+         delete localdS12[i];                                 
+         dMs(1,0) = new HypreParMatrix(comm, ndofs2, gndofs2, gndofs1, 
+                                      localdS21[i]->GetI(), localdS21[i]->GetJ(), 
+                                      localdS21[i]->GetData(),
+                                      offs2,offs1);
+         delete localdS21[i];                                 
+         dMs(1,1) = new HypreParMatrix(comm, ndofs2, gndofs2, gndofs2, 
+                                      localdS22[i]->GetI(), localdS22[i]->GetJ(), 
+                                      localdS22[i]->GetData(),
+                                      offs2,offs2);    
+         delete localdS22[i];                                 
+
+         dM[i] = HypreParMatrixFromBlocks(dMs);
+         delete dMs(0,0);
+         delete dMs(0,1);
+         delete dMs(1,0);
+         delete dMs(1,1);
+      }
+      dMs.DeleteAll();
+   }
+}
+
+double ParContactProblem::E(const Vector & d)
+{
+   Vector kd(K->Height());
+   K->Mult(d,kd);
+   return 0.5 * InnerProduct(comm,d, kd) - InnerProduct(comm,d, *B);
+}
+
+void ParContactProblem::DdE(const Vector &d, Vector &gradE)
+{
+   gradE.SetSize(K->Height());
+   K->Mult(d, gradE);
+   gradE.Add(-1.0, *B); 
+}
+
+HypreParMatrix* ParContactProblem::DddE(const Vector &d)
+{
+   return K; 
+}
+
+void ParContactProblem::g(const Vector &d, Vector &gd, bool compute_hessians_)
+{
+   compute_hessians = compute_hessians_;
+   int ndof1 = prob1->GetNumTDofs();
+   int ndof2 = prob2->GetNumTDofs();
+   double * data = d.GetData();
+   Vector displ1(data,ndof1);
+   Vector displ2(&data[ndof1],ndof2);
+
+   if (recompute)
+   {
+      ComputeGapFunctionAndDerivatives(displ1, displ2);
+      recompute = false;
+   }
+
+   gd = GetGapFunction();
+}
+
+HypreParMatrix* ParContactProblem::Ddg(const Vector &d)
+{
+  return GetJacobian();
+}
+
+HypreParMatrix* ParContactProblem::lDddg(const Vector &d, const Vector &l)
+{
+   return nullptr; // for now
+}
+
+
+QPOptParContactProblem::QPOptParContactProblem(ParContactProblem * problem_)
+: problem(problem_)
+{
+   dimU = problem->GetNumDofs();
+   dimM = problem->GetNumContraints();
+   dimC = problem->GetNumContraints();
+   ml.SetSize(dimM); ml = 0.0;
+   Vector negone(dimM); negone = -1.0;
+   SparseMatrix diag(negone);
+
+   int gsize = problem->GetGlobalNumConstraints();
+   int * rows = problem->GetConstraintsStarts().GetData();
+
+   NegId = new HypreParMatrix(problem->GetComm(),gsize, rows,&diag);
+   HypreStealOwnership(*NegId, diag);
+}
+
+int QPOptParContactProblem::GetDimU() { return dimU; }
+
+int QPOptParContactProblem::GetDimM() { return dimM; }
+
+int QPOptParContactProblem::GetDimC() { return dimC; }
+
+Vector & QPOptParContactProblem::Getml() { return ml; }
+
+HypreParMatrix * QPOptParContactProblem::Duuf(const BlockVector & x)
+{
+   return problem->DddE(x.GetBlock(0));
+}
+
+HypreParMatrix * QPOptParContactProblem::Dumf(const BlockVector & x)
+{
+   return nullptr;
+}
+
+HypreParMatrix * QPOptParContactProblem::Dmuf(const BlockVector & x)
+{
+   return nullptr;
+}
+
+HypreParMatrix * QPOptParContactProblem::Dmmf(const BlockVector & x)
+{
+   return nullptr;
+}
+
+HypreParMatrix * QPOptParContactProblem::Duc(const BlockVector & x)
+{
+   return problem->Ddg(x.GetBlock(0));
+}
+
+HypreParMatrix * QPOptParContactProblem::Dmc(const BlockVector & x)
+{
+   return NegId;
+}
+
+HypreParMatrix * QPOptParContactProblem::lDuuc(const BlockVector & x, const Vector & l)
+{
+   return nullptr;
+}
+
+void QPOptParContactProblem::c(const BlockVector &x, Vector & y)
+{
+   Vector g0;
+   problem->g(x.GetBlock(0),g0, false); // gap function
+   g0.Add(-1.0, x.GetBlock(1));  
+   problem->GetJacobian()->Mult(x.GetBlock(0),y);
+   y.Add(1.0, g0);
+}
+
+double QPOptParContactProblem::CalcObjective(const BlockVector & x)
+{
+   return problem->E(x.GetBlock(0));
+}
+
+void QPOptParContactProblem::CalcObjectiveGrad(const BlockVector & x, BlockVector & y)
+{
+   problem->DdE(x.GetBlock(0), y.GetBlock(0));
+   y.GetBlock(1) = 0.0;
+}
+
+QPOptParContactProblem::~QPOptParContactProblem()
+{
+   delete NegId;
+}

miniapps/contact/problems/parproblems.hpp

@@ -0,0 +1,218 @@
+
+#include "parproblems_util.hpp"
+
+class ParElasticityProblem
+{
+private:
+   MPI_Comm comm;
+   bool formsystem = false;
+   ParMesh * pmesh = nullptr;
+   int order;
+   int ndofs;
+   int ntdofs;
+   int gndofs;
+   FiniteElementCollection * fec = nullptr;
+   ParFiniteElementSpace * fes = nullptr;
+   Vector lambda, mu;
+   PWConstCoefficient lambda_cf, mu_cf;
+   Array<int> ess_bdr, ess_tdof_list;
+   ParBilinearForm *a=nullptr;
+   ParLinearForm b;
+   ParGridFunction x;
+   HypreParMatrix A;
+   Vector B,X;
+   void Init();
+   bool own_mesh;
+public:
+   ParElasticityProblem(MPI_Comm comm_, const char *mesh_file , int sref, int pref, int order_ = 1) : comm(comm_), order(order_) 
+   {
+      own_mesh = true;
+      Mesh * mesh = new Mesh(mesh_file,1,1);
+      for (int i = 0; i<sref; i++)
+      {
+         mesh->UniformRefinement();
+      }
+      pmesh = new ParMesh(comm,*mesh);
+      MFEM_VERIFY(pmesh->GetNE(), "ParElasticityProblem::Empty partition");
+      delete mesh;
+      for (int i = 0; i<pref; i++)
+      {
+         pmesh->UniformRefinement();
+      }
+      Init();
+   }
+
+   ParElasticityProblem(ParMesh * pmesh_, int order_ = 1) :  pmesh(pmesh_), order(order_)
+   {
+      own_mesh = false;
+      comm = pmesh->GetComm();
+      Init();
+   }
+
+   ParMesh * GetMesh() { return pmesh; }
+   ParFiniteElementSpace * GetFESpace() { return fes; }
+   FiniteElementCollection * GetFECol() { return fec; }
+   int GetNumDofs() { return ndofs; }
+   int GetNumTDofs() { return ntdofs; }
+   int GetGlobalNumDofs() { return gndofs; }
+   HypreParMatrix & GetOperator() 
+   { 
+      MFEM_VERIFY(formsystem, "System not formed yet. Call FormLinearSystem()"); 
+      return A; 
+   }
+   Vector & GetRHS() 
+   { 
+      MFEM_VERIFY(formsystem, "System not formed yet. Call FormLinearSystem()"); 
+      return B; 
+   }
+
+   void SetLambda(const Vector & lambda_) 
+   { 
+      lambda = lambda_; 
+      lambda_cf.UpdateConstants(lambda);
+   }
+   void SetMu(const Vector & mu_) 
+   { 
+      mu = mu_; 
+      mu_cf.UpdateConstants(mu);
+   }
+
+   void FormLinearSystem();
+   void UpdateLinearSystem();
+
+   void SetDisplacementDirichletData(const Vector & delta) 
+   {
+      VectorConstantCoefficient delta_cf(delta);
+      x.ProjectBdrCoefficient(delta_cf,ess_bdr);
+   };
+
+   ParGridFunction & GetDisplacementGridFunction() {return x;};
+   Array<int> & GetEssentialDofs() {return ess_tdof_list;};
+
+   ~ParElasticityProblem()
+   {
+      delete a;
+      delete fes;
+      delete fec;
+      if (own_mesh)
+      {
+         delete pmesh;
+      }
+   }
+};
+
+
+class ParContactProblem
+{
+private:
+   MPI_Comm comm;
+   int numprocs;
+   int myid;
+   ParElasticityProblem * prob1 = nullptr;
+   ParElasticityProblem * prob2 = nullptr;
+   ParFiniteElementSpace * vfes1 = nullptr;
+   ParFiniteElementSpace * vfes2 = nullptr;
+   int dim;
+   GridFunction nodes0;
+   GridFunction *nodes1 = nullptr;
+   std::set<int> contact_vertices;
+   bool recompute = true;
+   bool compute_hessians = true;
+   std::vector<int> dof_offsets;
+   std::vector<int> vertex_offsets; 
+   std::vector<int> constraints_offsets; 
+   Array<int> tdof_offsets;
+   Array<int> constraints_starts;
+   Array<int> globalvertices1;
+   Array<int> globalvertices2;
+   Array<int> vertices2;
+   Array<int> vertices1;
+
+protected:
+   int npoints=0;
+   int gnpoints=0;
+   int nv, gnv;
+   HypreParMatrix * K = nullptr;
+   BlockVector *B = nullptr;
+   Vector gapv;
+   HypreParMatrix * M=nullptr;
+   Array<HypreParMatrix*> dM;
+   void ComputeContactVertices();
+
+public:
+   ParContactProblem(ParElasticityProblem * prob1_, ParElasticityProblem * prob2_);
+
+   ParElasticityProblem * GetElasticityProblem1() {return prob1;}
+   ParElasticityProblem * GetElasticityProblem2() {return prob2;}
+   MPI_Comm GetComm() {return comm;}
+   int GetNumDofs() {return K->Height();}
+   int GetGlobalNumDofs() {return K->GetGlobalNumRows();}
+   int GetNumContraints() {return npoints;}
+   int GetGlobalNumConstraints() {return gnpoints;}
+   
+   std::vector<int> & GetDofOffets() { return dof_offsets; }
+   std::vector<int> & GetVertexOffsets() { return vertex_offsets; }
+   std::vector<int> & GetConstraintsOffsets() { return constraints_offsets; }
+   Array<int> & GetConstraintsStarts() { return constraints_starts; }
+   
+   Vector & GetGapFunction() {return gapv;}
+
+   HypreParMatrix * GetJacobian() {return M;}
+   Array<HypreParMatrix*> & GetHessian() {return dM;}
+   void ComputeGapFunctionAndDerivatives(const Vector & displ1, const Vector &displ2);
+
+   double E(const Vector & d);
+   void DdE(const Vector &d, Vector &gradE);
+   HypreParMatrix* DddE(const Vector &d);
+   void g(const Vector &d, Vector &gd, bool compute_hessians_ = true);
+   HypreParMatrix* Ddg(const Vector &d);
+   HypreParMatrix* lDddg(const Vector &d, const Vector &l);
+
+   ~ParContactProblem()
+   {
+      delete B;
+      delete K;
+      delete M;
+      for (int i = 0; i<dM.Size(); i++)
+      {
+         delete dM[i];
+      }
+      delete vfes1;
+      delete vfes2;
+   }
+};
+
+
+class QPOptParContactProblem
+{
+private:
+   ParContactProblem * problem = nullptr;
+   int dimU, dimM, dimC;
+   // Array<int> block_offsets;
+   Vector ml;
+   HypreParMatrix * NegId = nullptr;
+public:
+   QPOptParContactProblem(ParContactProblem * problem_);
+   int GetDimU();
+   int GetDimM();
+   int GetDimC();
+   Vector & Getml();
+   MPI_Comm GetComm() {return problem->GetComm();}
+   int * GetConstraintsStarts() {return problem->GetConstraintsStarts().GetData();}
+   int GetGlobalNumConstraints() {return problem->GetGlobalNumConstraints();}
+
+   ParElasticityProblem * GetElasticityProblem1() {return problem->GetElasticityProblem1();}
+   ParElasticityProblem * GetElasticityProblem2() {return problem->GetElasticityProblem2();}
+
+   HypreParMatrix * Duuf(const BlockVector &);
+   HypreParMatrix * Dumf(const BlockVector &);
+   HypreParMatrix * Dmuf(const BlockVector &);
+   HypreParMatrix * Dmmf(const BlockVector &);
+   HypreParMatrix * Duc(const BlockVector &);
+   HypreParMatrix * Dmc(const BlockVector &);
+   HypreParMatrix * lDuuc(const BlockVector &, const Vector &);
+   void c(const BlockVector &, Vector &);
+   double CalcObjective(const BlockVector &);
+   void CalcObjectiveGrad(const BlockVector &, BlockVector &);
+   ~QPOptParContactProblem();
+};

miniapps/contact/problems/parproblems_util.cpp

@@ -0,0 +1,554 @@
+#include "parproblems_util.hpp"
+
+void FindPointsInMesh(Mesh & mesh, const Array<int> & gvert, const Vector & xyz, const Array<int> & s_conn, Array<int>& conn,
+                      Vector & xyz2, Array<int> & s_conn2, Vector& xi, DenseMatrix & coords)
+{
+   const int dim = mesh.Dimension();
+   const int np = xyz.Size() / dim;
+
+   MFEM_VERIFY(np * dim == xyz.Size(), "");
+
+   mesh.EnsureNodes();
+
+   FindPointsGSLIB finder(MPI_COMM_WORLD);
+
+   finder.SetDistanceToleranceForPointsFoundOnBoundary(0.5);
+
+   const double bb_t = 0.5;
+   finder.Setup(mesh, bb_t);
+
+   finder.FindPoints(xyz,mfem::Ordering::byVDIM);
+
+   Array<unsigned int> procs = finder.GetProc();
+
+   /// Return code for each point searched by FindPoints: inside element (0), on
+   /// element boundary (1), or not found (2).
+   Array<unsigned int> codes = finder.GetCode();
+
+   /// Return element number for each point found by FindPoints.
+   Array<unsigned int> elems = finder.GetElem();
+
+   /// Return reference coordinates for each point found by FindPoints.
+   Vector refcrd = finder.GetReferencePosition();
+
+   /// Return distance between the sought and the found point in physical space,
+   /// for each point found by FindPoints.
+   Vector dist = finder.GetDist();
+
+   finder.FreeData();
+
+   MFEM_VERIFY(dist.Size() == np, "");
+   MFEM_VERIFY(refcrd.Size() == np * dim, "");
+   MFEM_VERIFY(elems.Size() == np, "");
+   MFEM_VERIFY(codes.Size() == np, "");
+
+   bool allfound = true;
+   for (auto code : codes)
+      if (code == 2) { allfound = false; }
+
+   MFEM_VERIFY(allfound, "A point was not found");
+
+   // cout << "Maximum distance of projected points: " << dist.Max() << endl;
+
+
+   Array<unsigned int> elems_recv, proc_recv;
+   Vector ref_recv;
+   Vector xyz_recv;
+   Array<int> s_conn_recv;
+
+   MPICommunicator mycomm(MPI_COMM_WORLD, procs);
+   mycomm.Communicate(xyz,xyz_recv,3,mfem::Ordering::byNODES);
+   mycomm.Communicate(elems,elems_recv,1,mfem::Ordering::byVDIM);
+   mycomm.Communicate(refcrd,ref_recv,3,mfem::Ordering::byVDIM);
+   mycomm.Communicate(s_conn,s_conn_recv,1,mfem::Ordering::byVDIM);
+
+   proc_recv = mycomm.GetOriginProcs();
+
+   int np_loc = elems_recv.Size();
+   Array<int> conn_loc(np_loc*4);
+   Vector xi_send(np_loc*(dim-1));
+   for (int i=0; i<np_loc; ++i)
+   {
+      int refFace, refNormal;
+      // int refNormalSide;
+      bool is_interior = -1;
+
+      Vector normal = GetNormalVector(mesh, elems_recv[i],
+                                      ref_recv.GetData() + (i*dim),
+                                      refFace, refNormal, is_interior);
+
+      // continue;
+      int phyFace;
+      if (is_interior)
+      {
+         phyFace = -1; // the id of the face that has the closest point
+         FindSurfaceToProject(mesh, elems_recv[i], phyFace); // seems that this works
+
+         Array<int> cbdrVert;
+         mesh.GetFaceVertices(phyFace, cbdrVert);
+         Vector xs(dim);
+         xs[0] = xyz_recv[i + 0*np_loc];
+         xs[1] = xyz_recv[i + 1*np_loc];
+         xs[2] = xyz_recv[i + 2*np_loc];
+
+         Vector xi_tmp(dim-1);
+         // get nodes!
+
+         GridFunction *nodes = mesh.GetNodes();
+         DenseMatrix coord(4,3);
+         for (int j=0; j<4; j++)
+         {
+            for (int k=0; k<3; k++)
+            {
+               coord(j,k) = (*nodes)[cbdrVert[j]*3+k];
+            }
+         }
+         SlaveToMaster(coord, xs, xi_tmp);
+
+         for (int j=0; j<dim-1; ++j)
+         {
+            xi_send[i*(dim-1)+j] = xi_tmp[j];
+         }
+         // now get get the projection to the surface
+      }
+      else
+      {
+         Vector faceRefCrd(dim-1);
+         {
+            int fd = 0;
+            for (int j=0; j<dim; ++j)
+            {
+               if (j == refNormal)
+               {
+                  // refNormalSide = (ref_recv[(i*dim) + j] > 0.5); // not used
+               }
+               else
+               {
+                  faceRefCrd[fd] = ref_recv[(i*dim) + j];
+                  fd++;
+               }
+            }
+            MFEM_VERIFY(fd == dim-1, "");
+         }
+
+         for (int j=0; j<dim-1; ++j)
+         {
+            xi_send[i*(dim-1)+j] = faceRefCrd[j]*2.0 - 1.0;
+         }
+      }
+      // Get the element face
+      Array<int> faces;
+      Array<int> ori;
+      int face;
+
+      if (is_interior)
+      {
+         face = phyFace;
+      }
+      else
+      {
+         mesh.GetElementFaces(elems_recv[i], faces, ori);
+         face = faces[refFace];
+      }
+
+      Array<int> faceVert;
+      mesh.GetFaceVertices(face, faceVert);
+
+      for (int p=0; p<4; p++)
+      {
+         conn_loc[4*i+p] = faceVert[p];
+      }
+   }
+
+   if (0) // for debugging
+   {
+      int sz = xi_send.Size()/2;
+
+      for (int i = 0; i<sz; i++)
+      {
+         mfem::out << "("<<xi_send[i*(dim-1)]<<","<<xi_send[i*(dim-1)+1]<<"): -> ";
+         for (int j = 0; j<4; j++)
+         {
+            double * vc = mesh.GetVertex(conn_loc[4*i+j]);
+            if (j<3)
+            {
+               mfem::out << "("<<vc[0]<<","<<vc[1]<<","<<vc[2]<<"), ";
+            }
+            else
+            {
+               mfem::out << "("<<vc[0]<<","<<vc[1]<<","<<vc[2]<<") \n " << endl;
+            }
+         }
+      }
+   }
+
+   int sz = xi_send.Size()/2;
+   DenseMatrix coordsm(sz*4, dim);
+   for (int i = 0; i<sz; i++)
+   {
+      for (int j = 0; j<4; j++)
+      {
+         for (int k=0; k<dim; k++)
+         {
+            coordsm(i*4+j,k) = mesh.GetVertex(conn_loc[i*4+j])[k];
+         }
+      }
+   }
+
+   // pass global indices for conn_loc
+   for (int i = 0; i<conn_loc.Size(); i++)
+   {
+      conn_loc[i] = gvert[conn_loc[i]];
+   }
+
+   mycomm.UpdateDestinationProcs();
+   mycomm.Communicate(xyz_recv,xyz2,3,mfem::Ordering::byNODES);
+   mycomm.Communicate(xi_send,xi,2,mfem::Ordering::byVDIM);
+   mycomm.Communicate(s_conn_recv,s_conn2,1,mfem::Ordering::byVDIM);
+   mycomm.Communicate(conn_loc,conn,4,mfem::Ordering::byVDIM);
+   mycomm.Communicate(coordsm,coords,4,mfem::Ordering::byVDIM);
+}
+
+
+void FindPointsInMesh(Mesh & mesh, const Array<int> & gvert, Array<int> & s_conn, const Vector &x1, Vector & xyz, Array<int>& conn,
+                      Vector& xi, DenseMatrix & coords)
+{
+   const int dim = mesh.Dimension();
+   const int np = xyz.Size() / dim;
+   MFEM_VERIFY(np * dim == xyz.Size(), "");
+
+   mesh.EnsureNodes();
+
+   FindPointsGSLIB finder(MPI_COMM_WORLD);
+
+   finder.SetDistanceToleranceForPointsFoundOnBoundary(0.5);
+
+   const double bb_t = 0.5;
+   finder.Setup(mesh, bb_t);
+
+   finder.FindPoints(xyz,mfem::Ordering::byVDIM);
+
+   Array<unsigned int> procs = finder.GetProc();
+
+   /// Return code for each point searched by FindPoints: inside element (0), on
+   /// element boundary (1), or not found (2).
+   Array<unsigned int> codes = finder.GetCode();
+
+   /// Return element number for each point found by FindPoints.
+   Array<unsigned int> elems = finder.GetElem();
+
+   /// Return reference coordinates for each point found by FindPoints.
+   Vector refcrd = finder.GetReferencePosition();
+
+   /// Return distance between the sought and the found point in physical space,
+   /// for each point found by FindPoints.
+   Vector dist = finder.GetDist();
+
+   finder.FreeData();
+
+   MFEM_VERIFY(dist.Size() == np, "");
+   MFEM_VERIFY(refcrd.Size() == np * dim, "");
+   MFEM_VERIFY(elems.Size() == np, "");
+   MFEM_VERIFY(codes.Size() == np, "");
+
+   bool allfound = true;
+   for (auto code : codes)
+      if (code == 2) { allfound = false; }
+
+   MFEM_VERIFY(allfound, "A point was not found");
+
+   // reorder data so that the procs are in ascending order
+   // sort procs and save the permutation
+   std::vector<unsigned int> procs_index(np);
+   std::iota(procs_index.begin(),procs_index.end(),0); //Initializing
+   sort( procs_index.begin(),procs_index.end(), [&](int i,int j){return procs[i]<procs[j];} );
+
+   // map to sorted
+   Array<unsigned int> procs_sorted(np);
+   Array<unsigned int> elems_sorted(np);
+   Vector xyz_sorted(np*dim);
+   Vector refcrd_sorted(np*dim);
+   Array<int> s_conn_sorted(np);
+   for (int i = 0; i<np; i++)
+   {
+      int j = procs_index[i];
+      procs_sorted[i] = procs[j];
+      elems_sorted[i] = elems[j];
+      s_conn_sorted[i] = s_conn[j];
+      for (int d = 0; d<dim; d++)
+      {
+         xyz_sorted(i*dim+d) = xyz(j*dim+d);
+         refcrd_sorted(i*dim+d) = refcrd(j*dim+d);
+      }
+   }
+
+   Array<unsigned int> elems_recv, proc_recv;
+   xyz = xyz_sorted;
+   s_conn = s_conn_sorted;
+   Vector ref_recv;
+   Vector xyz_recv;
+
+   MPICommunicator mycomm(MPI_COMM_WORLD, procs_sorted);
+   mycomm.Communicate(xyz_sorted,xyz_recv,3,mfem::Ordering::byVDIM);
+   mycomm.Communicate(elems_sorted,elems_recv,1,mfem::Ordering::byVDIM);
+   mycomm.Communicate(refcrd_sorted,ref_recv,3,mfem::Ordering::byVDIM);
+
+
+   proc_recv = mycomm.GetOriginProcs();
+
+   int np_loc = elems_recv.Size();
+   Array<int> conn_loc(np_loc*4);
+   Vector xi_send(np_loc*(dim-1));
+   for (int i=0; i<np_loc; ++i)
+   {
+      int refFace, refNormal; 
+      // int refNormalSide;
+      bool is_interior = -1;
+
+      Vector normal = GetNormalVector(mesh, elems_recv[i],
+                                      ref_recv.GetData() + (i*dim),
+                                      refFace, refNormal, is_interior);
+
+      // continue;
+      int phyFace;
+      if (is_interior)
+      {
+         phyFace = -1; // the id of the face that has the closest point
+         FindSurfaceToProject(mesh, elems_recv[i], phyFace); // seems that this works
+
+         Array<int> cbdrVert;
+         mesh.GetFaceVertices(phyFace, cbdrVert);
+         Vector xs(dim);
+         xs[0] = xyz_recv[i*dim + 0];
+         xs[1] = xyz_recv[i*dim + 1];
+         xs[2] = xyz_recv[i*dim + 2];
+
+         Vector xi_tmp(dim-1);
+         // get nodes!
+
+         GridFunction *nodes = mesh.GetNodes();
+         DenseMatrix coord(4,3);
+         for (int j=0; j<4; j++)
+         {
+            for (int k=0; k<3; k++)
+            {
+               coord(j,k) = (*nodes)[cbdrVert[j]*3+k];
+            }
+         }
+         SlaveToMaster(coord, xs, xi_tmp);
+
+         for (int j=0; j<dim-1; ++j)
+         {
+            xi_send[i*(dim-1)+j] = xi_tmp[j];
+         }
+         // now get the projection to the surface
+      }
+      else
+      {
+         Vector faceRefCrd(dim-1);
+         {
+            int fd = 0;
+            for (int j=0; j<dim; ++j)
+            {
+               if (j == refNormal)
+               {
+                  // refNormalSide = (ref_recv[(i*dim) + j] > 0.5); // not used
+               }
+               else
+               {
+                  faceRefCrd[fd] = ref_recv[(i*dim) + j];
+                  fd++;
+               }
+            }
+            MFEM_VERIFY(fd == dim-1, "");
+         }
+
+         for (int j=0; j<dim-1; ++j)
+         {
+            xi_send[i*(dim-1)+j] = faceRefCrd[j]*2.0 - 1.0;
+         }
+      }
+      // Get the element face
+      Array<int> faces;
+      Array<int> ori;
+      int face;
+
+      if (is_interior)
+      {
+         face = phyFace;
+      }
+      else
+      {
+         mesh.GetElementFaces(elems_recv[i], faces, ori);
+         face = faces[refFace];
+      }
+
+      Array<int> faceVert;
+      mesh.GetFaceVertices(face, faceVert);
+
+      for (int p=0; p<4; p++)
+      {
+         conn_loc[4*i+p] = faceVert[p];
+      }
+   }
+
+   if (0) // for debugging
+   {
+      int sz = xi_send.Size()/2;
+
+      for (int i = 0; i<sz; i++)
+      {
+         mfem::out << "("<<xi_send[i*(dim-1)]<<","<<xi_send[i*(dim-1)+1]<<"): -> ";
+         for (int j = 0; j<4; j++)
+         {
+            double * vc = mesh.GetVertex(conn_loc[4*i+j]);
+            if (j<3)
+            {
+               mfem::out << "("<<vc[0]<<","<<vc[1]<<","<<vc[2]<<"), ";
+            }
+            else
+            {
+               mfem::out << "("<<vc[0]<<","<<vc[1]<<","<<vc[2]<<") \n " << endl;
+            }
+         }
+      }
+   }
+
+   int sz = xi_send.Size()/2;
+   DenseMatrix coordsm(sz*4, dim);
+   for (int i = 0; i<sz; i++)
+   {
+      for (int j = 0; j<4; j++)
+      {
+         for (int k=0; k<dim; k++)
+         {
+            coordsm(i*4+j,k) = mesh.GetVertex(conn_loc[i*4+j])[k]+x1[dim*conn_loc[i*4+j]+k];
+         }
+      }
+   }
+
+   // pass global indices for conn_loc
+   for (int i = 0; i<conn_loc.Size(); i++)
+   {
+      conn_loc[i] = gvert[conn_loc[i]];
+   }
+
+   mycomm.UpdateDestinationProcs();
+   mycomm.Communicate(xi_send,xi,2,mfem::Ordering::byVDIM);
+   mycomm.Communicate(conn_loc,conn,4,mfem::Ordering::byVDIM);
+   mycomm.Communicate(coordsm,coords,4,mfem::Ordering::byVDIM);
+}
+
+int get_rank(int tdof, std::vector<int> & tdof_offsets)
+{
+   int size = tdof_offsets.size();
+   if (size == 1) { return 0; }
+   std::vector<int>::iterator up;
+   up=std::upper_bound(tdof_offsets.begin(), tdof_offsets.end(),tdof); //
+   return std::distance(tdof_offsets.begin(),up)-1;
+}
+
+void ComputeTdofOffsets(const ParFiniteElementSpace * pfes,
+                        std::vector<int> & tdof_offsets)
+{
+   MPI_Comm comm = pfes->GetComm();
+   int num_procs;
+   MPI_Comm_size(comm, &num_procs);
+   tdof_offsets.resize(num_procs);
+   int mytoffset = pfes->GetMyTDofOffset();
+   MPI_Allgather(&mytoffset,1,MPI_INT,&tdof_offsets[0],1,MPI_INT,comm);
+}
+
+void ComputeTdofOffsets(MPI_Comm comm, int mytoffset, std::vector<int> & tdof_offsets)
+{
+   int num_procs;
+   MPI_Comm_size(comm,&num_procs);
+   tdof_offsets.resize(num_procs);
+   MPI_Allgather(&mytoffset,1,MPI_INT,&tdof_offsets[0],1,MPI_INT,comm);
+}
+
+void ComputeTdofs(MPI_Comm comm, int mytoffs, std::vector<int> & tdofs)
+{
+   int num_procs;
+   MPI_Comm_size(comm,&num_procs);
+   tdofs.resize(num_procs);
+   MPI_Allgather(&mytoffs,1,MPI_INT,&tdofs,1,MPI_INT,comm);
+}         
+
+
+// Performs Pᵀ * A * P for BlockOperator  P (with blocks as HypreParMatrices)
+// and A a HypreParMatrix, i.e., this handles the special case 
+// where P = [P₁ P₂ ⋅⋅⋅ Pₙ] 
+// C = Pᵀ * A * P 
+void RAP(const HypreParMatrix & A, const BlockOperator & P, 
+         BlockOperator & C)
+{
+   int nblocks = P.NumColBlocks();
+
+   const HypreParMatrix * Pi = nullptr;
+   const HypreParMatrix * Pj = nullptr;
+   HypreParMatrix * PitAPj = nullptr; 
+
+   for (int i = 0; i< nblocks; i++)
+   {
+      if (P.IsZeroBlock(0,i)) continue;
+      Pi = dynamic_cast<const HypreParMatrix*>(&P.GetBlock(0,i));
+      for (int j = 0; j<nblocks; j++)
+      {
+         if (P.IsZeroBlock(0,j)) continue;
+         Pj = dynamic_cast<const HypreParMatrix*>(&P.GetBlock(0,j));
+         if (i == j) 
+         {
+            PitAPj = RAP(&A, Pj); 
+         }
+         else
+         {
+            PitAPj = RAP(Pi, &A, Pj); 
+         }
+         C.SetBlock(i,j,PitAPj);
+      }
+   }
+}
+
+void ParAdd(const BlockOperator & A, const BlockOperator & B, BlockOperator & C)
+{
+   int n = A.NumRowBlocks();
+   int m = A.NumColBlocks();
+   MFEM_VERIFY(B.NumRowBlocks() == n, "Inconsistent number of row blocks");
+   MFEM_VERIFY(B.NumColBlocks() == m, "Inconsistent number of column blocks");
+
+   const HypreParMatrix * a;
+   const HypreParMatrix * b;
+   for (int i = 0; i<n; i++)
+   {
+      for (int j = 0; j<m; j++)
+      {
+         a = nullptr;
+         b = nullptr;
+         if (!A.IsZeroBlock(i,j))
+         {
+            a = dynamic_cast<const HypreParMatrix*>(&A.GetBlock(i,j));
+         }
+         if (!B.IsZeroBlock(i,j))
+         {
+            b = dynamic_cast<const HypreParMatrix*>(&B.GetBlock(i,j));
+         }
+         if (a && b)
+         {
+            C.SetBlock(i,j,ParAdd(a,b));
+         }
+         else if (a)
+         {
+            C.SetBlock(i,j,new HypreParMatrix(*a));
+         }
+         else if (b)
+         {
+            C.SetBlock(i,j,new HypreParMatrix(*b));
+         }
+         else
+         {
+            // do nothing
+         }
+      }
+   }
+}

miniapps/contact/problems/parproblems_util.hpp

@@ -0,0 +1,28 @@
+
+#include "mfem.hpp"
+#include "problems_util.hpp"
+#include "../util/mpicomm.hpp"
+
+// Coordinates in xyz are assumed to be ordered as [X, Y, Z]
+// where X is the list of x-coordinates for all points and so on.
+// conn: connectivity of the target surface elements
+// xi: surface reference cooridnates for the cloest point, involves a linear transformation from [0,1] to [-1,1]
+void FindPointsInMesh(Mesh & mesh, const Array<int> & gvert, const Vector & xyz, const Array<int> & s_conn, Array<int>& conn,
+                      Vector & xyz2, Array<int> & s_conn2, Vector& xi, DenseMatrix & coords);
+
+// somewhat simplified version of the above
+void FindPointsInMesh(Mesh & mesh, const Array<int> & gvert, Array<int> & s_conn, const Vector &x1, Vector & xyz, Array<int>& conn,
+                      Vector& xi, DenseMatrix & coords);                   
+
+int get_rank(int tdof, std::vector<int> & tdof_offsets);
+void ComputeTdofOffsets(const ParFiniteElementSpace * pfes,
+                        std::vector<int> & tdof_offsets);
+void ComputeTdofOffsets(MPI_Comm comm, int mytoffset, std::vector<int> & tdof_offsets);
+void ComputeTdofs(MPI_Comm comm, int mytoffs, std::vector<int> & tdofs);
+
+
+// Performs Pᵀ * A * P for BlockOperator  P (with blocks as HypreParMatrices)
+// and A a HypreParMatrix, i.e., this handles the special case 
+// where P = [P₁ P₂ ⋅⋅⋅ Pₙ] 
+void RAP(const HypreParMatrix & A, const BlockOperator & P, BlockOperator & C);
+void ParAdd(const BlockOperator & A, const BlockOperator & B, BlockOperator & C);

miniapps/contact/problems/problems.cpp

@@ -0,0 +1,367 @@
+#include "problems.hpp"
+
+
+void ElasticityProblem::Init()
+{
+   int dim = mesh->Dimension();
+   fec = new H1_FECollection(order,dim);
+   fes = new FiniteElementSpace(mesh,fec,dim,Ordering::byVDIM);
+   ndofs = fes->GetTrueVSize();
+   mesh->SetNodalFESpace(fes);
+   if (mesh->bdr_attributes.Size())
+   {
+      ess_bdr.SetSize(mesh->bdr_attributes.Max());
+   }
+   ess_bdr = 0; ess_bdr[1] = 1;
+   fes->GetEssentialTrueDofs(ess_bdr,ess_tdof_list);
+   // Solution GridFunction
+   x.SetSpace(fes);  x = 0.0;
+   // RHS
+   b.Update(fes); 
+   // Elasticity operator
+   lambda.SetSize(mesh->attributes.Max()); lambda = 57.6923076923;
+   mu.SetSize(mesh->attributes.Max()); mu = 38.4615384615;
+
+   lambda_cf.UpdateConstants(lambda);
+   mu_cf.UpdateConstants(mu);
+   a = new BilinearForm(fes);
+   a->SetDiagonalPolicy(mfem::Operator::DIAG_ONE);
+   a->AddDomainIntegrator(new ElasticityIntegrator(lambda_cf,mu_cf));
+}
+
+void ElasticityProblem::FormLinearSystem()
+{
+   if (!formsystem) 
+   {
+      formsystem = true;
+      b.Assemble();
+      a->Assemble();
+      a->FormLinearSystem(ess_tdof_list, x, b, A, X, B);
+   }
+}
+void ElasticityProblem::UpdateLinearSystem()
+{
+   if (formsystem)
+   {
+      b.Update();
+      a->Update();
+      formsystem = false;
+   }
+   FormLinearSystem();
+}
+
+ContactProblem::ContactProblem(ElasticityProblem * prob1_, ElasticityProblem * prob2_)
+: prob1(prob1_), prob2(prob2_)
+{
+   // 1. Set up block system
+   Mesh* mesh1 = prob1->GetMesh();
+   int dim = mesh1->Dimension();
+
+   nodes0.SetSpace(mesh1->GetNodes()->FESpace());
+   nodes0 = *mesh1->GetNodes();
+   nodes1 = mesh1->GetNodes();
+
+   Vector delta1(dim);
+   delta1 = 0.0; delta1[0] = 0.1;
+   prob1->SetDisplacementDirichletData(delta1);
+   prob1->FormLinearSystem();
+   
+   Vector delta2(dim);
+   delta2 = 0.0; 
+   prob2->SetDisplacementDirichletData(delta2);
+   prob2->FormLinearSystem();
+
+   int ndof1 = prob1->GetNumDofs();
+   int ndof2 = prob2->GetNumDofs();
+
+   Array<int> offsets(3);
+   offsets[0] = 0;
+   offsets[1] = ndof1;
+   offsets[2] = ndof2;
+   offsets.PartialSum();
+
+   BlockMatrix Kb(offsets);
+   SparseMatrix A1 = prob1->GetOperator();
+   SparseMatrix A2 = prob2->GetOperator();
+
+   Kb.SetBlock(0,0,&A1);
+   Kb.SetBlock(1,1,&A2);
+
+   K = Kb.CreateMonolithic();
+   K->Threshold(0.0);
+   K->SortColumnIndices();
+
+   B = new BlockVector(offsets);
+   B->GetBlock(0).Set(1.0, prob1->GetRHS());
+   B->GetBlock(1).Set(1.0, prob2->GetRHS());
+
+   ComputeContactVertrices();
+}
+
+void ContactProblem::ComputeContactVertrices()
+{
+   if (npoints>0) return;
+   Mesh * mesh2 = prob2->GetMesh();
+   Array<int> vert;
+   for (int b=0; b<mesh2->GetNBE(); b++)
+   {
+      if (mesh2->GetBdrAttribute(b) == 3)
+      {
+         mesh2->GetBdrElementVertices(b, vert);
+         for (auto v : vert)
+         {
+            contact_vertices.insert(v);
+         }
+      }
+   }
+   npoints = contact_vertices.size();
+}
+
+void ContactProblem::ComputeGapFunctionAndDerivatives(const Vector &displ1, 
+                                                      const Vector & displ2)
+{
+   ComputeContactVertrices();
+
+   Mesh * mesh1 = prob1->GetMesh();
+   int dim = mesh1->Dimension();
+   Mesh * mesh2 = prob2->GetMesh();
+
+   int ndof1 = prob1->GetNumDofs();
+   int ndof2 = prob2->GetNumDofs();
+   int ndofs = ndof1 + ndof2;
+
+   int nv1 = mesh1->GetNV();
+   // connectivity of the second mesh
+
+   Array<int> conn2(npoints); 
+   // mesh2->MoveNodes(displ2);
+   Vector xyz(dim * npoints);
+
+   int cnt = 0;
+   for (auto v : contact_vertices)
+   {
+      for (int d = 0; d<dim; d++)
+      {
+         xyz(cnt*dim + d) = mesh2->GetVertex(v)[d]+displ2[v*dim+d];
+      }
+      conn2[cnt] = v + nv1; 
+      cnt++;
+   }
+   
+   MFEM_VERIFY(cnt == npoints, "");
+   gapv.SetSize(npoints*dim);
+
+   // segment reference coordinates of the closest point
+   Vector xi1(npoints*(dim-1));
+   Array<int> conn1(npoints*4);
+   
+   // add(nodes0, displ1, *nodes1);
+   FindPointsInMesh(*mesh1, xyz, conn1, xi1);
+
+   DenseMatrix coordsm(npoints*4, dim);
+   for (int i=0; i<npoints; i++)
+   {
+      for (int j=0; j<4; j++)
+      {
+         for (int k=0; k<dim; k++)
+         {
+            coordsm(i*4+j,k) = mesh1->GetVertex(conn1[i*4+j])[k]+displ1[dim*conn1[i*4+j]+k];
+         }
+      }
+   }
+
+   if (M)
+   {
+      delete M;
+      for (int i = 0; i<dM.Size(); i++)
+      {
+         delete dM[i];
+      }
+      dM.SetSize(0);
+   }
+
+   int h = npoints;
+   M = new SparseMatrix(h,ndofs);
+   dM.SetSize(npoints);
+   for (int i = 0; i<npoints; i++)
+   {
+      dM[i] = new SparseMatrix(ndofs,ndofs);
+   }
+   Assemble_Contact(xyz, xi1, coordsm, conn2, conn1, gapv, *M, dM);
+}
+
+
+double ContactProblem::E(const Vector & d)
+{
+   return 0.5 * K->InnerProduct(d, d) - InnerProduct(d, *B);
+}
+
+void ContactProblem::DdE(const Vector &d, Vector &gradE)
+{
+   gradE.SetSize(K->Height());
+   K->Mult(d, gradE);
+   gradE.Add(-1.0, *B); 
+}
+
+SparseMatrix* ContactProblem::DddE(const Vector &d)
+{
+  return K; 
+}
+
+void ContactProblem::g(const Vector &d, Vector &gd)
+{
+   int ndof1 = prob1->GetNumDofs();
+   int ndof2 = prob2->GetNumDofs();
+   double * data = d.GetData();
+   Vector displ1(data,ndof1);
+   Vector displ2(&data[ndof1],ndof2);
+   if (recompute)
+   {
+      ComputeGapFunctionAndDerivatives(displ1, displ2);
+      recompute = false;
+   }
+
+   gd = GetGapFunction();
+}
+
+SparseMatrix* ContactProblem::Ddg(const Vector &d)
+{
+  return GetJacobian();
+}
+
+SparseMatrix* ContactProblem::lDddg(const Vector &d, const Vector &l)
+{
+   return nullptr; // for now
+}
+
+QPContactProblem::QPContactProblem(ElasticityProblem * prob1_, ElasticityProblem * prob2_)
+: ContactProblem(prob1_,prob2_) 
+{ 
+   ContactProblem::ComputeContactVertrices();
+   dimS = npoints;
+   dimD = K->Height();
+}
+
+// E(d) = 1 / 2 d^T K d + f^T d
+double QPContactProblem::E(const Vector &d)
+{
+   return ContactProblem::E(d);
+}
+
+// gradient(E) = K d + f
+void QPContactProblem::DdE(const Vector &d, Vector &gradE)
+{
+   ContactProblem::DdE(d,gradE);
+}
+
+// Hessian(E) = K
+SparseMatrix* QPContactProblem::DddE(const Vector &d)
+{
+  return ContactProblem::DddE(d);
+}
+
+// g(d) = J * d + g0 >= 0
+void QPContactProblem::g(const Vector &d, Vector &gd)
+{
+  Vector g0;
+  ContactProblem::g(d,g0);
+  M->Mult(d, gd);
+  gd.Add(1.0, g0);
+}
+
+// Jacobian(g) = J
+SparseMatrix* QPContactProblem::Ddg(const Vector &d)
+{
+  return M;
+}
+
+SparseMatrix* QPContactProblem::lDddg(const Vector &d, const Vector &l)
+{
+   return ContactProblem::lDddg(d,l);
+}
+
+
+QPOptContactProblem::QPOptContactProblem(ContactProblem * problem_)
+: problem(problem_)
+{
+   dimU = problem->GetNumDofs();
+   dimM = problem->GetNumConstraints();
+   dimC = problem->GetNumConstraints();
+   block_offsets.SetSize(3);
+   block_offsets[0] = 0;
+   block_offsets[1] = dimU;
+   block_offsets[2] = dimM;
+   block_offsets.PartialSum();
+   ml.SetSize(dimM); ml = 0.0;
+   Vector negone(dimM); negone = -1.0;
+   NegId = new SparseMatrix(negone);
+}
+
+int QPOptContactProblem::GetDimU() { return dimU; }
+
+int QPOptContactProblem::GetDimM() { return dimM; }
+
+int QPOptContactProblem::GetDimC() { return dimC; }
+
+Vector & QPOptContactProblem::Getml() { return ml; }
+
+SparseMatrix * QPOptContactProblem::Duuf(const BlockVector & x)
+{
+   return problem->DddE(x.GetBlock(0));
+}
+
+SparseMatrix * QPOptContactProblem::Dumf(const BlockVector & x)
+{
+   return nullptr;
+}
+
+SparseMatrix * QPOptContactProblem::Dmuf(const BlockVector & x)
+{
+   return nullptr;
+}
+
+SparseMatrix * QPOptContactProblem::Dmmf(const BlockVector & x)
+{
+   return nullptr;
+}
+
+SparseMatrix * QPOptContactProblem::Duc(const BlockVector & x)
+{
+   return problem->Ddg(x.GetBlock(0));
+}
+
+SparseMatrix * QPOptContactProblem::Dmc(const BlockVector & x)
+{
+   return NegId;
+}
+
+SparseMatrix * QPOptContactProblem::lDuuc(const BlockVector & x, const Vector & l)
+{
+   return nullptr;
+}
+
+void QPOptContactProblem::c(const BlockVector &x, Vector & y)
+{
+   Vector g0;
+   problem->g(x.GetBlock(0),g0); // gap function
+   g0.Add(-1.0, x.GetBlock(1));  
+
+   problem->GetJacobian()->Mult(x.GetBlock(0),y);
+   y.Add(1.0, g0);
+}
+
+double QPOptContactProblem::CalcObjective(const BlockVector & x)
+{
+   return problem->E(x.GetBlock(0));
+}
+
+void QPOptContactProblem::CalcObjectiveGrad(const BlockVector & x, BlockVector & y)
+{
+   problem->DdE(x.GetBlock(0), y.GetBlock(0));
+   y.GetBlock(1) = 0.0;
+}
+
+QPOptContactProblem::~QPOptContactProblem()
+{
+   delete NegId;
+}

miniapps/contact/problems/problems.hpp

@@ -0,0 +1,169 @@
+#include "problems_util.hpp"
+
+
+class ElasticityProblem
+{
+private:
+   bool formsystem = false;
+   Mesh * mesh = nullptr;
+   int order;
+   int ndofs;
+   FiniteElementCollection * fec = nullptr;
+   FiniteElementSpace * fes = nullptr;
+   Vector lambda, mu;
+   PWConstCoefficient lambda_cf, mu_cf;
+   Array<int> ess_bdr, ess_tdof_list;
+   BilinearForm *a=nullptr;
+   LinearForm b;
+   GridFunction x;
+   SparseMatrix A;
+   Vector B,X;
+   void Init();
+public:
+   ElasticityProblem(const char *mesh_file , int ref, int order_ = 1) : order(order_) 
+   {
+      mesh = new Mesh(mesh_file,1,1);
+      for (int i = 0; i<ref; i++)
+      {
+         mesh->UniformRefinement();
+      }
+      Init();
+   }
+
+   Mesh * GetMesh() { return mesh; }
+   FiniteElementSpace * GetFESpace() { return fes; }
+   int GetNumDofs() { return ndofs; }
+   SparseMatrix & GetOperator() 
+   {
+      MFEM_VERIFY(formsystem, "System not formed yet. Call FormLinearSystem()"); 
+      return A; 
+   }
+
+   Vector & GetRHS() 
+   {
+      MFEM_VERIFY(formsystem, "System not formed yet. Call FormLinearSystem()"); 
+      return B; 
+   }
+
+   void FormLinearSystem();
+   void UpdateLinearSystem();
+
+   void SetDisplacementDirichletData(const Vector & delta) 
+   {
+      VectorConstantCoefficient delta_cf(delta);
+      x.ProjectBdrCoefficient(delta_cf,ess_bdr);
+   };
+
+   void UpdateDisplacement(const Vector & x_) 
+   {
+      // x = x_;
+      // mesh->MoveVertices(x);
+      // mesh->NodesUpdated();
+   };
+
+   GridFunction & GetDisplacementGridFunction() {return x;};
+   Array<int> & GetEssentialDofs() {return ess_tdof_list;};
+
+   ~ElasticityProblem()
+   {
+      delete a;
+      delete fes;
+      delete fec;
+      delete mesh;
+   }
+};
+
+
+class ContactProblem
+{
+private:
+   ElasticityProblem * prob1 = nullptr;
+   ElasticityProblem * prob2 = nullptr;
+   GridFunction nodes0;
+   GridFunction *nodes1 = nullptr;
+   std::set<int> contact_vertices;
+   bool recompute = true;
+
+protected:
+   int npoints=0;
+   SparseMatrix *K =nullptr;
+   BlockVector *B = nullptr;
+   Vector gapv;
+   Array<SparseMatrix*> dM;
+   SparseMatrix * M=nullptr;
+   void ComputeContactVertrices();
+public:
+   ContactProblem(ElasticityProblem * prob1_, ElasticityProblem * prob2_);
+
+   ElasticityProblem * GetElasticityProblem1() {return prob1;}
+   ElasticityProblem * GetElasticityProblem2() {return prob2;}
+
+   int GetNumDofs() {return K->Height();}
+   int GetNumConstraints() {return npoints;}
+   Vector & GetGapFunction() {return gapv;}
+   SparseMatrix * GetJacobian() {return M;}
+   Array<SparseMatrix*> & GetHessian() {return dM;}
+   void ComputeGapFunctionAndDerivatives(const Vector & displ1, const Vector &displ2);
+
+   virtual double E(const Vector & d);
+   virtual void DdE(const Vector &d, Vector &gradE);
+   virtual SparseMatrix* DddE(const Vector &d);
+   void g(const Vector &d, Vector &gd);
+   virtual SparseMatrix* Ddg(const Vector &d);
+   virtual SparseMatrix* lDddg(const Vector &d, const Vector &l);
+
+   ~ContactProblem()
+   {
+      delete B;
+      delete K;
+      delete M;
+      for (int i = 0; i<dM.Size(); i++)
+      {
+         delete dM[i];
+      }
+   }
+};
+
+
+class QPContactProblem : public ContactProblem
+{
+private:
+   int dimD, dimS;
+public:
+   QPContactProblem(ElasticityProblem * prob1_, ElasticityProblem * prob2_);
+
+   double E(const Vector & d);
+   void DdE(const Vector &d, Vector &gradE);
+   SparseMatrix* DddE(const Vector &d);
+   void g(const Vector &d, Vector &gd);
+   SparseMatrix* Ddg(const Vector &d);
+   SparseMatrix* lDddg(const Vector &d, const Vector &l);
+};
+
+
+class QPOptContactProblem
+{
+private:
+   ContactProblem * problem = nullptr;
+   int dimU, dimM, dimC;
+   Array<int> block_offsets;
+   Vector ml;
+   SparseMatrix * NegId = nullptr;
+public:
+   QPOptContactProblem(ContactProblem * problem_);
+   int GetDimU();
+   int GetDimM();
+   int GetDimC();
+   Vector & Getml();
+   SparseMatrix * Duuf(const BlockVector &);
+   SparseMatrix * Dumf(const BlockVector &);
+   SparseMatrix * Dmuf(const BlockVector &);
+   SparseMatrix * Dmmf(const BlockVector &);
+   SparseMatrix * Duc(const BlockVector &);
+   SparseMatrix * Dmc(const BlockVector &);
+   SparseMatrix * lDuuc(const BlockVector &, const Vector &);
+   void c(const BlockVector &, Vector &);
+   double CalcObjective(const BlockVector &);
+   void CalcObjectiveGrad(const BlockVector &, BlockVector &);
+   ~QPOptContactProblem();
+};

miniapps/contact/problems/problems_util.hpp

@@ -0,0 +1,61 @@
+
+#include "mfem.hpp"
+
+using namespace std;
+using namespace mfem;
+
+void BasisEval(const Vector xi, Vector &N, DenseMatrix &dNdxi); // dNdxi is 2*4
+void BasisEvalDerivs(const Vector xi, Vector& N, DenseMatrix& dNdxi,
+                     DenseMatrix& dN2dxi);
+// returns the vector and matrix form of the shape functions and its derivative
+void BasisVectorDerivs(const Vector xi, DenseMatrix& N, DenseMatrix& dNdxi,
+                       DenseMatrix& ddNdxi);
+void cross(const Vector a, const Vector b, Vector& c);
+// a outer b
+void outer(const Vector a, const Vector b, DenseMatrix& c);
+// dphidxi 2*4
+// coords 4*3
+void ComputeNormal(const DenseMatrix& dphidxi, const DenseMatrix& coords,
+                   Vector& normal, double& nnorm);
+void SlaveToMaster(const DenseMatrix& m_coords, const Vector& s_x, Vector& xi);
+
+// m_coords is expected to be 4 * 3
+void  ComputeGapJacobian(const Vector x_s, const Vector xi,
+                         const DenseMatrix m_coords,
+                         double& gap, Vector& normal, Vector& dgdxm, Vector& dgdxs);
+
+void ComputeGapHessian(const Vector x_s, const Vector xi,
+                       const DenseMatrix m_coords,
+                       DenseMatrix& dg2dx);
+void NodeSegConPairs(const Vector x1, const Vector xi2,
+                     const DenseMatrix coords2,
+                     double& node_g, Vector& node_dg, DenseMatrix& node_dg2);
+// coordsm : (npoints*4, 3) use what class?
+// m_conn: (npoints*4)
+void Assemble_Contact(const Vector x_s,
+                      const Vector xi, const DenseMatrix coordsm, const Array<int> s_conn,
+                      const Array<int> m_conn, Vector& g, SparseMatrix& M,
+                      Array<SparseMatrix *> & dM);
+
+void Assemble_Contact(const Vector x_s,
+                      const Vector xi, const DenseMatrix coordsm, const Array<int> s_conn,
+                      const Array<int> m_conn, Vector & g, SparseMatrix & M1, SparseMatrix & M2, 
+                      Array<SparseMatrix *> & dM11,
+                      Array<SparseMatrix *> & dM12,
+                      Array<SparseMatrix *> & dM21,
+                      Array<SparseMatrix *> & dM22);
+void Assemble_Contact(const Vector x_s,
+                      const Vector xi, const DenseMatrix coordsm, const Array<int> s_conn,
+                      const Array<int> m_conn, Vector & g, SparseMatrix & M1, SparseMatrix & M2,const Array<int> & points_map); 
+
+void FindSurfaceToProject(Mesh& mesh, const int elem, int& cbdrface);
+
+Vector GetNormalVector(Mesh & mesh, const int elem, const double *ref,
+                       int & refFace, int & refNormal, bool & interior);
+int GetHexVertex(int cdim, int c, int fa, int fb, Vector & refCrd);
+
+// Coordinates in xyz are assumed to be ordered as [X, Y, Z]
+// where X is the list of x-coordinates for all points and so on.
+// conn: connectivity of the target surface elements
+// xi: surface reference cooridnates for the cloest point, involves a linear transformation from [0,1] to [-1,1]
+void FindPointsInMesh(Mesh & mesh, Vector const& xyz, Array<int>& conn, Vector& xi);

miniapps/contact/util/mpicomm.cpp

@@ -0,0 +1,530 @@
+#include "mpicomm.hpp"
+#include "util.hpp"
+
+
+MPICommunicator::MPICommunicator(MPI_Comm comm_, int offset_, int gsize)
+: comm(comm_), offset(offset_)
+{
+   MPI_Comm_size(comm,&num_procs);
+   MPI_Comm_rank(comm,&myid);
+   offsets.resize(num_procs);
+   MPI_Allgather(&offset,1,MPI_INT,&offsets[0],1,MPI_INT,comm);
+   lsize = (myid == num_procs-1) ? gsize - offsets[myid]
+                                : offsets[myid+1]-offsets[myid];
+
+   send_count.SetSize(num_procs); send_count = 0;
+   send_displ.SetSize(num_procs); send_displ = 0;
+   recv_count.SetSize(num_procs); recv_count = 0;
+   recv_displ.SetSize(num_procs); recv_displ = 0;
+} 
+
+MPICommunicator::MPICommunicator(MPI_Comm comm_, Array<unsigned int> & destination_procs_)
+: comm(comm_), destination_procs(destination_procs_)
+{
+   MPI_Comm_size(comm,&num_procs);
+   MPI_Comm_rank(comm,&myid);   
+   send_count.SetSize(num_procs); 
+   send_displ.SetSize(num_procs); 
+   recv_count.SetSize(num_procs); 
+   recv_displ.SetSize(num_procs); 
+   resetcounts();
+}
+
+
+int MPICommunicator::get_rank(int dof)
+{
+   if (num_procs == 1) { return 0; }
+   std::vector<int>::iterator up;
+   up=std::upper_bound(offsets.begin(), offsets.end(),dof);
+   return std::distance(offsets.begin(),up)-1;
+}
+
+
+void MPICommunicator::Communicate(const Vector & x_s, Vector & x_r, int vdim, int ordering)
+{
+   int npts = x_s.Size()/vdim;
+   MFEM_VERIFY(npts == destination_procs.Size(), "Inconsistent number of points to be send");
+
+   // construct send count
+   for (int i = 0; i<npts; i++)
+   {
+      int rank = destination_procs[i];
+      send_count[rank] += vdim + 1; // including the sending processor id
+   }
+
+   // 2. Compute recv_count
+   MPI_Alltoall(&send_count[0],1,MPI_INT,&recv_count[0],1,MPI_INT,comm);
+
+   // 3. Compute displacements
+   for (int k=0; k<num_procs-1; k++)
+   {
+      send_displ[k+1] = send_displ[k] + send_count[k];
+      recv_displ[k+1] = recv_displ[k] + recv_count[k];
+   }
+   int sbuff_size = send_count.Sum();
+   int rbuff_size = recv_count.Sum();
+
+   // 4. Allocate memory and fill in send buffers
+   Array<double> sendvals(sbuff_size);  sendvals = 0.0;
+   Array<int> sendoffs(num_procs); sendoffs = 0;
+   for (int i = 0; i<npts; i++)
+   {
+      int rank = destination_procs[i];
+      int j = send_displ[rank] + sendoffs[rank];
+      sendoffs[rank] += vdim+1;
+      sendvals[j] = (double)myid;
+      for (int k = 0; k<vdim; k++)
+      {
+         int kk = (ordering == mfem::Ordering::byNODES) ? k*npts+i : i*vdim + k;
+         sendvals[j+k+1] = x_s(kk);
+      }
+   }
+
+   // 5. Communication
+   Array<double> recvvals(rbuff_size);
+
+   double * sendvals_ptr = nullptr;
+   double * recvvals_ptr = nullptr;
+   if (sbuff_size !=0 ) { sendvals_ptr = &sendvals[0]; }   
+   if (rbuff_size !=0 ) { recvvals_ptr = &recvvals[0]; }
+
+   MPI_Alltoallv(sendvals_ptr, send_count, send_displ, MPI_DOUBLE, recvvals_ptr,
+                 recv_count, recv_displ, MPI_DOUBLE, comm);
+
+   // 6. Unpack
+   int n = rbuff_size/(vdim+1);
+   origin_procs.SetSize(n);
+   x_r.SetSize(vdim*n);
+   for (int i = 0; i<n; i++)
+   {
+      origin_procs[i] = (unsigned int)recvvals[(vdim+1)*i];
+      for (int j=0; j<vdim; j++)
+      {
+         int kk = (ordering == mfem::Ordering::byNODES) ? j*n+i : i*vdim + j;
+         x_r(kk) = recvvals[(vdim+1)*i + j+1];
+      }
+   }
+   resetcounts();
+}
+
+void MPICommunicator::Communicate(const Array<unsigned int> & x_s, Array<unsigned int> & x_r, int vdim, int ordering)
+{
+   int npts = x_s.Size()/vdim;
+   MFEM_VERIFY(npts == destination_procs.Size(), "Inconsistent number of points to be send");
+
+   // construct send count
+   for (int i = 0; i<npts; i++)
+   {
+      int rank = destination_procs[i];
+      send_count[rank] += vdim + 1; // including the sending processor id
+   }
+
+   // 2. Compute recv_count
+   MPI_Alltoall(&send_count[0],1,MPI_INT,&recv_count[0],1,MPI_INT,comm);
+
+   // 3. Compute displacements
+   for (int k=0; k<num_procs-1; k++)
+   {
+      send_displ[k+1] = send_displ[k] + send_count[k];
+      recv_displ[k+1] = recv_displ[k] + recv_count[k];
+   }
+   int sbuff_size = send_count.Sum();
+   int rbuff_size = recv_count.Sum();
+
+   // 4. Allocate memory and fill in send buffers
+   Array<unsigned int> sendvals(sbuff_size);  sendvals = 0.0;
+   Array<int> sendoffs(num_procs); sendoffs = 0;
+   for (int i = 0; i<npts; i++)
+   {
+      int rank = destination_procs[i];
+      int j = send_displ[rank] + sendoffs[rank];
+      sendoffs[rank] += vdim+1;
+      sendvals[j] = myid;
+      for (int k = 0; k<vdim; k++)
+      {
+         int kk = (ordering == mfem::Ordering::byNODES) ? k*npts+i : i*vdim + k;
+         sendvals[j+k+1] = x_s[kk];
+      }
+   }
+
+   // 5. Communication
+   Array<unsigned int> recvvals(rbuff_size);
+
+   unsigned int * sendvals_ptr = nullptr;
+   unsigned int * recvvals_ptr = nullptr;
+   if (sbuff_size !=0 ) { sendvals_ptr = &sendvals[0]; }   
+   if (rbuff_size !=0 ) { recvvals_ptr = &recvvals[0]; }
+
+   MPI_Alltoallv(sendvals_ptr, send_count, send_displ, MPI_UNSIGNED, recvvals_ptr,
+                 recv_count, recv_displ, MPI_UNSIGNED, comm);
+
+   // 6. Unpack
+   int n = rbuff_size/(vdim+1);
+   origin_procs.SetSize(n);
+   x_r.SetSize(vdim*n);
+   for (int i = 0; i<n; i++)
+   {
+      origin_procs[i] = recvvals[(vdim+1)*i];
+      for (int j=0; j<vdim; j++)
+      {
+         int kk = (ordering == mfem::Ordering::byNODES) ? j*n+i : i*vdim + j;
+         x_r[kk] = recvvals[(vdim+1)*i + j+1];
+      }
+   }
+   resetcounts();
+}
+
+void MPICommunicator::Communicate(const Array<int> & x_s, Array<int> & x_r, int vdim, int ordering)
+{
+   int npts = x_s.Size()/vdim;
+   MFEM_VERIFY(npts == destination_procs.Size(), "Inconsistent number of points to be send");
+
+   // construct send count
+   for (int i = 0; i<npts; i++)
+   {
+      int rank = destination_procs[i];
+      send_count[rank] += vdim + 1; // including the sending processor id
+   }
+
+   // 2. Compute recv_count
+   MPI_Alltoall(&send_count[0],1,MPI_INT,&recv_count[0],1,MPI_INT,comm);
+
+   // 3. Compute displacements
+   for (int k=0; k<num_procs-1; k++)
+   {
+      send_displ[k+1] = send_displ[k] + send_count[k];
+      recv_displ[k+1] = recv_displ[k] + recv_count[k];
+   }
+   int sbuff_size = send_count.Sum();
+   int rbuff_size = recv_count.Sum();
+
+   // 4. Allocate memory and fill in send buffers
+   Array<int> sendvals(sbuff_size);  sendvals = 0.0;
+   Array<int> sendoffs(num_procs); sendoffs = 0;
+   for (int i = 0; i<npts; i++)
+   {
+      int rank = destination_procs[i];
+      int j = send_displ[rank] + sendoffs[rank];
+      sendoffs[rank] += vdim+1;
+      sendvals[j] = myid;
+      for (int k = 0; k<vdim; k++)
+      {
+         int kk = (ordering == mfem::Ordering::byNODES) ? k*npts+i : i*vdim + k;
+         sendvals[j+k+1] = x_s[kk];
+      }
+   }
+
+   // 5. Communication
+   Array<int> recvvals(rbuff_size);
+
+   int * sendvals_ptr = nullptr;
+   int * recvvals_ptr = nullptr;
+   if (sbuff_size !=0 ) { sendvals_ptr = &sendvals[0]; }   
+   if (rbuff_size !=0 ) { recvvals_ptr = &recvvals[0]; }
+
+   MPI_Alltoallv(sendvals_ptr, send_count, send_displ, MPI_INT, recvvals_ptr,
+                 recv_count, recv_displ, MPI_INT, comm);
+
+   // 6. Unpack
+   int n = rbuff_size/(vdim+1);
+   origin_procs.SetSize(n);
+   x_r.SetSize(vdim*n);
+   for (int i = 0; i<n; i++)
+   {
+      origin_procs[i] = (unsigned int)recvvals[(vdim+1)*i];
+      for (int j=0; j<vdim; j++)
+      {
+         int kk = (ordering == mfem::Ordering::byNODES) ? j*n+i : i*vdim + j;
+         x_r[kk] = recvvals[(vdim+1)*i + j+1];
+      }
+   }
+   resetcounts();
+}
+
+void MPICommunicator::Communicate(const DenseMatrix & A_s, DenseMatrix & A_r, int vdim, int ordering)
+{
+   // matrix width corresponds to dim coordinates
+   // matrix rows might include vdim copies
+   int npts = A_s.Height()/vdim;
+   int dim = A_s.Width();
+   MFEM_VERIFY(npts == destination_procs.Size(), "Inconsistent number of points to be send");
+
+   // construct send count
+   for (int i = 0; i<npts; i++)
+   {
+      int rank = destination_procs[i];
+      send_count[rank] += dim*vdim + 1; // including the sending processor id
+   }
+
+   // 2. Compute recv_count
+   MPI_Alltoall(&send_count[0],1,MPI_INT,&recv_count[0],1,MPI_INT,comm);
+
+   // 3. Compute displacements
+   for (int k=0; k<num_procs-1; k++)
+   {
+      send_displ[k+1] = send_displ[k] + send_count[k];
+      recv_displ[k+1] = recv_displ[k] + recv_count[k];
+   }
+   int sbuff_size = send_count.Sum();
+   int rbuff_size = recv_count.Sum();
+
+   // 4. Allocate memory and fill in send buffers
+   Array<double> sendvals(sbuff_size);  sendvals = 0.0;
+   Array<int> sendoffs(num_procs); sendoffs = 0;
+   for (int i = 0; i<npts; i++)
+   {
+      int rank = destination_procs[i];
+      int j = send_displ[rank] + sendoffs[rank];
+      sendoffs[rank] += dim*vdim+1;
+      sendvals[j] = myid;
+      for (int k = 0; k<vdim; k++)
+      {
+         int kk = (ordering == mfem::Ordering::byNODES) ? k*npts+i : i*vdim + k;
+         for (int d=0; d<dim; d++)
+         {
+            sendvals[j+k*dim+d+1] = A_s(kk,d);
+         }
+      }
+   }
+   // 5. Communication
+   Array<double> recvvals(rbuff_size);
+
+   double * sendvals_ptr = nullptr;
+   double * recvvals_ptr = nullptr;
+   if (sbuff_size !=0 ) { sendvals_ptr = &sendvals[0]; }   
+   if (rbuff_size !=0 ) { recvvals_ptr = &recvvals[0]; }
+
+   MPI_Alltoallv(sendvals_ptr, send_count, send_displ, MPI_DOUBLE, recvvals_ptr,
+                 recv_count, recv_displ, MPI_DOUBLE, comm);
+
+   // 6. Unpack
+   int n = rbuff_size/(dim*vdim+1);
+   origin_procs.SetSize(n);
+   A_r.SetSize(vdim*n,dim);
+
+   for (int i = 0; i<n; i++)
+   {
+      origin_procs[i] = (unsigned int)recvvals[(dim*vdim+1)*i];
+      for (int j=0; j<vdim; j++)
+      {
+         int kk = (ordering == mfem::Ordering::byNODES) ? j*n+i : i*vdim + j;
+         for (int d=0; d<dim; d++)
+         {
+            A_r(kk,d) = recvvals[(dim*vdim+1)*i + j*dim + d+1];
+         }
+      }
+   }
+   resetcounts();
+
+}
+
+
+void MPICommunicator::Communicate(const SparseMatrix & mat_s , SparseMatrix & mat_r)
+{
+   // 1. Compute send_count 
+   int n = mat_s.NumRows();
+   for (int i = 0; i<n; i++)
+   {
+      int rsize = mat_s.RowSize(i);
+      if (rsize == 0) continue;
+      int rank = get_rank(i);
+      send_count[rank] += rsize+2;
+   }
+   // 2. Compute recv_count
+   MPI_Alltoall(&send_count[0],1,MPI_INT,&recv_count[0],1,MPI_INT,comm);
+
+   // 3. Compute displacements
+   for (int k=0; k<num_procs-1; k++)
+   {
+      send_displ[k+1] = send_displ[k] + send_count[k];
+      recv_displ[k+1] = recv_displ[k] + recv_count[k];
+   }
+   int sbuff_size = send_count.Sum();
+   int rbuff_size = recv_count.Sum();
+
+   // 4. Allocate memory and fill in send buffers
+   Array<double> sendvals(sbuff_size);  sendvals = 0.0;
+   Array<int> sendcols(sbuff_size);  sendcols = 0;
+   Array<int> sendoffs(num_procs); sendoffs = 0;
+   Array<int> cols;
+   Vector vals;
+   for (int i = 0; i<n; i++)
+   {
+      int rsize = mat_s.RowSize(i);
+      if (rsize == 0) continue;
+      int rank = get_rank(i);
+      int j = send_displ[rank] + sendoffs[rank];
+      mat_s.GetRow(i,cols,vals);
+      sendoffs[rank] += rsize+2;
+      sendvals[j] = (double)i;
+      sendvals[j+1] = (double)rsize;
+      sendcols[j] = i;
+      sendcols[j+1] = rsize;
+      for (int l=0; l<rsize ; l++)
+      {
+         sendvals[j+l+2] = vals[l];
+         sendcols[j+l+2] = cols[l];
+      }
+   }
+
+   // 5. Communication
+   Array<double> recvvals(rbuff_size);
+   Array<int> recvcols(rbuff_size);
+
+   double * sendvals_ptr = nullptr;
+   double * recvvals_ptr = nullptr;
+   int * sendcols_ptr = nullptr;
+   int * recvcols_ptr = nullptr;
+   if (sbuff_size !=0 ) 
+   {
+      sendvals_ptr = &sendvals[0]; 
+      sendcols_ptr = &sendcols[0]; 
+   }   
+   if (rbuff_size !=0 ) 
+   {
+      recvvals_ptr = &recvvals[0]; 
+      recvcols_ptr = &recvcols[0]; 
+   }
+
+   MPI_Alltoallv(sendvals_ptr, send_count, send_displ, MPI_DOUBLE, recvvals_ptr,
+                 recv_count, recv_displ, MPI_DOUBLE, comm);
+
+   MPI_Alltoallv(sendcols_ptr, send_count, send_displ, MPI_INT, recvcols_ptr,
+                 recv_count, recv_displ, MPI_INT, comm);
+
+   // 6. Unpack and store to the output SparseMatrix
+   MFEM_VERIFY(mat_r.Height() == lsize, "Inconsistent row size of output SparseMatrix");
+   MFEM_VERIFY(mat_r.Width() == mat_s.Width(), "Inconsistent column size of output SparseMatrix");
+
+   int counter = 0;
+   while (counter < rbuff_size)
+   {
+      int row = recvcols[counter] - offset;
+      int size = recvcols[counter+1];
+      vals.SetSize(size);
+      cols.SetSize(size);
+      for (int i = 0; i<size; i++)
+      {
+         vals[i] = recvvals[counter+2 + i];
+         cols[i] = recvcols[counter+2 + i];
+      }
+      mat_r.AddRow(row,cols,vals);
+      counter += size+2; 
+   }
+   MFEM_VERIFY(counter == rbuff_size, "inconsistent rbuff size");
+   mat_r.Finalize();
+   mat_r.SortColumnIndices();
+   resetcounts();
+}
+
+void MPICommunicator::Communicate(const Array<SparseMatrix*> & vmat_s, Array<SparseMatrix*> & vmat_r)
+{
+   // 1. Compute send_count 
+   for (int k = 0; k<vmat_s.Size(); k++)
+   {
+      if (!vmat_s[k]) continue;
+      if (vmat_s[k]->NumNonZeroElems() == 0) continue;
+      int nrows = vmat_s[k]->NumRows();
+      for (int i = 0; i<nrows; i++)
+      {
+         int rsize = vmat_s[k]->RowSize(i);
+         if (rsize == 0) continue;
+         int rank = get_rank(i);
+         send_count[rank] += rsize+3;
+      }
+   } 
+
+   // 2. Compute recv_count
+   MPI_Alltoall(&send_count[0],1,MPI_INT,&recv_count[0],1,MPI_INT,comm);
+   
+   // 3. Compute displacements
+   for (int k=0; k<num_procs-1; k++)
+   {
+      send_displ[k+1] = send_displ[k] + send_count[k];
+      recv_displ[k+1] = recv_displ[k] + recv_count[k];
+   }
+   int sbuff_size = send_count.Sum();
+   int rbuff_size = recv_count.Sum();
+
+   // 4. Allocate memory and fill in send buffers
+   Array<double> sendvals(sbuff_size);  sendvals = 0.0;
+   Array<int> sendcols(sbuff_size);  sendcols = 0;
+   Array<int> sendoffs(num_procs); sendoffs = 0;
+   for (int k = 0; k<vmat_s.Size(); k++)
+   {
+      if (!vmat_s[k]) continue;
+      if (vmat_s[k]->NumNonZeroElems() == 0) continue;
+      int nrows = vmat_s[k]->NumRows();
+      for (int i = 0; i<nrows; i++)
+      {
+         int rsize = vmat_s[k]->RowSize(i);
+         if (rsize == 0) continue;
+         int rank = get_rank(i);
+         int j = send_displ[rank] + sendoffs[rank];
+         Array<int> cols;
+         Vector vals;
+         vmat_s[k]->GetRow(i,cols,vals);
+         sendoffs[rank] += rsize+3;
+         sendvals[j] = (double)k;
+         sendvals[j+1] = (double)i;
+         sendvals[j+2] = (double)rsize;
+         sendcols[j] = k;
+         sendcols[j+1] = i;
+         sendcols[j+2] = rsize;
+         for (int l=0; l<rsize ; l++)
+         {
+            sendvals[j+l+3] = vals[l];
+            sendcols[j+l+3] = cols[l];
+         }
+      }
+   }
+   // 5. Communication
+   Array<double> recvvals(rbuff_size);
+   Array<int> recvcols(rbuff_size);
+   double * sendvals_ptr = nullptr;
+   double * recvvals_ptr = nullptr;
+   int * sendcols_ptr = nullptr;
+   int * recvcols_ptr = nullptr;
+   if (sbuff_size !=0 ) 
+   {
+      sendvals_ptr = &sendvals[0]; 
+      sendcols_ptr = &sendcols[0]; 
+   }   
+   if (rbuff_size !=0 ) 
+   {
+      recvvals_ptr = &recvvals[0]; 
+      recvcols_ptr = &recvcols[0]; 
+   }
+
+   MPI_Alltoallv(sendvals_ptr, send_count, send_displ, MPI_DOUBLE, recvvals_ptr,
+                 recv_count, recv_displ, MPI_DOUBLE,comm);
+
+   MPI_Alltoallv(sendcols_ptr, send_count, send_displ, MPI_INT, recvcols_ptr,
+                 recv_count, recv_displ, MPI_INT,comm);
+
+   // 6. Unpack and store to the output SparseMatrix
+   int counter = 0;
+   while (counter < rbuff_size)
+   {
+      int npt = recvcols[counter];
+      int row = recvcols[counter+1] - offset;
+      int size = recvcols[counter+2];
+      Vector vals(size);
+      Array<int> cols(size);
+      for (int i = 0; i<size; i++)
+      {
+         vals[i] = recvvals[counter+3 + i];
+         cols[i] = recvcols[counter+3 + i];
+      }
+      vmat_r[npt]->AddRow(row,cols,vals);
+      counter += size+3; 
+   }
+   MFEM_VERIFY(counter == rbuff_size, "inconsistent size");
+
+   for (int i = 0; i<vmat_r.Size(); i++)
+   {
+      vmat_r[i]->Finalize();
+      vmat_r[i]->SortColumnIndices();
+   }
+   resetcounts();
+}

miniapps/contact/util/mpicomm.hpp

@@ -0,0 +1,47 @@
+#include "mfem.hpp"
+
+using namespace std;
+using namespace mfem;
+
+
+class MPICommunicator
+{
+private:
+    MPI_Comm comm;
+    int myid, num_procs;
+    Array<unsigned int > origin_procs;
+    Array<unsigned int > destination_procs;
+    int offset, lsize;
+    std::vector<int> offsets; 
+    Array<int> send_count;
+    Array<int> send_displ;
+    Array<int> recv_count;
+    Array<int> recv_displ;
+    void resetcounts()
+    {
+        send_count = 0;
+        send_displ = 0;
+        recv_count = 0;
+        recv_displ = 0;
+    }
+
+public:
+    MPICommunicator(MPI_Comm comm_, int offset_, int gsize);
+    MPICommunicator(MPI_Comm comm_, Array<unsigned int> & destination_procs_);
+
+    int get_rank(int dof);
+
+    Array<unsigned int> & GetOriginProcs() {return origin_procs;}
+    void UpdateDestinationProcs() 
+    {   
+        destination_procs.SetSize(origin_procs.Size());
+        destination_procs = origin_procs;
+        resetcounts();
+    }
+    void Communicate(const Vector & x_s, Vector & x_r, int vdim, int ordering);
+    void Communicate(const Array<int> & x_s, Array<int> & x_r, int vdim, int ordering);
+    void Communicate(const DenseMatrix & A_s, DenseMatrix & A_r, int vdim, int ordering);
+    void Communicate(const Array<unsigned int> & x_s, Array<unsigned int> & x_r, int vdim, int ordering);
+    void Communicate(const SparseMatrix & mat_s , SparseMatrix & mat_r);
+    void Communicate(const Array<SparseMatrix*> & vmat_s, Array<SparseMatrix*> & vmat_r);
+};

miniapps/contact/util/util.cpp

@@ -0,0 +1,171 @@
+#include "util.hpp"
+
+
+void PrintVertex(Mesh * mesh, int vertex)
+{
+   Array<int> vertices;
+   mfem::out << "vertex: " << vertex << ":  ";
+   double *coords = mesh->GetVertex(vertex);
+   mfem::out << "(" << coords[0] << ", " << coords[1] << ", " << coords[2] << ") \n";
+}
+
+void PrintElementVertices(Mesh * mesh, int elem)
+{
+   Array<int> vertices;
+   mfem::out <<  "elem: " << elem << ". Vertices = \n" ;
+   mesh->GetElementVertices(elem,vertices);
+   for (int i = 0; i<vertices.Size(); i++)
+   {
+      PrintVertex(mesh,vertices[i]);
+   }
+   mfem::out << endl;
+}
+
+void PrintFaceVertices(Mesh * mesh, int face)
+{
+   Array<int> vertices;
+   mfem::out << "face: " << face << ". Vertices = \n" ;
+   mesh->GetFaceVertices(face,vertices);
+   for (int i = 0; i<vertices.Size(); i++)
+   {
+      PrintVertex(mesh,vertices[i]);
+   }
+   mfem::out << endl;
+}
+
+void PrintSet(const std::set<int> & a, const char *aname)
+{
+   mfem::out << aname << " = " ;
+   for (std::set<int>::iterator it = a.begin(); it!= a.end(); it++)
+   {
+      mfem::out << *it << "  ";
+   }
+   mfem::out << endl;
+}
+
+void PrintVector(const Vector & a, const char *aname)
+{
+   int sz = a.Size();
+   mfem::out << aname << " = " ;
+   for (int i = 0; i<sz; i++)
+   {
+      mfem::out << a[i] << "  ";
+   }
+   mfem::out << endl;
+}
+
+void PrintVertex(Mesh * mesh, int vertex,  int printid)
+{
+   int myid = Mpi::WorldRank();
+   if (myid == printid)
+   {
+      mfem::out << "myid = " << myid <<":   " <<  "vertex: " << vertex << ":  ";
+      double *coords = mesh->GetVertex(vertex);
+      mfem::out << "(" << coords[0] << ", " << coords[1] << ", " << coords[2] << ")\n";
+   }
+}
+
+void PrintElementVertices(Mesh * mesh, int elem,  int printid)
+{
+   int myid = Mpi::WorldRank();
+   Array<int> vertices;
+   if (myid == printid)
+   {
+      mfem::out << "myid = " << myid <<":   " <<  "elem: " << elem <<
+                ". Vertices = \n" ;
+      mesh->GetElementVertices(elem,vertices);
+      for (int i = 0; i<vertices.Size(); i++)
+      {
+         PrintVertex(mesh,vertices[i],printid);
+      }
+      mfem::out << endl;
+   }
+}
+
+void PrintFaceVertices(Mesh * mesh, int face,  int printid)
+{
+   int myid = Mpi::WorldRank();
+   Array<int> vertices;
+   if (myid == printid)
+   {
+      mfem::out << "myid = " << myid <<":   " <<  "face: " << face <<
+                ". Vertices = \n" ;
+      mesh->GetFaceVertices(face,vertices);
+      for (int i = 0; i<vertices.Size(); i++)
+      {
+         PrintVertex(mesh,vertices[i],printid);
+      }
+      mfem::out << endl;
+   }
+}
+
+
+void PrintSet(const std::set<int> & a, const char *aname,  int printid)
+{
+   int myid = Mpi::WorldRank();
+   if (myid == printid)
+   {
+      mfem::out << "myid = " << myid <<":   " << aname << " = " ;
+      for (std::set<int>::iterator it = a.begin(); it!= a.end(); it++)
+      {
+         mfem::out << *it << "  ";
+      }
+      mfem::out << endl;
+   }
+}
+
+void PrintVector(const Vector & a, const char *aname,  int printid)
+{
+   int myid = Mpi::WorldRank();
+   if (myid == printid)
+   {
+      int sz = a.Size();
+      mfem::out << "myid = " << myid <<":   " << aname << " = " ;
+      for (int i = 0; i<sz; i++)
+      {
+         mfem::out << a[i] << "  ";
+      }
+      mfem::out << endl;
+   }
+}
+
+void PrintVector(const std::vector<int> & a, const char *aname,  int printid)
+{
+   int myid = Mpi::WorldRank();
+   if (myid == printid)
+   {
+      int sz = a.size();
+      mfem::out << "myid = " << myid <<":   " << aname << " = " ;
+      for (int i = 0; i<sz; i++)
+      {
+         mfem::out << a[i] << "  ";
+      }
+      mfem::out << endl;
+   }
+}
+
+void PrintVector(const std::vector<unsigned int> & a, const char *aname,  int printid)
+{
+   int myid = Mpi::WorldRank();
+   if (myid == printid)
+   {
+      int sz = a.size();
+      mfem::out << "myid = " << myid <<":   " << aname << " = " ;
+      for (int i = 0; i<sz; i++)
+      {
+         mfem::out << a[i] << "  ";
+      }
+      mfem::out << endl;
+   }
+}
+
+void PrintSparseMatrix(const SparseMatrix & a, const char *aname,  int printid)
+{
+   int myid = Mpi::WorldRank();
+   if (myid == printid)
+   {
+      mfem::out << "myid = " << myid <<":   " << aname << " = " ;
+      a.PrintMatlab(mfem::out);
+   }
+   mfem::out << endl;
+}

miniapps/contact/util/util.hpp

@@ -0,0 +1,46 @@
+#include "mfem.hpp"
+
+using namespace std;
+using namespace mfem;
+
+void PrintVertex(Mesh * mesh, int vertex);
+void PrintElementVertices(Mesh * mesh, int elem);
+void PrintFaceVertices(Mesh * mesh, int face);
+template <class T>
+void PrintArray(const Array<T> & a, const char *aname)
+{
+   int sz = a.Size();
+   mfem::out << aname << " = " ;
+   for (int i = 0; i<sz; i++)
+   {
+      mfem::out << a[i] << "  ";
+   }
+   mfem::out << endl;
+}
+void PrintSet(const std::set<int> & a, const char *aname);
+void PrintVector(const Vector & a, const char *aname);
+
+// for parallel 
+void PrintVertex(Mesh * mesh, int vertex,  int printid);
+void PrintElementVertices(Mesh * mesh, int elem,  int printid);
+void PrintFaceVertices(Mesh * mesh, int face,  int printid);
+template <class T>
+void PrintArray(const Array<T> & a, const char *aname,  int printid)
+{
+   int myid = Mpi::WorldRank();
+   if (myid == printid)
+   {
+      int sz = a.Size();
+      mfem::out << "myid = " << myid <<":   " << aname << " = " ;
+      for (int i = 0; i<sz; i++)
+      {
+         mfem::out << a[i] << "  ";
+      }
+      mfem::out << endl;
+   }
+}
+void PrintSet(const std::set<int> & a, const char *aname,  int printid);
+void PrintVector(const Vector & a, const char *aname,  int printid);
+void PrintVector(const std::vector<int> & a, const char *aname,  int printid);
+void PrintVector(const std::vector<unsigned int> & a, const char *aname,  int printid);
+void PrintSparseMatrix(const SparseMatrix & a, const char *aname,  int printid);