EshelbianContact.cpp

Go to the documentation of this file.

/**
 * @file EshelbianContact.cpp
 * @brief
 * @date 2023-05-13
 *
 * @copyright Copyright (c) 2023
 *
 */
 
#define SINGULARITY
#include <MoFEM.hpp>
using namespace MoFEM;
 
#include <CGGTonsorialBubbleBase.hpp>
 
#include <EshelbianPlasticity.hpp>
 
namespace ContactOps {
 
using EntData = EntitiesFieldData::EntData;
using BoundaryEle = FaceElementForcesAndSourcesCore;
using BoundaryEleOp = BoundaryEle::UserDataOperator;
using SideEle = FaceElementForcesAndSourcesCoreOnSide;
 
using AssemblyBoundaryEleOp =
    FormsIntegrators<BoundaryEleOp>::Assembly<A>::OpBase;
 
double cn_contact = 1;
double alpha_contact_const = 1;
double alpha_contact_quadratic = 1;
double scale = 1;
 
double airplane_ray_distance = 1; // thi is point from which plane send ray.
                                  // This is multiple of elem radius.
 
} // namespace ContactOps
 
#ifdef ENABLE_PYTHON_BINDING
  #include <boost/python.hpp>
  #include <boost/python/def.hpp>
  #include <boost/python/numpy.hpp>
namespace bp = boost::python;
namespace np = boost::python::numpy;
 
  #pragma message "With ENABLE_PYTHON_BINDING"
 
#else
 
  #pragma message "Without ENABLE_PYTHON_BINDING"
 
#endif
 
#include <ContactOps.hpp>
 
#include <EshelbianContact.hpp>
 
extern "C" {
void tricircumcenter3d_tp(double a[3], double b[3], double c[3],
                          double circumcenter[3], double *xi, double *eta);
}
 
namespace EshelbianPlasticity {
 
struct ContactSDFPython : public ContactOps::SDFPython {
  using ContactOps::SDFPython::SDFPython;
};
 
boost::shared_ptr<ContactSDFPython> setupContactSdf() {
  boost::shared_ptr<ContactSDFPython> sdf_python_ptr;
 
#ifdef ENABLE_PYTHON_BINDING
 
  auto file_exists = [](std::string myfile) {
    std::ifstream file(myfile.c_str());
    if (file) {
      return true;
    }
    return false;
  };
 
  char sdf_file_name[255] = "sdf.py";
  CHKERR PetscOptionsGetString(PETSC_NULLPTR, PETSC_NULLPTR, "-sdf_file",
                               sdf_file_name, 255, PETSC_NULLPTR);
 
  if (file_exists(sdf_file_name)) {
    MOFEM_LOG("EP", Sev::inform) << sdf_file_name << " file found";
    sdf_python_ptr = boost::make_shared<ContactSDFPython>();
    CHKERR sdf_python_ptr->sdfInit(sdf_file_name);
    ContactOps::sdfPythonWeakPtr = sdf_python_ptr;
    MOFEM_LOG("EP", Sev::inform) << "SdfPython initialized";
  } else {
    MOFEM_LOG("EP", Sev::warning) << sdf_file_name << " file NOT found";
  }
 
#endif // ENABLE_PYTHON_BINDING
 
  return sdf_python_ptr;
};
 
struct ContactTree
    : public PostProcBrokenMeshInMoabBase<FaceElementForcesAndSourcesCore> {
 
  using Base = PostProcBrokenMeshInMoabBase<FaceElementForcesAndSourcesCore>;
  using Base::refElementsMap;
 
  ContactTree(MoFEM::Interface &m_field,
              boost::shared_ptr<moab::Core> core_mesh_ptr, int max_order,
              std::map<int, Range> &&body_map);
  MoFEMErrorCode preProcess();
  MoFEMErrorCode postProcess();
  inline boost::shared_ptr<OrientedBoxTreeTool> &getTreeSurfPtr() {
    return treeSurfPtr;
  }
  inline auto getRootSetSurf() { return rootSetSurf; }
  int getMaxLevel() const { return refElementsMap.at(MBTRI)->defMaxLevel; }
 
  friend struct OpMoveNode;
  friend struct OpTreeSearch;
 
  struct FaceData {
    int gaussPtNb; //< integration points number
    std::array<double, 3> slavePoint;
    std::array<double, 3> masterPoint;
    std::array<double, 3> rayPoint;
    std::array<double, 3> unitRay;
    double eleRadius;
 
    // std::vector<int> dofsSlaveIds;
    // std::vector<double> dofsSlaveCoeff;
    // std::vector<double> baseSlaveFuncs;
 
    std::array<double, 9> masterPointNodes;
    std::array<double, 9> masterTractionNodes;
    std::array<double, 9> slavePointNodes;
    std::array<double, 9> slaveTractionNodes;
 
    FaceData() = default;
  };
 
  using MapFaceData = std::map<EntityHandle, std::vector<FaceData>>;
 
  inline auto findFaceDataVecPtr(EntityHandle fe_ent) {
    auto &map_face_data = shadowDataMap;
    auto it = map_face_data.find(fe_ent);
    if (it == map_face_data.end()) {
      return (std::vector<FaceData> *)nullptr;
    }
    return &(it->second);
  }
 
  inline auto getFaceDataPtr(std::vector<FaceData>::iterator &it, int gg,
                             std::vector<FaceData> *vec_ptr) {
    FaceData *face_data_ptr = nullptr;
    if (it != vec_ptr->end()) {
      if (it->gaussPtNb == gg) {
        face_data_ptr = &(*it);
        ++it;
      }
    }
    return face_data_ptr;
  }
 
protected:
  MoFEMErrorCode buildTree(Range &ents);
  boost::shared_ptr<OrientedBoxTreeTool> treeSurfPtr;
  EntityHandle rootSetSurf;
 
  Tag thEleId;
  // Tag thCoeff;
  // Tag thIds;
  // Tag thBases;
  Tag thBodyId;
  Tag thSmallX;
  Tag thLargeX;
  Tag thTraction;
 
  MapFaceData shadowDataMap;
 
  std::map<int, Range> bodyMap;
 
  const int maxOrder;
};
 
auto checkSdf(EntityHandle fe_ent, std::map<int, Range> &sdf_map_range) {
  for (auto &m_sdf : sdf_map_range) {
    if (m_sdf.second.find(fe_ent) != m_sdf.second.end())
      return m_sdf.first;
  }
  return -1;
}
 
template <typename OP_PTR>
auto getSdf(OP_PTR op_ptr, MatrixDouble &contact_disp, int block_id,
            bool eval_hessian) {
 
  auto nb_gauss_pts = op_ptr->getGaussPts().size2();
 
  auto ts_time = op_ptr->getTStime();
  auto ts_time_step = op_ptr->getTStimeStep();
  if (EshelbianCore::dynamicRelaxation) {
    ts_time = EshelbianCore::dynamicTime;
    ts_time_step = EshelbianCore::dynamicStep;
  }
 
  auto m_spatial_coords = ContactOps::get_spatial_coords(
      op_ptr->getFTensor1CoordsAtGaussPts(),
      getFTensor1FromMat<3>(contact_disp), nb_gauss_pts);
  auto m_normals_at_pts = ContactOps::get_normalize_normals(
      op_ptr->getFTensor1NormalsAtGaussPts(), nb_gauss_pts);
  auto v_sdf = ContactOps::surface_distance_function(
      ts_time_step, ts_time, nb_gauss_pts, m_spatial_coords, m_normals_at_pts,
      block_id);
  auto m_grad_sdf = ContactOps::grad_surface_distance_function(
      ts_time_step, ts_time, nb_gauss_pts, m_spatial_coords, m_normals_at_pts,
      block_id);
 
#ifndef NDEBUG
  if (v_sdf.size() != nb_gauss_pts)
    CHK_THROW_MESSAGE(MOFEM_DATA_INCONSISTENCY,
                      "Wrong number of integration pts");
  if (m_grad_sdf.size2() != nb_gauss_pts)
    CHK_THROW_MESSAGE(MOFEM_DATA_INCONSISTENCY,
                      "Wrong number of integration pts");
  if (m_grad_sdf.size1() != 3)
    CHK_THROW_MESSAGE(MOFEM_DATA_INCONSISTENCY, "Should be size of 3");
#endif // NDEBUG
 
  if (eval_hessian) {
    auto m_hess_sdf = ContactOps::hess_surface_distance_function(
        ts_time_step, ts_time, nb_gauss_pts, m_spatial_coords, m_normals_at_pts,
        block_id);
    return std::make_tuple(block_id, m_normals_at_pts, v_sdf, m_grad_sdf,
                           m_hess_sdf);
  } else {
 
    return std::make_tuple(block_id, m_normals_at_pts, v_sdf, m_grad_sdf,
                           MatrixDouble(6, nb_gauss_pts, 0.));
  }
};
 
/**
 * @brief Calculate points data on contact surfaces
 *
 * @tparam T1
 * @param unit_ray
 * @param point
 * @param elem_point_nodes
 * @param elem_traction_nodes
 * @param t_spatial_coords
 * @return auto
 */
template <typename T1>
auto multiPoint(
 
    std::array<double, 3> &unit_ray, std::array<double, 3> &point,
 
    std::array<double, 9> &elem_point_nodes,
    std::array<double, 9> &elem_traction_nodes,
 
    FTensor::Tensor1<T1, 3> &t_spatial_coords) {
  FTensor::Index<'i', 3> i;
 
  auto t_unit_ray = getFTensor1FromPtr<3>(unit_ray.data());
  auto t_point = getFTensor1FromPtr<3>(point.data());
 
  auto get_normal = [](auto &ele_coords) {
    FTensor::Tensor1<double, 3> t_normal;
    Tools::getTriNormal(ele_coords.data(), &t_normal(0));
    return t_normal;
  };
 
  auto t_normal = get_normal(elem_point_nodes);
  t_normal(i) /= t_normal.l2();
 
  auto sn = t_normal(i) * t_point(i);
  auto nm = t_normal(i) * t_spatial_coords(i);
  auto nr = t_normal(i) * t_unit_ray(i);
 
  auto gamma = (sn - nm) / nr;
 
  FTensor::Tensor1<T1, 3> t_point_current;
  t_point_current(i) = t_spatial_coords(i) + gamma * t_unit_ray(i);
 
  auto get_local_point_shape_functions = [&](auto &&t_elem_coords,
                                             auto &t_point) {
    std::array<T1, 2> loc_coords;
    CHK_THROW_MESSAGE(
        Tools::getLocalCoordinatesOnReferenceThreeNodeTri(
            &t_elem_coords(0, 0), &t_point(0), 1, loc_coords.data()),
        "get local coords");
    return FTensor::Tensor1<T1, 3>{N_MBTRI0(loc_coords[0], loc_coords[1]),
                                   N_MBTRI1(loc_coords[0], loc_coords[1]),
                                   N_MBTRI2(loc_coords[0], loc_coords[1])};
  };
 
  auto eval_position = [&](auto &&t_field, auto &t_shape_fun) {
    FTensor::Index<'i', 3> i;
    FTensor::Index<'j', 3> j;
    FTensor::Tensor1<T1, 3> t_point_out;
    t_point_out(i) = t_shape_fun(j) * t_field(j, i);
    return t_point_out;
  };
 
  auto t_shape_fun = get_local_point_shape_functions(
      getFTensor2FromPtr<3, 3>(elem_point_nodes.data()), t_point_current);
  auto t_slave_point_updated = eval_position(
      getFTensor2FromPtr<3, 3>(elem_point_nodes.data()), t_shape_fun);
  auto t_traction_updated = eval_position(
      getFTensor2FromPtr<3, 3>(elem_traction_nodes.data()), t_shape_fun);
 
  return std::make_tuple(t_slave_point_updated, t_traction_updated, t_normal);
};
 
template <typename T1>
auto multiMasterPoint(
 
    ContactTree::FaceData *face_data_ptr,
    FTensor::Tensor1<T1, 3> &t_spatial_coords
 
) {
 
  return multiPoint(face_data_ptr->unitRay, face_data_ptr->masterPoint,
                    face_data_ptr->masterPointNodes,
                    face_data_ptr->masterTractionNodes, t_spatial_coords);
};
 
template <typename T1>
auto multiSlavePoint(
 
    ContactTree::FaceData *face_data_ptr,
    FTensor::Tensor1<T1, 3> &t_spatial_coords
 
) {
 
  return multiPoint(face_data_ptr->unitRay, face_data_ptr->slavePoint,
                    face_data_ptr->slavePointNodes,
                    face_data_ptr->slaveTractionNodes, t_spatial_coords);
};
 
/**
 * Evaluate gap and tractions between master and slave points
 */
template <typename T1>
auto multiGetGap(ContactTree::FaceData *face_data_ptr,
                 FTensor::Tensor1<T1, 3> &t_spatial_coords) {
 
  auto [t_slave_point_current, t_slave_traction_current, t_slave_normal] =
      multiSlavePoint(face_data_ptr, t_spatial_coords);
  auto [t_master_point_current, t_master_traction_current, t_master_normal] =
      multiMasterPoint(face_data_ptr, t_spatial_coords);
 
  FTensor::Index<'i', 3> i;
 
  FTensor::Tensor1<double, 3> t_normal;
  t_normal(i) = t_master_normal(i) - t_slave_normal(i);
  t_normal.normalize();
 
  auto gap = t_normal(i) * (t_slave_point_current(i) - t_spatial_coords(i));
  auto tn_master = t_master_traction_current(i) * t_normal(i);
  auto tn_slave = t_slave_traction_current(i) * t_normal(i);
  auto tn = std::max(-tn_master, tn_slave);
 
  return std::make_tuple(gap, tn_master, tn_slave,
                         ContactOps::constrain(gap, tn),
                         t_master_traction_current, t_slave_traction_current);
};
 
enum MultiPointRhsType { U, P };
 
/** Caluclate rhs term for contact
 */
template <typename T1, typename T2, typename T3>
auto multiPointRhs(
 
    ContactTree::FaceData *face_data_ptr, FTensor::Tensor1<T1, 3> &t_coords,
    FTensor::Tensor1<T2, 3> &t_spatial_coords,
    FTensor::Tensor1<T3, 3> &t_master_traction, MultiPointRhsType type,
    bool debug = false
 
) {
  FTensor::Index<'i', 3> i;
  FTensor::Index<'j', 3> j;
 
  FTensor::Tensor1<T2, 3> t_u;
  t_u(i) = t_spatial_coords(i) - t_coords(i);
 
  FTensor::Tensor1<typename FTensor::promote<T2, T3>::V, 3> t_rhs;
 
  switch (type) {
  case MultiPointRhsType::U: {
 
    auto [t_slave_point_current, t_slave_traction_current, t_slave_normal] =
        multiSlavePoint(face_data_ptr, t_spatial_coords);
    auto [t_master_point_current, t_master_traction_current, t_master_normal] =
        multiMasterPoint(face_data_ptr, t_spatial_coords);
 
    // average normal surface
    FTensor::Tensor1<double, 3> t_normal;
    t_normal(i) = t_master_normal(i) - t_slave_normal(i);
    t_normal.normalize();
 
    // get projection operators
    FTensor::Tensor2<double, 3, 3> t_P;
    t_P(i, j) = t_normal(i) * t_normal(j);
    FTensor::Tensor2<double, 3, 3> t_Q;
    t_Q(i, j) = kronecker_delta(i, j) - t_P(i, j);
 
    constexpr double beta = 0.5;
 
    auto zeta = 1e-8 * face_data_ptr->eleRadius;
    auto alpha1 = ContactOps::alpha_contact_const;
    auto alpha2 = ContactOps::alpha_contact_quadratic;
 
    // this is regularised std::min(d,s)
    auto f_min_gap = [zeta](auto d, auto s) {
      return 0.5 * (d + s - std::sqrt((d - s) * (d - s) + zeta));
    };
    // this derivative of regularised std::min(d,s)
    auto f_diff_min_gap = [zeta](auto d, auto s) {
      return 0.5 * (1 - (d - s) / std::sqrt((d - s) * (d - s) + zeta));
    };
 
    // add penalty on penetration side
    auto f_barrier = [alpha1, alpha2, f_min_gap, f_diff_min_gap](auto g,
                                                                 auto tn) {
      auto d = alpha1 * g;
      auto b1 =
          0.5 * (tn + f_min_gap(d, tn) + f_diff_min_gap(d, tn) * (d - tn));
      auto b2 = alpha2 * f_min_gap(g, 0) * g;
      return b1 - b2;
    };
 
    FTensor::Tensor1<T2, 3> t_gap_vec;
    t_gap_vec(i) = beta * t_spatial_coords(i) +
                   (1 - beta) * t_slave_point_current(i) - t_spatial_coords(i);
    FTensor::Tensor1<typename FTensor::promote<T2, T3>::V, 3> t_traction_vec;
    t_traction_vec(i) =
        -beta * t_master_traction(i) + (beta - 1) * t_slave_traction_current(i);
 
    auto t_gap = t_normal(i) * t_gap_vec(i);
    auto t_tn = t_normal(i) * t_traction_vec(i);
    auto barrier = f_barrier(t_gap, t_tn);
 
    FTensor::Tensor1<typename FTensor::promote<T2, T3>::V, 3> t_traction_bar;
    // add penalty on penetration side
    t_traction_bar(i) = t_normal(i) * barrier;
 
    t_rhs(i) =
 
        t_Q(i, j) * t_master_traction(j) +
 
        t_P(i, j) * (t_master_traction(j) - t_traction_bar(j));
 
    if (debug) {
      auto is_nan_or_inf = [](double value) -> bool {
        return std::isnan(value) || std::isinf(value);
      };
 
      double v = std::complex<double>(t_rhs(i) * t_rhs(i)).real();
      if (is_nan_or_inf(v)) {
        MOFEM_LOG_CHANNEL("SELF");
        MOFEM_LOG("SELF", Sev::error) << "t_rhs " << t_rhs;
        CHK_MOAB_THROW(MOFEM_DATA_INCONSISTENCY, "rhs is nan or inf");
      }
    }
 
  } break;
  case MultiPointRhsType::P:
    CHK_THROW_MESSAGE(MOFEM_DATA_INCONSISTENCY, "not implemented");
    break;
  }
 
  return t_rhs;
};
 
struct OpConstrainBoundaryL2Rhs : public ContactOps::AssemblyBoundaryEleOp {
  OpConstrainBoundaryL2Rhs(
      const std::string row_field_name,
      boost::shared_ptr<ContactOps::CommonData> common_data_ptr,
      boost::shared_ptr<ContactTree> contact_tree_ptr,
      boost::shared_ptr<std::map<int, Range>> sdf_map_range_ptr = nullptr);
 
  MoFEMErrorCode iNtegrate(EntitiesFieldData::EntData &row_data);
 
private:
  boost::shared_ptr<ContactOps::CommonData> commonDataPtr;
  boost::shared_ptr<ContactTree> contactTreePtr;
  boost::shared_ptr<std::map<int, Range>> sdfMapRangePtr;
};
 
OpConstrainBoundaryL2Rhs::OpConstrainBoundaryL2Rhs(
    const std::string row_field_name,
    boost::shared_ptr<ContactOps::CommonData> common_data_ptr,
    boost::shared_ptr<ContactTree> contact_tree_ptr,
    boost::shared_ptr<std::map<int, Range>> sdf_map_range_ptr)
    : ContactOps::AssemblyBoundaryEleOp(row_field_name, row_field_name,
                                        ContactOps::BoundaryEleOp::OPROW),
      commonDataPtr(common_data_ptr), contactTreePtr(contact_tree_ptr),
      sdfMapRangePtr(sdf_map_range_ptr) {
  CHK_THROW_MESSAGE(PetscOptionsGetScalar(PETSC_NULLPTR, "", "-cn",
                                          &ContactOps::cn_contact,
                                          PETSC_NULLPTR),
                    "get cn failed");
  ContactOps::alpha_contact_const = 1. / ContactOps::cn_contact;
  CHK_THROW_MESSAGE(
      PetscOptionsGetScalar(PETSC_NULLPTR, "", "-alpha_contact_const",
                            &ContactOps::alpha_contact_const, PETSC_NULLPTR),
      "get alpha contact failed");
  CHK_THROW_MESSAGE(PetscOptionsGetScalar(
                        PETSC_NULLPTR, "", "-alpha_contact_quadratic",
                        &ContactOps::alpha_contact_quadratic, PETSC_NULLPTR),
                    "get alpha contact failed");
  CHK_THROW_MESSAGE(
      PetscOptionsGetScalar(PETSC_NULLPTR, "", "-airplane_ray_distance",
                            &ContactOps::airplane_ray_distance, PETSC_NULLPTR),
      "get alpha contact failed");
 
  MOFEM_LOG("EP", Sev::inform) << "cn " << ContactOps::cn_contact;
  MOFEM_LOG("EP", Sev::inform)
      << "alpha_contact_const " << ContactOps::alpha_contact_const;
  MOFEM_LOG("EP", Sev::inform)
      << "alpha_contact_quadratic " << ContactOps::alpha_contact_quadratic;
  MOFEM_LOG("EP", Sev::inform)
      << "airplane_ray_distance " << ContactOps::airplane_ray_distance;
}
 
MoFEMErrorCode
OpConstrainBoundaryL2Rhs::iNtegrate(EntitiesFieldData::EntData &data) {
  MoFEMFunctionBegin;
 
  FTensor::Index<'i', 3> i;
  FTensor::Index<'j', 3> j;
  FTensor::Index<'k', 3> k;
  FTensor::Index<'l', 3> l;
 
  const size_t nb_gauss_pts = getGaussPts().size2();
 
#ifndef NDEBUG
  if (commonDataPtr->contactDisp.size2() != nb_gauss_pts) {
    SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
            "Wrong number of integration pts %ld != %ld",
            commonDataPtr->contactDisp.size2(), nb_gauss_pts);
  }
#endif // !NDEBUG
 
  auto &nf = locF;
  locF.clear();
 
  auto t_w = getFTensor0IntegrationWeight();
  auto t_coords = getFTensor1CoordsAtGaussPts();
  auto t_disp = getFTensor1FromMat<3>(commonDataPtr->contactDisp);
  auto t_traction = getFTensor1FromMat<3>(commonDataPtr->contactTraction);
 
  // placeholder to pass boundary block id to python. default SDF is set on
  // block = -1, one can choose different block by making block "CONTACT_SDF",
  // then specific SDF can be set to that block.
  auto [block_id, m_normals_at_pts, v_sdf, m_grad_sdf, m_hess_sdf] =
      getSdf(this, commonDataPtr->contactDisp,
             checkSdf(getFEEntityHandle(), *sdfMapRangePtr), false);
 
  auto t_sdf_v = getFTensor0FromVec(v_sdf);
  auto t_grad_sdf_v = getFTensor1FromMat<3>(m_grad_sdf);
  auto t_normalize_normal = getFTensor1FromMat<3>(m_normals_at_pts);
 
  auto next = [&]() {
    ++t_w;
    ++t_coords;
    ++t_disp;
    ++t_traction;
    ++t_normalize_normal;
    ++t_sdf_v;
    ++t_grad_sdf_v;
  };
 
  auto face_data_vec_ptr =
      contactTreePtr->findFaceDataVecPtr(getFEEntityHandle());
  auto face_gauss_pts_it = face_data_vec_ptr->begin();
 
  auto nb_base_functions = data.getN().size2();
  auto t_base = data.getFTensor0N();
  for (auto gg = 0; gg != nb_gauss_pts; ++gg) {
 
    FTensor::Tensor1<double, 3> t_rhs;
    auto face_data_ptr = contactTreePtr->getFaceDataPtr(face_gauss_pts_it, gg,
                                                        face_data_vec_ptr);
 
    auto check_face_contact = [&]() {
      if (checkSdf(getFEEntityHandle(), *sdfMapRangePtr) != -1)
        return false;
 
      if (face_data_ptr) {
        return true;
      }
      return false;
    };
 
#ifdef ENABLE_PYTHON_BINDING
    double c = 0.;
    if (ContactOps::sdfPythonWeakPtr.lock()) {
      auto tn = t_traction(i) * t_grad_sdf_v(i);
      c = ContactOps::constrain(t_sdf_v, tn);
    }
#else
    constexpr double c = 0;
#endif
 
    if (!c && check_face_contact()) {
      FTensor::Tensor1<double, 3> t_spatial_coords;
      t_spatial_coords(i) = t_coords(i) + t_disp(i);
      auto t_rhs_tmp = multiPointRhs(face_data_ptr, t_coords, t_spatial_coords,
                                     t_traction, MultiPointRhsType::U, true);
      t_rhs(i) = t_rhs_tmp(i);
 
    } else {
 
#ifdef ENABLE_PYTHON_BINDING
      auto inv_cn = 1. / ContactOps::cn_contact;
 
      if (ContactOps::sdfPythonWeakPtr.lock()) {
        FTensor::Tensor2<double, 3, 3> t_cP, t_cQ;
        t_cP(i, j) = (c * t_grad_sdf_v(i)) * t_grad_sdf_v(j);
        t_cQ(i, j) = kronecker_delta(i, j) - t_cP(i, j);
        t_rhs(i) = t_cQ(i, j) * t_traction(j) +
                   (c * inv_cn * t_sdf_v) * t_grad_sdf_v(i);
      } else {
        t_rhs(i) = t_traction(i);
      }
#else
      t_rhs(i) = t_traction(i);
#endif
    }
 
    auto t_nf = getFTensor1FromPtr<3>(&nf[0]);
    const double alpha = t_w * getMeasure();
 
    size_t bb = 0;
    for (; bb != nbRows / 3; ++bb) {
      const double beta = alpha * t_base;
      t_nf(i) -= beta * t_rhs(i);
      ++t_nf;
      ++t_base;
    }
    for (; bb < nb_base_functions; ++bb)
      ++t_base;
 
    next();
  }
 
  MoFEMFunctionReturn(0);
}
 
template <AssemblyType A, IntegrationType I> struct OpConstrainBoundaryHDivRhs;
 
template <AssemblyType A>
struct OpConstrainBoundaryHDivRhs<A, IntegrationType::GAUSS>
    : public FormsIntegrators<FaceUserDataOperator>::Assembly<A>::OpBrokenBase {
 
  using OP = typename FormsIntegrators<FaceUserDataOperator>::Assembly<
      A>::OpBrokenBase;
 
  OpConstrainBoundaryHDivRhs(
      boost::shared_ptr<std::vector<BrokenBaseSideData>> broken_base_side_data,
      boost::shared_ptr<ContactOps::CommonData> common_data_ptr,
      boost::shared_ptr<ContactTree> contact_tree_ptr);
 
  MoFEMErrorCode iNtegrate(EntitiesFieldData::EntData &row_data);
 
private:
  boost::shared_ptr<ContactOps::CommonData> commonDataPtr;
  boost::shared_ptr<ContactTree> contactTreePtr;
};
 
template <AssemblyType A>
OpConstrainBoundaryHDivRhs<A, IntegrationType::GAUSS>::
    OpConstrainBoundaryHDivRhs(
        boost::shared_ptr<std::vector<BrokenBaseSideData>>
            broken_base_side_data,
        boost::shared_ptr<ContactOps::CommonData> common_data_ptr,
        boost::shared_ptr<ContactTree> contact_tree_ptr)
    : OP(broken_base_side_data), commonDataPtr(common_data_ptr),
      contactTreePtr(contact_tree_ptr) {}
 
template <AssemblyType A>
MoFEMErrorCode OpConstrainBoundaryHDivRhs<A, IntegrationType::GAUSS>::iNtegrate(
    EntitiesFieldData::EntData &data) {
  MoFEMFunctionBegin;
 
  FTENSOR_INDEX(SPACE_DIM, i);
  FTENSOR_INDEX(SPACE_DIM, j);
  FTENSOR_INDEX(SPACE_DIM, k);
  FTENSOR_INDEX(SPACE_DIM, l);
 
  const size_t nb_gauss_pts = OP::getGaussPts().size2();
 
#ifndef NDEBUG
  if (commonDataPtr->contactDisp.size2() != nb_gauss_pts) {
    SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
            "Wrong number of integration pts %ld != %ld",
            commonDataPtr->contactDisp.size2(), nb_gauss_pts);
  }
#endif // !NDEBUG
 
  auto &nf = OP::locF;
  OP::locF.clear();
 
  auto t_w = OP::getFTensor0IntegrationWeight();
  auto t_material_normal = OP::getFTensor1NormalsAtGaussPts();
  auto t_coords = OP::getFTensor1CoordsAtGaussPts();
 
  auto t_disp = getFTensor1FromMat<3>(commonDataPtr->contactDisp);
  auto t_traction = getFTensor1FromMat<3>(commonDataPtr->contactTraction);
 
  auto next = [&]() {
    ++t_w;
    ++t_disp;
    ++t_traction;
    ++t_coords;
    ++t_material_normal;
  };
 
  auto face_data_vec_ptr =
      contactTreePtr->findFaceDataVecPtr(OP::getFEEntityHandle());
  auto face_gauss_pts_it = face_data_vec_ptr->begin();
 
  auto nb_base_functions = data.getN().size2() / 3;
  auto t_base = data.getFTensor1N<3>();
  for (auto gg = 0; gg != nb_gauss_pts; ++gg) {
 
    auto t_nf = getFTensor1FromPtr<3>(&nf[0]);
    const double alpha = t_w / 2.;
 
    size_t bb = 0;
    for (; bb != OP::nbRows / 3; ++bb) {
      const double beta = alpha * t_base(i) * t_material_normal(i);
      t_nf(i) += beta * t_disp(i);
      ++t_nf;
      ++t_base;
    }
    for (; bb < nb_base_functions; ++bb)
      ++t_base;
 
    next();
  }
 
  MoFEMFunctionReturn(0);
}
 
struct OpConstrainBoundaryL2Lhs_dU : public ContactOps::AssemblyBoundaryEleOp {
 
  OpConstrainBoundaryL2Lhs_dU(
      const std::string row_field_name, const std::string col_field_name,
      boost::shared_ptr<ContactOps::CommonData> common_data_ptr,
      boost::shared_ptr<ContactTree> contact_tree_ptr,
      boost::shared_ptr<std::map<int, Range>> sdf_map_range_ptr = nullptr);
 
  MoFEMErrorCode iNtegrate(EntitiesFieldData::EntData &row_data,
                           EntitiesFieldData::EntData &col_data);
 
private:
  boost::shared_ptr<ContactOps::CommonData> commonDataPtr;
  boost::shared_ptr<ContactTree> contactTreePtr;
  boost::shared_ptr<std::map<int, Range>> sdfMapRangePtr;
};
 
OpConstrainBoundaryL2Lhs_dU::OpConstrainBoundaryL2Lhs_dU(
    const std::string row_field_name, const std::string col_field_name,
    boost::shared_ptr<ContactOps::CommonData> common_data_ptr,
    boost::shared_ptr<ContactTree> contact_tree_ptr,
    boost::shared_ptr<std::map<int, Range>> sdf_map_range_ptr)
    : ContactOps::AssemblyBoundaryEleOp(row_field_name, col_field_name,
                                        ContactOps::BoundaryEleOp::OPROWCOL),
      commonDataPtr(common_data_ptr), contactTreePtr(contact_tree_ptr),
      sdfMapRangePtr(sdf_map_range_ptr) {
 
  sYmm = false;
}
 
MoFEMErrorCode
OpConstrainBoundaryL2Lhs_dU::iNtegrate(EntitiesFieldData::EntData &row_data,
                                       EntitiesFieldData::EntData &col_data) {
  MoFEMFunctionBegin;
 
  using namespace ContactOps;
 
  FTensor::Index<'i', 3> i;
  FTensor::Index<'j', 3> j;
  FTensor::Index<'k', 3> k;
 
  auto nb_rows = row_data.getIndices().size();
  auto nb_cols = col_data.getIndices().size();
 
  auto &locMat = AssemblyBoundaryEleOp::locMat;
  locMat.resize(nb_rows, nb_cols, false);
  locMat.clear();
 
  if (nb_cols && nb_rows) {
 
    auto nb_gauss_pts = getGaussPts().size2();
    auto t_w = getFTensor0IntegrationWeight();
    auto t_coords = getFTensor1CoordsAtGaussPts();
    auto t_disp = getFTensor1FromMat<3>(commonDataPtr->contactDisp);
    auto t_traction = getFTensor1FromMat<3>(commonDataPtr->contactTraction);
 
    // placeholder to pass boundary block id to python
    auto [block_id, m_normals_at_pts, v_sdf, m_grad_sdf, m_hess_sdf] =
        getSdf(this, commonDataPtr->contactDisp,
               checkSdf(getFEEntityHandle(), *sdfMapRangePtr), true);
 
    auto t_sdf_v = getFTensor0FromVec(v_sdf);
    auto t_grad_sdf_v = getFTensor1FromMat<3>(m_grad_sdf);
    auto t_hess_sdf_v = getFTensor2SymmetricFromMat<3>(m_hess_sdf);
    auto t_normalized_normal = getFTensor1FromMat<3>(m_normals_at_pts);
 
    auto next = [&]() {
      ++t_w;
      ++t_coords;
      ++t_disp;
      ++t_traction;
      ++t_sdf_v;
      ++t_grad_sdf_v;
      ++t_hess_sdf_v;
      ++t_normalized_normal;
    };
 
    auto face_data_vec_ptr =
        contactTreePtr->findFaceDataVecPtr(getFEEntityHandle());
    auto face_gauss_pts_it = face_data_vec_ptr->begin();
 
    auto t_row_base = row_data.getFTensor0N();
    auto nb_face_functions = row_data.getN().size2() / 3;
    constexpr auto t_kd = FTensor::Kronecker_Delta<int>();
 
    for (size_t gg = 0; gg != nb_gauss_pts; ++gg) {
 
      auto face_data_ptr = contactTreePtr->getFaceDataPtr(face_gauss_pts_it, gg,
                                                          face_data_vec_ptr);
 
      auto check_face_contact = [&]() {
        if (checkSdf(getFEEntityHandle(), *sdfMapRangePtr) != -1)
          return false;
 
        if (face_data_ptr) {
          return true;
        }
        return false;
      };
 
      FTensor::Tensor2<double, 3, 3> t_res_dU;
 
#ifdef ENABLE_PYTHON_BINDING
      double c = 0.;
      if (ContactOps::sdfPythonWeakPtr.lock()) {
        auto tn = t_traction(i) * t_grad_sdf_v(i);
        c = ContactOps::constrain(t_sdf_v, tn);
      }
#else
      constexpr double c = 0;
#endif
 
      if (!c && check_face_contact()) {
        FTensor::Tensor1<double, 3> t_spatial_coords;
        t_spatial_coords(i) = t_coords(i) + t_disp(i);
        constexpr double eps = std::numeric_limits<float>::epsilon();
        for (auto ii = 0; ii < 3; ++ii) {
          FTensor::Tensor1<std::complex<double>, 3> t_spatial_coords_cx{
              t_spatial_coords(0), t_spatial_coords(1), t_spatial_coords(2)};
          t_spatial_coords_cx(ii) += eps * 1i;
          auto t_rhs_tmp =
              multiPointRhs(face_data_ptr, t_coords, t_spatial_coords_cx,
                            t_traction, MultiPointRhsType::U);
          for (int jj = 0; jj != 3; ++jj) {
            auto v = t_rhs_tmp(jj).imag();
            t_res_dU(jj, ii) = v / eps;
          }
        }
 
      } else {
 
#ifdef ENABLE_PYTHON_BINDING
 
        if (ContactOps::sdfPythonWeakPtr.lock()) {
          auto inv_cn = 1. / ContactOps::cn_contact;
          t_res_dU(i, j) =
 
              (-c) * (t_hess_sdf_v(i, j) * t_grad_sdf_v(k) * t_traction(k) +
                      t_grad_sdf_v(i) * t_hess_sdf_v(k, j) * t_traction(k))
 
              + (c * inv_cn) * (t_sdf_v * t_hess_sdf_v(i, j) +
 
                                t_grad_sdf_v(j) * t_grad_sdf_v(i));
        } else {
          t_res_dU(i, j) = 0;
        }
#else
        t_res_dU(i, j) = 0;
#endif
      }
 
      auto alpha = t_w * getMeasure();
 
      size_t rr = 0;
      for (; rr != nb_rows / 3; ++rr) {
 
        auto t_mat = getFTensor2FromArray<3, 3, 3>(locMat, 3 * rr);
        auto t_col_base = col_data.getFTensor0N(gg, 0);
 
        for (size_t cc = 0; cc != nb_cols / 3; ++cc) {
          auto beta = alpha * t_row_base * t_col_base;
          t_mat(i, j) -= beta * t_res_dU(i, j);
          ++t_col_base;
          ++t_mat;
        }
 
        ++t_row_base;
      }
      for (; rr < nb_face_functions; ++rr)
        ++t_row_base;
 
      next();
    }
  }
 
  MoFEMFunctionReturn(0);
}
 
template <AssemblyType A, IntegrationType I> struct OpConstrainBoundaryL2Lhs_dP;
 
template <AssemblyType A>
struct OpConstrainBoundaryL2Lhs_dP<A, IntegrationType::GAUSS>
    : public FormsIntegrators<FaceUserDataOperator>::Assembly<A>::OpBrokenBase {
 
  using OP = typename FormsIntegrators<FaceUserDataOperator>::Assembly<
      A>::OpBrokenBase;
 
  OpConstrainBoundaryL2Lhs_dP(
      std::string row_field_name,
      boost::shared_ptr<std::vector<BrokenBaseSideData>> broken_base_side_data,
      boost::shared_ptr<ContactOps::CommonData> common_data_ptr,
      boost::shared_ptr<ContactTree> contact_tree_ptr,
      boost::shared_ptr<std::map<int, Range>> sdf_map_range_ptr = nullptr);
 
  MoFEMErrorCode iNtegrate(EntitiesFieldData::EntData &row_data,
                           EntitiesFieldData::EntData &col_data);
 
private:
  boost::shared_ptr<ContactOps::CommonData> commonDataPtr;
  boost::shared_ptr<ContactTree> contactTreePtr;
  boost::shared_ptr<std::map<int, Range>> sdfMapRangePtr;
};
 
template <AssemblyType A>
OpConstrainBoundaryL2Lhs_dP<A, IntegrationType::GAUSS>::
    OpConstrainBoundaryL2Lhs_dP(
        std::string row_field_name,
        boost::shared_ptr<std::vector<BrokenBaseSideData>>
            broken_base_side_data,
        boost::shared_ptr<ContactOps::CommonData> common_data_ptr,
        boost::shared_ptr<ContactTree> contact_tree_ptr,
        boost::shared_ptr<std::map<int, Range>> sdf_map_range_ptr)
    : OP(row_field_name, broken_base_side_data, false, false),
      commonDataPtr(common_data_ptr), contactTreePtr(contact_tree_ptr),
      sdfMapRangePtr(sdf_map_range_ptr) {
  OP::sYmm = false;
}
 
template <AssemblyType A>
MoFEMErrorCode
OpConstrainBoundaryL2Lhs_dP<A, IntegrationType::GAUSS>::iNtegrate(
    EntitiesFieldData::EntData &row_data,
    EntitiesFieldData::EntData &col_data) {
  MoFEMFunctionBegin;
 
  using namespace ContactOps;
 
  FTensor::Index<'i', 3> i;
  FTensor::Index<'j', 3> j;
  FTensor::Index<'k', 3> k;
 
  auto nb_rows = row_data.getIndices().size();
  auto nb_cols = col_data.getIndices().size();
 
  auto &locMat = AssemblyBoundaryEleOp::locMat;
  locMat.resize(nb_rows, nb_cols, false);
  locMat.clear();
 
  if (nb_cols && nb_rows) {
 
    const size_t nb_gauss_pts = OP::getGaussPts().size2();
 
    auto t_w = OP::getFTensor0IntegrationWeight();
    auto t_disp = getFTensor1FromMat<3>(commonDataPtr->contactDisp);
    auto t_traction = getFTensor1FromMat<3>(commonDataPtr->contactTraction);
    auto t_coords = OP::getFTensor1CoordsAtGaussPts();
    auto t_material_normal = OP::getFTensor1NormalsAtGaussPts();
 
    // placeholder to pass boundary block id to python
    auto [block_id, m_normals_at_pts, v_sdf, m_grad_sdf, m_hess_sdf] =
        getSdf(this, commonDataPtr->contactDisp,
               checkSdf(OP::getFEEntityHandle(), *sdfMapRangePtr), false);
 
    auto t_sdf_v = getFTensor0FromVec(v_sdf);
    auto t_grad_sdf_v = getFTensor1FromMat<3>(m_grad_sdf);
 
    auto next = [&]() {
      ++t_w;
      ++t_disp;
      ++t_traction;
      ++t_coords;
      ++t_material_normal;
      ++t_sdf_v;
      ++t_grad_sdf_v;
    };
 
    auto face_data_vec_ptr =
        contactTreePtr->findFaceDataVecPtr(OP::getFEEntityHandle());
    auto face_gauss_pts_it = face_data_vec_ptr->begin();
 
    auto t_row_base = row_data.getFTensor0N();
    auto nb_face_functions = row_data.getN().size2();
 
    constexpr auto t_kd = FTensor::Kronecker_Delta<int>();
 
    for (size_t gg = 0; gg != nb_gauss_pts; ++gg) {
 
      auto face_data_ptr = contactTreePtr->getFaceDataPtr(face_gauss_pts_it, gg,
                                                          face_data_vec_ptr);
 
      auto check_face_contact = [&]() {
        if (checkSdf(OP::getFEEntityHandle(), *sdfMapRangePtr) != -1)
          return false;
 
        if (face_data_ptr) {
          return true;
        }
        return false;
      };
 
      FTensor::Tensor2<double, 3, 3> t_res_dP;
 
#ifdef ENABLE_PYTHON_BINDING
      double c = 0.;
      if (ContactOps::sdfPythonWeakPtr.lock()) {
        auto tn = t_traction(i) * t_grad_sdf_v(i);
        c = ContactOps::constrain(t_sdf_v, tn);
      }
#else
      constexpr double c = 0;
#endif
 
      if (!c && check_face_contact()) {
        FTensor::Tensor1<double, 3> t_spatial_coords;
        t_spatial_coords(i) = t_coords(i) + t_disp(i);
        constexpr double eps = std::numeric_limits<float>::epsilon();
        for (auto ii = 0; ii != 3; ++ii) {
          FTensor::Tensor1<std::complex<double>, 3> t_traction_cx{
              t_traction(0), t_traction(1), t_traction(2)};
          t_traction_cx(ii) += eps * 1i;
          auto t_rhs_tmp =
              multiPointRhs(face_data_ptr, t_coords, t_spatial_coords,
                            t_traction_cx, MultiPointRhsType::U);
          for (int jj = 0; jj != 3; ++jj) {
            auto v = t_rhs_tmp(jj).imag();
            t_res_dP(jj, ii) = v / eps;
          }
        }
      } else {
 
#ifdef ENABLE_PYTHON_BINDING
        if (ContactOps::sdfPythonWeakPtr.lock()) {
          FTensor::Tensor2<double, 3, 3> t_cP, t_cQ;
          t_cP(i, j) = (c * t_grad_sdf_v(i)) * t_grad_sdf_v(j);
          t_cQ(i, j) = kronecker_delta(i, j) - t_cP(i, j);
          t_res_dP(i, j) = t_cQ(i, j);
        } else {
          t_res_dP(i, j) = t_kd(i, j);
        }
#else
        t_res_dP(i, j) = t_kd(i, j);
#endif
      }
 
      const double alpha = t_w / 2.;
      size_t rr = 0;
      for (; rr != nb_rows / 3; ++rr) {
 
        auto t_mat = getFTensor2FromArray<3, 3, 3>(locMat, 3 * rr);
        auto t_col_base = col_data.getFTensor1N<3>(gg, 0);
 
        for (size_t cc = 0; cc != nb_cols / 3; ++cc) {
          auto col_base = t_col_base(i) * t_material_normal(i);
          const double beta = alpha * t_row_base * col_base;
          t_mat(i, j) -= beta * t_res_dP(i, j);
          ++t_col_base;
          ++t_mat;
        }
 
        ++t_row_base;
      }
      for (; rr < nb_face_functions; ++rr)
        ++t_row_base;
 
      next();
    }
  }
 
  MoFEMFunctionReturn(0);
}
 
template <AssemblyType A, IntegrationType I>
struct OpConstrainBoundaryHDivLhs_dU;
 
template <AssemblyType A>
struct OpConstrainBoundaryHDivLhs_dU<A, IntegrationType::GAUSS>
    : public FormsIntegrators<FaceUserDataOperator>::Assembly<A>::OpBrokenBase {
 
  using OP = typename FormsIntegrators<FaceUserDataOperator>::Assembly<
      A>::OpBrokenBase;
 
  OpConstrainBoundaryHDivLhs_dU(
      boost::shared_ptr<std::vector<BrokenBaseSideData>> broken_base_side_data,
      std::string col_field_name,
      boost::shared_ptr<ContactOps::CommonData> common_data_ptr,
      boost::shared_ptr<ContactTree> contact_tree_ptr);
 
  MoFEMErrorCode iNtegrate(EntitiesFieldData::EntData &row_data,
                           EntitiesFieldData::EntData &col_data);
 
private:
  boost::shared_ptr<ContactOps::CommonData> commonDataPtr;
  boost::shared_ptr<ContactTree> contactTreePtr;
};
 
template <AssemblyType A>
OpConstrainBoundaryHDivLhs_dU<A, IntegrationType::GAUSS>::
    OpConstrainBoundaryHDivLhs_dU(
        boost::shared_ptr<std::vector<BrokenBaseSideData>>
            broken_base_side_data,
        std::string col_field_name,
        boost::shared_ptr<ContactOps::CommonData> common_data_ptr,
        boost::shared_ptr<ContactTree> contact_tree_ptr)
    : OP(col_field_name, broken_base_side_data, true, true),
      commonDataPtr(common_data_ptr), contactTreePtr(contact_tree_ptr) {
  OP::sYmm = false;
}
 
template <AssemblyType A>
MoFEMErrorCode
OpConstrainBoundaryHDivLhs_dU<A, IntegrationType::GAUSS>::iNtegrate(
    EntitiesFieldData::EntData &col_data,
    EntitiesFieldData::EntData &row_data) {
  MoFEMFunctionBegin;
 
  // Note: col_data and row_data are swapped in this function, we going to
  // transpose locMat at the end
 
  using namespace ContactOps;
 
  FTENSOR_INDEX(SPACE_DIM, i);
  FTENSOR_INDEX(SPACE_DIM, j);
  FTENSOR_INDEX(SPACE_DIM, k);
 
  auto nb_rows = row_data.getIndices().size();
  auto nb_cols = col_data.getIndices().size();
 
  auto &locMat = AssemblyBoundaryEleOp::locMat;
  locMat.resize(nb_rows, nb_cols, false);
  locMat.clear();
 
  if (nb_cols && nb_rows) {
 
    const size_t nb_gauss_pts = OP::getGaussPts().size2();
 
    auto t_w = OP::getFTensor0IntegrationWeight();
    auto t_disp = getFTensor1FromMat<3>(commonDataPtr->contactDisp);
    auto t_traction = getFTensor1FromMat<3>(commonDataPtr->contactTraction);
    auto t_coords = OP::getFTensor1CoordsAtGaussPts();
    auto t_material_normal = OP::getFTensor1NormalsAtGaussPts();
 
    auto next = [&]() {
      ++t_w;
      ++t_disp;
      ++t_traction;
      ++t_coords;
      ++t_material_normal;
    };
 
    constexpr auto t_kd = FTensor::Kronecker_Delta<int>();
 
    auto face_data_vec_ptr =
        contactTreePtr->findFaceDataVecPtr(OP::getFEEntityHandle());
    auto face_gauss_pts_it = face_data_vec_ptr->begin();
 
    auto t_row_base = row_data.getFTensor1N<3>();
    auto nb_face_functions = row_data.getN().size2() / 3;
    for (size_t gg = 0; gg != nb_gauss_pts; ++gg) {
 
      const auto alpha = t_w / 2.;
 
      size_t rr = 0;
      for (; rr != nb_rows / 3; ++rr) {
 
        auto row_base = alpha * (t_row_base(i) * t_material_normal(i));
 
        auto t_mat = getFTensor2FromArray<3, 3, 3>(locMat, 3 * rr);
        auto t_col_base = col_data.getFTensor0N(gg, 0);
 
        for (size_t cc = 0; cc != nb_cols / 3; ++cc) {
          const auto beta = row_base * t_col_base;
          t_mat(i, j) += beta * t_kd(i, j);
          ++t_col_base;
          ++t_mat;
        }
 
        ++t_row_base;
      }
      for (; rr < nb_face_functions; ++rr)
        ++t_row_base;
 
      next();
    }
  }
 
  locMat = trans(locMat);
 
  MoFEMFunctionReturn(0);
}
 
ContactTree::ContactTree(MoFEM::Interface &m_field,
                         boost::shared_ptr<moab::Core> core_mesh_ptr,
                         int max_order, std::map<int, Range> &&body_map)
    : Base(m_field, core_mesh_ptr, "contact"), maxOrder(max_order),
      bodyMap(body_map) {
 
  auto ref_ele_ptr = boost::make_shared<PostProcGenerateRefMesh<MBTRI>>();
  ref_ele_ptr->hoNodes =
      PETSC_FALSE; ///< So far only linear geometry is implemented for contact
 
  CHK_THROW_MESSAGE(ref_ele_ptr->getOptions(optionsPrefix), "getOptions");
  CHK_THROW_MESSAGE(ref_ele_ptr->generateReferenceElementMesh(),
                    "Error when generating reference element");
 
  MOFEM_LOG("EP", Sev::inform) << "Contact hoNodes " << ref_ele_ptr->hoNodes
      ? "true"
      : "false";
  MOFEM_LOG("EP", Sev::inform)
      << "Contact maxOrder " << ref_ele_ptr->defMaxLevel;
 
  refElementsMap[MBTRI] = ref_ele_ptr;
 
  int def_ele_id = -1;
  CHKERR getPostProcMesh().tag_get_handle("ELE_ID", 1, MB_TYPE_INTEGER, thEleId,
                                          MB_TAG_CREAT | MB_TAG_DENSE,
                                          &def_ele_id);
  CHKERR getPostProcMesh().tag_get_handle("BODY_ID", 1, MB_TYPE_INTEGER,
                                          thBodyId, MB_TAG_CREAT | MB_TAG_DENSE,
                                          &def_ele_id);
 
  // std::vector<int> def_ids(3 * nb_bases, 0);
  // CHKERR getPostProcMesh().tag_get_handle("IDS", 3 * nb_bases,
  // MB_TYPE_INTEGER,
  //                                         thIds, MB_TAG_CREAT | MB_TAG_DENSE,
  //                                         &*def_ids.begin());
  // std::vector<double> def_coeffs(3 * nb_bases, 0);
  // CHKERR getPostProcMesh().tag_get_handle("COEFF", 3 * nb_bases,
  // MB_TYPE_DOUBLE,
  //                                         thCoeff, MB_TAG_CREAT |
  //                                         MB_TAG_DENSE,
  //                                         &*def_coeffs.begin());
  // std::vector<double> def_basses(nb_bases, 0);
  // CHKERR getPostProcMesh().tag_get_handle("BASES", nb_bases, MB_TYPE_DOUBLE,
  //                                         thBases, MB_TAG_CREAT |
  //                                         MB_TAG_DENSE,
  //                                         &*def_basses.begin());
 
  std::array<double, 3> def_small_x{0., 0., 0.};
  CHKERR getPostProcMesh().tag_get_handle("x", 3, MB_TYPE_DOUBLE, thSmallX,
                                          MB_TAG_CREAT | MB_TAG_DENSE,
                                          def_small_x.data());
  CHKERR getPostProcMesh().tag_get_handle("X", 3, MB_TYPE_DOUBLE, thLargeX,
                                          MB_TAG_CREAT | MB_TAG_DENSE,
                                          def_small_x.data());
 
  std::array<double, 3> def_tractions{0., 0., 0.};
  CHKERR getPostProcMesh().tag_get_handle(
      "TRACTION", 3, MB_TYPE_DOUBLE, thTraction, MB_TAG_CREAT | MB_TAG_DENSE,
      &*def_tractions.begin());
}
 
MoFEMErrorCode ContactTree::preProcess() {
  MoFEMFunctionBegin;
  CHKERR Base::preProcess();
  MoFEMFunctionReturn(0);
}
 
MoFEMErrorCode ContactTree::postProcess() {
  MoFEMFunctionBegin;
 
  CHKERR Base::postProcess();
  shadowDataMap.clear();
 
  PetscBarrier(nullptr);
 
  ParallelComm *pcomm_post_proc_mesh =
      ParallelComm::get_pcomm(&getPostProcMesh(), MYPCOMM_INDEX);
  if (pcomm_post_proc_mesh == nullptr)
    SETERRQ(PETSC_COMM_WORLD, MOFEM_DATA_INCONSISTENCY, "PComm not allocated");
 
  // MOFEM_LOG("EP", Sev::verbose) << "ContactTree broadcast entities";
 
  auto brodacts = [&](auto &brodacts_ents) {
    MoFEMFunctionBegin;
    Range recived_ents;
    for (auto rr = 0; rr != mField.get_comm_size(); ++rr) {
      pcomm_post_proc_mesh->broadcast_entities(
          rr, rr == mField.get_comm_rank() ? brodacts_ents : recived_ents,
          false, true);
    }
    MoFEMFunctionReturn(0);
  };
 
  Range brodacts_ents;
  CHKERR getPostProcMesh().get_entities_by_dimension(0, 2, brodacts_ents, true);
  CHKERR brodacts(brodacts_ents);
 
  // MOFEM_LOG("EP", Sev::verbose) << "ContactTree build tree";
 
  Range ents;
  CHKERR getPostProcMesh().get_entities_by_dimension(0, 2, ents, true);
  CHKERR buildTree(ents);
 
#ifndef NDEBUG
  CHKERR writeFile("debug_tree.h5m");
#endif // NDEBUG
 
  MoFEMFunctionReturn(0);
}
 
MoFEMErrorCode ContactTree::buildTree(Range &ents) {
  MoFEMFunctionBegin;
  // MOFEM_LOG("SELF", Sev::inform) << ents << endl;
 
  treeSurfPtr = boost::shared_ptr<OrientedBoxTreeTool>(
      new OrientedBoxTreeTool(&getPostProcMesh(), "ROOTSETSURF", true));
  CHKERR treeSurfPtr->build(ents, rootSetSurf);
 
  // CHKERR treeSurfPtr->stats(rootSetSurf, std::cerr);
 
  MoFEMFunctionReturn(0);
}
 
struct OpMoveNode : public FaceElementForcesAndSourcesCore::UserDataOperator {
 
  using UOP = FaceElementForcesAndSourcesCore::UserDataOperator;
 
  OpMoveNode(boost::shared_ptr<ContactTree> contact_tree_ptr,
             boost::shared_ptr<ContactOps::CommonData> common_data_ptr,
             boost::shared_ptr<MatrixDouble> u_h1_ptr);
  MoFEMErrorCode doWork(int side, EntityType type,
                        EntitiesFieldData::EntData &data);
 
protected:
  boost::shared_ptr<ContactTree> contactTreePtr;
  boost::shared_ptr<MatrixDouble> uH1Ptr;
  boost::shared_ptr<ContactOps::CommonData> commonDataPtr;
};
 
OpMoveNode::OpMoveNode(
    boost::shared_ptr<ContactTree> contact_tree_ptr,
    boost::shared_ptr<ContactOps::CommonData> common_data_ptr,
    boost::shared_ptr<MatrixDouble> u_h1_ptr)
    : UOP(NOSPACE, UOP::OPSPACE), contactTreePtr(contact_tree_ptr),
      uH1Ptr(u_h1_ptr), commonDataPtr(common_data_ptr) {}
 
MoFEMErrorCode OpMoveNode::doWork(int side, EntityType type,
                                  EntitiesFieldData::EntData &data) {
  MoFEMFunctionBegin;
 
  auto get_body_id = [&](auto fe_ent) {
    for (auto &m : contactTreePtr->bodyMap) {
      if (m.second.find(fe_ent) != m.second.end()) {
        return m.first;
      }
    }
    return -1;
  };
 
  auto &moab_post_proc_mesh = contactTreePtr->getPostProcMesh();
  auto &post_proc_ents = contactTreePtr->getPostProcElements();
 
  auto fe_ent = getNumeredEntFiniteElementPtr()->getEnt();
  auto fe_id = id_from_handle(fe_ent);
  auto body_id = get_body_id(fe_ent);
  auto &map_gauss_pts = contactTreePtr->getMapGaussPts();
 
  CHKERR moab_post_proc_mesh.tag_clear_data(contactTreePtr->thEleId,
                                            post_proc_ents, &fe_id);
  CHKERR moab_post_proc_mesh.tag_clear_data(contactTreePtr->thBodyId,
                                            post_proc_ents, &body_id);
 
  auto nb_gauss_pts = getGaussPts().size2();
  auto t_u_h1 = getFTensor1FromMat<3>(*uH1Ptr);
  auto t_u_l2 = getFTensor1FromMat<3>(commonDataPtr->contactDisp);
  auto t_coords = getFTensor1CoordsAtGaussPts();
 
  MatrixDouble x_h1(nb_gauss_pts, 3);
  auto t_x_h1 = getFTensor1FromPtr<3>(&x_h1(0, 0));
  MatrixDouble x_l2(nb_gauss_pts, 3);
  auto t_x_l2 = getFTensor1FromPtr<3>(&x_l2(0, 0));
  MatrixDouble tractions = trans(commonDataPtr->contactTraction);
  MatrixDouble coords = getCoordsAtGaussPts();
 
  FTensor::Index<'i', 3> i;
 
  // VectorDouble bases(nb_bases, 0);
  for (auto gg = 0; gg != nb_gauss_pts; ++gg) {
    t_x_h1(i) = t_coords(i) + t_u_h1(i);
    t_x_l2(i) = t_coords(i) + t_u_l2(i);
 
    ++t_coords;
    ++t_u_h1;
    ++t_u_l2;
    ++t_x_h1;
    ++t_x_l2;
  }
 
  CHKERR moab_post_proc_mesh.set_coords(
      &*map_gauss_pts.begin(), map_gauss_pts.size(), &*x_h1.data().begin());
  CHKERR moab_post_proc_mesh.tag_set_data(
      contactTreePtr->thSmallX, &*map_gauss_pts.begin(), map_gauss_pts.size(),
      &*x_h1.data().begin());
  CHKERR moab_post_proc_mesh.tag_set_data(
      contactTreePtr->thLargeX, &*map_gauss_pts.begin(), map_gauss_pts.size(),
      &*coords.data().begin());
  CHKERR moab_post_proc_mesh.tag_set_data(
      contactTreePtr->thTraction, &*map_gauss_pts.begin(), map_gauss_pts.size(),
      &*tractions.data().begin());
 
  MoFEMFunctionReturn(0);
}
 
struct OpTreeSearch : public FaceElementForcesAndSourcesCore::UserDataOperator {
 
  using UOP = FaceElementForcesAndSourcesCore::UserDataOperator;
 
  OpTreeSearch(boost::shared_ptr<ContactTree> contact_tree_ptr,
               boost::shared_ptr<MatrixDouble> u_h1_ptr,
               boost::shared_ptr<MatrixDouble> traction_ptr, Range r,
 
               moab::Interface *post_proc_mesh_ptr,
               std::vector<EntityHandle> *map_gauss_pts_ptr
 
  );
  MoFEMErrorCode doWork(int side, EntityType type,
                        EntitiesFieldData::EntData &data);
 
protected:
  boost::shared_ptr<ContactTree> contactTreePtr;
  boost::shared_ptr<MatrixDouble> uH1Ptr;
  boost::shared_ptr<MatrixDouble> tractionPtr;
  moab::Interface *postProcMeshPtr = nullptr;
  std::vector<EntityHandle> *mapGaussPtsPtr = nullptr;
 
  Range contactRange;
};
 
OpTreeSearch::OpTreeSearch(boost::shared_ptr<ContactTree> contact_tree_ptr,
                           boost::shared_ptr<MatrixDouble> u_h1_ptr,
                           boost::shared_ptr<MatrixDouble> traction_ptr,
                           Range r,
 
                           moab::Interface *post_proc_mesh_ptr,
                           std::vector<EntityHandle> *map_gauss_pts_ptr
 
                           )
    : UOP(NOSPACE, UOP::OPSPACE), contactTreePtr(contact_tree_ptr),
      uH1Ptr(u_h1_ptr), tractionPtr(traction_ptr),
      postProcMeshPtr(post_proc_mesh_ptr), mapGaussPtsPtr(map_gauss_pts_ptr),
      contactRange(r) {}
 
MoFEMErrorCode OpTreeSearch::doWork(int side, EntityType type,
                                    EntitiesFieldData::EntData &data) {
  MoFEMFunctionBegin;
 
  auto &m_field = getPtrFE()->mField;
  auto fe_ent = getNumeredEntFiniteElementPtr()->getEnt();
  auto fe_id = id_from_handle(fe_ent);
 
  if (contactRange.find(fe_ent) == contactRange.end())
    MoFEMFunctionReturnHot(0);
 
  FTensor::Index<'i', 3> i;
  FTensor::Index<'j', 3> j;
 
  const auto nb_gauss_pts = getGaussPts().size2();
 
  auto t_disp_h1 = getFTensor1FromMat<3>(*uH1Ptr);
  auto t_coords = getFTensor1CoordsAtGaussPts();
  auto t_traction = getFTensor1FromMat<3>(*tractionPtr);
 
  auto next = [&]() {
    ++t_disp_h1;
    ++t_traction;
    ++t_coords;
  };
 
  auto get_ele_centre = [i](auto t_ele_coords) {
    FTensor::Tensor1<double, 3> t_ele_center;
    t_ele_center(i) = 0;
    for (int nn = 0; nn != 3; nn++) {
      t_ele_center(i) += t_ele_coords(i);
      ++t_ele_coords;
    }
    t_ele_center(i) /= 3;
    return t_ele_center;
  };
 
  auto get_ele_radius = [i](auto t_ele_center, auto t_ele_coords) {
    FTensor::Tensor1<double, 3> t_n0;
    t_n0(i) = t_ele_center(i) - t_ele_coords(i);
    return t_n0.l2();
  };
 
  auto get_face_conn = [this](auto face) {
    const EntityHandle *conn;
    int num_nodes;
    CHK_MOAB_THROW(contactTreePtr->getPostProcMesh().get_connectivity(
                       face, conn, num_nodes, true),
                   "get conn");
    if (num_nodes != 3) {
      CHK_THROW_MESSAGE(MOFEM_DATA_INCONSISTENCY, "face is not a triangle");
    }
    return conn;
  };
 
  auto get_face_coords = [this](auto conn) {
    std::array<double, 9> coords;
    CHKERR contactTreePtr->getPostProcMesh().get_coords(conn, 3, coords.data());
    return coords;
  };
 
  auto get_closet_face = [this](auto *point_ptr, auto r) {
    FTensor::Tensor1<double, 3> t_point_out;
    std::vector<EntityHandle> faces_out;
    CHK_MOAB_THROW(
        contactTreePtr->getTreeSurfPtr()->sphere_intersect_triangles(
            point_ptr, r / 8, contactTreePtr->getRootSetSurf(), faces_out),
        "get closest faces");
    return faces_out;
  };
 
  auto get_faces_out = [this](auto *point_ptr, auto *unit_ray_ptr, auto radius,
                              auto eps) {
    std::vector<double> distances_out;
    std::vector<EntityHandle> faces_out;
    CHK_MOAB_THROW(
 
        contactTreePtr->getTreeSurfPtr()->ray_intersect_triangles(
            distances_out, faces_out, contactTreePtr->getRootSetSurf(), eps,
            point_ptr, unit_ray_ptr, &radius),
 
        "get closest faces");
    return std::make_pair(faces_out, distances_out);
  };
 
  auto get_normal = [](auto &ele_coords) {
    FTensor::Tensor1<double, 3> t_normal;
    Tools::getTriNormal(ele_coords.data(), &t_normal(0));
    return t_normal;
  };
 
  auto make_map = [&](auto &face_out, auto &face_dist, auto &t_ray_point,
                      auto &t_unit_ray, auto &t_master_coord) {
    FTensor::Index<'i', 3> i;
    FTensor::Index<'j', 3> j;
    std::map<double, EntityHandle> m;
    for (auto ii = 0; ii != face_out.size(); ++ii) {
      auto face_conn = get_face_conn(face_out[ii]);
      auto face_coords = get_face_coords(face_conn);
      auto t_face_normal = get_normal(face_coords);
      t_face_normal.normalize();
      FTensor::Tensor1<double, 3> t_x;
      t_x(i) = t_ray_point(i) + t_unit_ray(i) * face_dist[ii];
      FTensor::Tensor2<double, 3, 3> t_chi;
      t_chi(i, j) =
          t_x(i) * t_face_normal(j) - t_master_coord(i) * t_unit_ray(j);
      if (t_unit_ray(i) * t_face_normal(i) > std::cos(M_PI / 3)) {
        auto dot = std::sqrt(t_chi(i, j) * t_chi(i, j));
        m[dot] = face_out[ii];
      }
    }
    return m;
  };
 
  auto get_tag_data = [this](auto tag, auto face, auto &vec) {
    MoFEMFunctionBegin;
    int tag_length;
    CHKERR contactTreePtr->getPostProcMesh().tag_get_length(tag, tag_length);
    vec.resize(tag_length);
    CHKERR contactTreePtr->getPostProcMesh().tag_get_data(tag, &face, 1,
                                                          &*vec.begin());
    MoFEMFunctionReturn(0);
  };
 
  auto create_tag = [this](const std::string tag_name, const int size) {
    double def_VAL[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
    Tag th;
    if (postProcMeshPtr) {
      CHKERR postProcMeshPtr->tag_get_handle(
          tag_name.c_str(), size, MB_TYPE_DOUBLE, th,
          MB_TAG_CREAT | MB_TAG_SPARSE, def_VAL);
      CHKERR postProcMeshPtr->tag_clear_data(th, &*mapGaussPtsPtr->begin(),
                                             mapGaussPtsPtr->size(), def_VAL);
    }
    return th;
  };
 
  auto set_float_precision = [](const double x) {
    if (std::abs(x) < std::numeric_limits<float>::epsilon())
      return 0.;
    else
      return x;
  };
 
  // scalars
  auto save_scal_tag = [&](auto &th, auto v, const int gg) {
    MoFEMFunctionBegin;
    if (postProcMeshPtr) {
      v = set_float_precision(v);
      CHKERR postProcMeshPtr->tag_set_data(th, &(*mapGaussPtsPtr)[gg], 1, &v);
    }
    MoFEMFunctionReturn(0);
  };
 
  // adjacencies
  auto get_fe_adjacencies = [this](auto fe_ent) {
    Range adj_faces;
    CHK_MOAB_THROW(getFaceFE()->mField.get_moab().get_adjacencies(
                       &fe_ent, 1, 2, false, adj_faces, moab::Interface::UNION),
                   "get adj");
    std::set<int> adj_ids;
    for (auto f : adj_faces) {
      adj_ids.insert(id_from_handle(f));
    }
    return adj_ids;
  };
 
  auto get_face_id = [this](auto face) {
    int id;
    if (contactTreePtr->getPostProcMesh().tag_get_data(
            contactTreePtr->thEleId, &face, 1, &id) == MB_SUCCESS) {
      return id;
    }
    return -1;
  };
 
  auto get_body_id = [this](auto face) {
    int id;
    if (contactTreePtr->getPostProcMesh().tag_get_data(
            contactTreePtr->thBodyId, &face, 1, &id) == MB_SUCCESS) {
      return id;
    }
    return -1;
  };
 
  auto get_face_part = [this](auto face) {
    ParallelComm *pcomm_post_proc_mesh = ParallelComm::get_pcomm(
        &(contactTreePtr->getPostProcMesh()), MYPCOMM_INDEX);
    int part;
    if (contactTreePtr->getPostProcMesh().tag_get_data(
            pcomm_post_proc_mesh->part_tag(), &face, 1, &part) == MB_SUCCESS) {
      return part;
    }
    return -1;
  };
 
  auto check_face = [&](auto face, auto fe_id, auto part) {
    auto face_id = get_face_id(face);
    auto face_part = get_face_part(face);
    if (face_id == fe_id && face_part == part)
      return true;
    return false;
  };
 
  // vectors
  VectorDouble3 v(3);
  FTensor::Tensor1<FTensor::PackPtr<double *, 0>, 3> t_v(&v[0], &v[1], &v[2]);
  auto save_vec_tag = [&](auto &th, auto &t_d, const int gg) {
    MoFEMFunctionBegin;
    if (postProcMeshPtr) {
      t_v(i) = t_d(i);
      for (auto &a : v.data())
        a = set_float_precision(a);
      CHKERR postProcMeshPtr->tag_set_data(th, &(*mapGaussPtsPtr)[gg], 1,
                                           &*v.data().begin());
    }
    MoFEMFunctionReturn(0);
  };
 
  Tag th_mark = create_tag("contact_mark", 1);
  Tag th_mark_slave = create_tag("contact_mark_slave", 1);
  Tag th_body_id = create_tag("contact_body_id", 1);
  Tag th_gap = create_tag("contact_gap", 1);
  Tag th_tn_master = create_tag("contact_tn_master", 1);
  Tag th_tn_slave = create_tag("contact_tn_slave", 1);
  Tag th_contact_traction = create_tag("contact_traction", 3);
  Tag th_contact_traction_master = create_tag("contact_traction_master", 3);
  Tag th_contact_traction_slave = create_tag("contact_traction_slave", 3);
  Tag th_c = create_tag("contact_c", 1);
  Tag th_normal = create_tag("contact_normal", 3);
  Tag th_dist = create_tag("contact_dip", 3);
 
  auto t_ele_centre = get_ele_centre(getFTensor1Coords());
  auto ele_radius = get_ele_radius(t_ele_centre, getFTensor1Coords());
 
  contactTreePtr->shadowDataMap.clear();
  auto &shadow_vec = contactTreePtr->shadowDataMap[fe_ent];
  shadow_vec.clear();
 
  auto adj_fe_ids = get_fe_adjacencies(fe_ent);
 
  for (auto gg = 0; gg != nb_gauss_pts; ++gg) {
 
    FTensor::Tensor1<double, 3> t_spatial_coords;
    t_spatial_coords(i) = t_coords(i) + t_disp_h1(i);
 
    if (postProcMeshPtr) {
      CHKERR save_vec_tag(th_contact_traction, t_traction, gg);
    }
 
    auto faces_close = get_closet_face(&t_spatial_coords(0), ele_radius);
    for (auto face_close : faces_close) {
      if (check_face(face_close, fe_id, m_field.get_comm_rank())) {
 
        auto body_id = get_body_id(face_close);
 
        auto master_face_conn = get_face_conn(face_close);
        std::array<double, 9> master_coords;
        CHKERR contactTreePtr->getPostProcMesh().tag_get_data(
            contactTreePtr->thSmallX, master_face_conn, 3,
            master_coords.data());
        std::array<double, 9> master_traction;
        CHKERR contactTreePtr->getPostProcMesh().tag_get_data(
            contactTreePtr->thTraction, master_face_conn, 3,
            master_traction.data());
        auto t_normal_face_close = get_normal(master_coords);
        t_normal_face_close.normalize();
 
        if (postProcMeshPtr) {
          double m = 1;
          CHKERR save_scal_tag(th_mark, m, gg);
          CHKERR save_scal_tag(th_body_id, static_cast<double>(body_id), gg);
          CHKERR save_vec_tag(th_normal, t_normal_face_close, gg);
          CHKERR save_vec_tag(th_contact_traction, t_traction, gg);
        }
 
        FTensor::Tensor1<double, 3> t_unit_ray;
        t_unit_ray(i) = -t_normal_face_close(i);
        FTensor::Tensor1<double, 3> t_ray_point;
        t_ray_point(i) =
            t_spatial_coords(i) -
            t_unit_ray(i) * ContactOps::airplane_ray_distance * ele_radius;
 
        constexpr double eps = 1e-3;
        auto [faces_out, faces_dist] =
            get_faces_out(&t_ray_point(0), &t_unit_ray(0),
                          2 * ContactOps::airplane_ray_distance * ele_radius,
                          eps * ele_radius);
 
        auto m = make_map(faces_out, faces_dist, t_ray_point, t_unit_ray,
                          t_spatial_coords);
        for (auto m_it = m.begin(); m_it != m.end(); ++m_it) {
          auto face = m_it->second;
          if (face != face_close) {
 
            if (
 
                (adj_fe_ids.find(get_face_id(face)) == adj_fe_ids.end() ||
                 get_face_part(face) != m_field.get_comm_rank())
 
            ) {
 
              shadow_vec.push_back(ContactTree::FaceData());
              shadow_vec.back().gaussPtNb = gg;
 
              auto slave_face_conn = get_face_conn(face);
              std::array<double, 9> slave_coords;
              CHKERR contactTreePtr->getPostProcMesh().tag_get_data(
                  contactTreePtr->thSmallX, slave_face_conn, 3,
                  slave_coords.data());
              auto t_normal_face = get_normal(slave_coords);
              std::array<double, 9> slave_tractions;
              CHKERR contactTreePtr->getPostProcMesh().tag_get_data(
                  contactTreePtr->thTraction, slave_face_conn, 3,
                  slave_tractions.data());
 
              auto t_master_point =
                  getFTensor1FromPtr<3>(shadow_vec.back().masterPoint.data());
              auto t_slave_point =
                  getFTensor1FromPtr<3>(shadow_vec.back().slavePoint.data());
              auto t_ray_point_data =
                  getFTensor1FromPtr<3>(shadow_vec.back().rayPoint.data());
              auto t_unit_ray_data =
                  getFTensor1FromPtr<3>(shadow_vec.back().unitRay.data());
 
              t_slave_point(i) = t_ray_point(i) + m_it->first * t_unit_ray(i);
 
              auto eval_position = [&](auto &&t_elem_coords, auto &&t_point) {
                std::array<double, 2> loc_coords;
                CHK_THROW_MESSAGE(
                    Tools::getLocalCoordinatesOnReferenceThreeNodeTri(
                        &t_elem_coords(0, 0), &t_point(0), 1,
                        loc_coords.data()),
                    "get local coords");
                FTensor::Tensor1<double, 3> t_shape_fun;
                CHK_THROW_MESSAGE(Tools::shapeFunMBTRI<0>(&t_shape_fun(0),
                                                          &loc_coords[0],
                                                          &loc_coords[1], 1),
                                  "calc shape fun");
                FTensor::Index<'i', 3> i;
                FTensor::Index<'j', 3> j;
                FTensor::Tensor1<double, 3> t_point_out;
                t_point_out(i) = t_shape_fun(j) * t_elem_coords(j, i);
                return t_point_out;
              };
 
              auto t_master_point_updated = eval_position(
                  getFTensor2FromPtr<3, 3>(master_coords.data()),
                  getFTensor1FromPtr<3>(shadow_vec.back().masterPoint.data()));
              t_master_point(i) = t_master_point_updated(i);
 
              auto t_slave_point_updated = eval_position(
                  getFTensor2FromPtr<3, 3>(slave_coords.data()),
                  getFTensor1FromPtr<3>(shadow_vec.back().slavePoint.data()));
              t_slave_point(i) = t_slave_point_updated(i);
 
              t_ray_point_data(i) = t_ray_point(i);
              t_unit_ray_data(i) = t_unit_ray(i);
 
              std::copy(master_coords.begin(), master_coords.end(),
                        shadow_vec.back().masterPointNodes.begin());
              std::copy(master_traction.begin(), master_traction.end(),
                        shadow_vec.back().masterTractionNodes.begin());
              std::copy(slave_coords.begin(), slave_coords.end(),
                        shadow_vec.back().slavePointNodes.begin());
              std::copy(slave_tractions.begin(), slave_tractions.end(),
                        shadow_vec.back().slaveTractionNodes.begin());
 
              shadow_vec.back().eleRadius = ele_radius;
 
              // CHKERR get_tag_data(contactTreePtr->thIds, face,
              //                     shadow_vec.back().dofsSlaveIds);
              // CHKERR get_tag_data(contactTreePtr->thCoeff, face,
              //                     shadow_vec.back().dofsSlaveCoeff);
              // CHKERR get_tag_data(contactTreePtr->thBases, face,
              //                     shadow_vec.back().baseSlaveFuncs);
 
              if (postProcMeshPtr) {
                auto [gap, tn_master, tn_slave, c, t_master_traction,
                      t_slave_traction] =
                    multiGetGap(&(shadow_vec.back()), t_spatial_coords);
                FTensor::Tensor1<double, 3> t_gap_vec;
                t_gap_vec(i) = t_slave_point(i) - t_spatial_coords(i);
                CHKERR save_scal_tag(th_gap, gap, gg);
                CHKERR save_scal_tag(th_tn_master, tn_master, gg);
                CHKERR save_scal_tag(th_tn_slave, tn_slave, gg);
                CHKERR save_scal_tag(th_c, c, gg);
                double m = 1;
                CHKERR save_scal_tag(th_mark_slave, m, gg);
                CHKERR save_vec_tag(th_dist, t_gap_vec, gg);
                CHKERR save_vec_tag(th_contact_traction_master,
                                    t_master_traction, gg);
                CHKERR save_vec_tag(th_contact_traction_slave, t_slave_traction,
                                    gg);
              }
 
              break;
            }
          }
        }
        break;
      }
    }
    next();
  }
 
  MoFEMFunctionReturn(0);
}
 
static auto get_range_from_block(MoFEM::Interface &m_field,
                                 const std::string block_name, int dim) {
  Range r;
 
  auto mesh_mng = m_field.getInterface<MeshsetsManager>();
  auto bcs = mesh_mng->getCubitMeshsetPtr(
 
      std::regex((boost::format("%s(.*)") % block_name).str())
 
  );
 
  for (auto bc : bcs) {
    Range faces;
    CHK_MOAB_THROW(bc->getMeshsetIdEntitiesByDimension(m_field.get_moab(), dim,
                                                       faces, true),
                   "get meshset ents");
    r.merge(faces);
  }
 
  return r;
};
 
boost::shared_ptr<ForcesAndSourcesCore>
createContactDetectionFiniteElement(EshelbianCore &ep) {
 
  auto &m_field = ep.mField;
 
  boost::shared_ptr<ContactTree> fe_contact_tree;
 
  auto impl = [&]() {
    MoFEMFunctionBegin;
 
    /** Contact requires that body is marked */
    auto get_body_range = [&](auto name, int dim, auto sev) {
      std::map<int, Range> map;
 
      for (auto m_ptr :
           m_field.getInterface<MeshsetsManager>()->getCubitMeshsetPtr(
               std::regex(
 
                   (boost::format("%s(.*)") % name).str()
 
                       ))
 
      ) {
        Range ents;
        CHK_MOAB_THROW(m_ptr->getMeshsetIdEntitiesByDimension(
                           m_field.get_moab(), dim, ents, true),
                       "by dim");
        map[m_ptr->getMeshsetId()] = ents;
        MOFEM_LOG("EPSYNC", sev) << "Meshset: " << m_ptr->getMeshsetId() << " "
                                 << ents.size() << " entities";
      }
 
      MOFEM_LOG_SEVERITY_SYNC(m_field.get_comm(), sev);
      return map;
    };
 
    auto get_map_skin = [&](auto &&map) {
      ParallelComm *pcomm =
          ParallelComm::get_pcomm(&m_field.get_moab(), MYPCOMM_INDEX);
 
      Skinner skin(&m_field.get_moab());
      for (auto &m : map) {
        Range skin_faces;
        CHKERR skin.find_skin(0, m.second, false, skin_faces);
        CHK_MOAB_THROW(pcomm->filter_pstatus(
                           skin_faces, PSTATUS_SHARED | PSTATUS_MULTISHARED,
                           PSTATUS_NOT, -1, nullptr),
                       "filter");
        m.second.swap(skin_faces);
      }
      return map;
    };
 
    /* The above code is written in C++ and it appears to be defining and using
    various operations on boundary elements and side elements. */
    using BoundaryEle = FaceElementForcesAndSourcesCore;
    using BoundaryEleOp = BoundaryEle::UserDataOperator;
 
    auto contact_common_data_ptr = boost::make_shared<ContactOps::CommonData>();
 
    auto calcs_side_traction = [&](auto &pip) {
      MoFEMFunctionBegin;
      using EleOnSide =
          PipelineManager::ElementsAndOpsByDim<SPACE_DIM>::FaceSideEle;
      using SideEleOp = EleOnSide::UserDataOperator;
      auto op_loop_domain_side = new OpLoopSide<EleOnSide>(
          m_field, ep.elementVolumeName, SPACE_DIM, Sev::noisy);
      op_loop_domain_side->getSideFEPtr()->getUserPolynomialBase() =
          boost::make_shared<CGGUserPolynomialBase>();
      EshelbianPlasticity::AddHOOps<SPACE_DIM, SPACE_DIM, SPACE_DIM>::add(
          op_loop_domain_side->getOpPtrVector(), {HDIV, H1, L2},
          ep.materialH1Positions, ep.frontAdjEdges);
      op_loop_domain_side->getOpPtrVector().push_back(
          new OpCalculateHVecTensorTrace<SPACE_DIM, SideEleOp>(
              ep.piolaStress, contact_common_data_ptr->contactTractionPtr(),
              boost::make_shared<double>(1.0)));
      pip.push_back(op_loop_domain_side);
      MoFEMFunctionReturn(0);
    };
 
    auto add_contact_three = [&]() {
      MoFEMFunctionBegin;
      auto tree_moab_ptr = boost::make_shared<moab::Core>();
      fe_contact_tree = boost::make_shared<ContactTree>(
          m_field, tree_moab_ptr, ep.spaceOrder,
          get_body_range("CONTACT", SPACE_DIM - 1, Sev::inform));
      fe_contact_tree->getOpPtrVector().push_back(
          new OpCalculateVectorFieldValues<3>(
              ep.contactDisp, contact_common_data_ptr->contactDispPtr()));
      CHKERR calcs_side_traction(fe_contact_tree->getOpPtrVector());
      auto u_h1_ptr = boost::make_shared<MatrixDouble>();
      fe_contact_tree->getOpPtrVector().push_back(
          new OpCalculateVectorFieldValues<3>(ep.spatialH1Disp, u_h1_ptr));
      fe_contact_tree->getOpPtrVector().push_back(
          new OpMoveNode(fe_contact_tree, contact_common_data_ptr, u_h1_ptr));
      MoFEMFunctionReturn(0);
    };
 
    CHKERR add_contact_three();
 
    MoFEMFunctionReturn(0);
  };
 
  CHK_THROW_MESSAGE(impl(), "createContactDetectionFiniteElement");
 
  struct exclude_sdf {
    exclude_sdf(Range &&r) : map(r) {}
    bool operator()(FEMethod *fe_method_ptr) {
      auto ent = fe_method_ptr->getFEEntityHandle();
      if (map.find(ent) != map.end()) {
        return false;
      }
      return true;
    }
 
  private:
    Range map;
  };
 
  fe_contact_tree->exeTestHook =
      exclude_sdf(get_range_from_block(m_field, "CONTACT_SDF", SPACE_DIM - 1));
 
  return fe_contact_tree;
};
 
static auto get_body_range(MoFEM::Interface &m_field, const std::string name,
                           int dim) {
  std::map<int, Range> map;
 
  for (auto m_ptr :
       m_field.getInterface<MeshsetsManager>()->getCubitMeshsetPtr(std::regex(
 
           (boost::format("%s(.*)") % name).str()
 
               ))
 
  ) {
    Range ents;
    CHK_MOAB_THROW(m_ptr->getMeshsetIdEntitiesByDimension(m_field.get_moab(),
                                                          dim, ents, true),
                   "by dim");
    map[m_ptr->getMeshsetId()] = ents;
  }
 
  return map;
};
 
MoFEMErrorCode pushContactOpsRhs(
    EshelbianCore &ep, boost::shared_ptr<ForcesAndSourcesCore> fe_ptr,
    boost::ptr_deque<ForcesAndSourcesCore::UserDataOperator> &pip) {
  MoFEMFunctionBegin;
 
  auto contact_tree_ptr = boost::dynamic_pointer_cast<ContactTree>(fe_ptr);
 
  auto &m_field = ep.mField;
 
  using BoundaryEle =
      PipelineManager::ElementsAndOpsByDim<SPACE_DIM>::BoundaryEle;
  using EleOnSide =
      PipelineManager::ElementsAndOpsByDim<SPACE_DIM>::FaceSideEle;
  using SideEleOp = EleOnSide::UserDataOperator;
  using BdyEleOp = BoundaryEle::UserDataOperator;
 
  // First: Iterate over skeleton FEs adjacent to Domain FEs
  // Note:  BoundaryEle, i.e. uses skeleton interation rule
  auto op_loop_skeleton_side = new OpLoopSide<BoundaryEle>(
      m_field, ep.contactElement, SPACE_DIM - 1, Sev::noisy);
 
  auto rule_contact = [](int, int, int o) { return -1; };
  auto refine = Tools::refineTriangle(ep.contactRefinementLevels);
 
  auto set_rule_contact = [refine](
 
                              ForcesAndSourcesCore *fe_raw_ptr, int order_row,
                              int order_col, int order_data
 
                          ) {
    MoFEMFunctionBegin;
    auto rule = 2 * order_data;
    fe_raw_ptr->gaussPts = Tools::refineTriangleIntegrationPts(rule, refine);
    MoFEMFunctionReturn(0);
  };
 
  op_loop_skeleton_side->getSideFEPtr()->getRuleHook = rule_contact;
  op_loop_skeleton_side->getSideFEPtr()->setRuleHook = set_rule_contact;
  CHKERR
  EshelbianPlasticity::AddHOOps<SPACE_DIM - 1, SPACE_DIM, SPACE_DIM>::add(
      op_loop_skeleton_side->getOpPtrVector(), {L2}, ep.materialH1Positions,
      ep.frontAdjEdges);
 
  // Second: Iterate over domain FEs adjacent to skelton, particularly
  // one domain element.
  auto broken_data_ptr = boost::make_shared<std::vector<BrokenBaseSideData>>();
 
  // Data storing contact fields
  auto contact_common_data_ptr = boost::make_shared<ContactOps::CommonData>();
 
  auto add_ops_domain_side = [&](auto &pip) {
    MoFEMFunctionBegin;
    // Note: EleOnSide, i.e. uses on domain projected skeleton rule
    auto op_loop_domain_side = new OpBrokenLoopSide<EleOnSide>(
        m_field, ep.elementVolumeName, SPACE_DIM, Sev::noisy);
    op_loop_domain_side->getSideFEPtr()->getUserPolynomialBase() =
        boost::make_shared<CGGUserPolynomialBase>();
    CHKERR
    EshelbianPlasticity::AddHOOps<SPACE_DIM, SPACE_DIM, SPACE_DIM>::add(
        op_loop_domain_side->getOpPtrVector(), {HDIV, H1, L2},
        ep.materialH1Positions, ep.frontAdjEdges);
    op_loop_domain_side->getOpPtrVector().push_back(
        new OpGetBrokenBaseSideData<SideEleOp>(ep.piolaStress,
                                               broken_data_ptr));
    op_loop_domain_side->getOpPtrVector().push_back(
        new OpCalculateHVecTensorTrace<SPACE_DIM, SideEleOp>(
            ep.piolaStress, contact_common_data_ptr->contactTractionPtr()));
    pip.push_back(op_loop_domain_side);
    MoFEMFunctionReturn(0);
  };
 
  auto add_ops_contact_rhs = [&](auto &pip) {
    MoFEMFunctionBegin;
    // get body id and SDF range
    auto contact_sfd_map_range_ptr = boost::make_shared<std::map<int, Range>>(
        get_body_range(m_field, "CONTACT_SDF", SPACE_DIM - 1));
 
    pip.push_back(new OpCalculateVectorFieldValues<3>(
        ep.contactDisp, contact_common_data_ptr->contactDispPtr()));
    auto u_h1_ptr = boost::make_shared<MatrixDouble>();
    pip.push_back(
        new OpCalculateVectorFieldValues<3>(ep.spatialH1Disp, u_h1_ptr));
    pip.push_back(new OpTreeSearch(
        contact_tree_ptr, u_h1_ptr,
        contact_common_data_ptr->contactTractionPtr(),
        get_range_from_block(m_field, "CONTACT", SPACE_DIM - 1), nullptr,
        nullptr));
    pip.push_back(new EshelbianPlasticity::OpConstrainBoundaryL2Rhs(
        ep.contactDisp, contact_common_data_ptr, contact_tree_ptr,
        contact_sfd_map_range_ptr));
    pip.push_back(new EshelbianPlasticity::OpConstrainBoundaryHDivRhs<A, GAUSS>(
        broken_data_ptr, contact_common_data_ptr, contact_tree_ptr));
 
    MoFEMFunctionReturn(0);
  };
 
  // push ops to face/side pipeline
  CHKERR add_ops_domain_side(op_loop_skeleton_side->getOpPtrVector());
  CHKERR add_ops_contact_rhs(op_loop_skeleton_side->getOpPtrVector());
 
  // Add skeleton to domain pipeline
  pip.push_back(op_loop_skeleton_side);
 
  MoFEMFunctionReturn(0);
};
 
MoFEMErrorCode pushContactOpsLhs(
    EshelbianCore &ep, boost::shared_ptr<ForcesAndSourcesCore> fe_ptr,
    boost::ptr_deque<ForcesAndSourcesCore::UserDataOperator> &pip) {
  MoFEMFunctionBegin;
 
  auto contact_tree_ptr = boost::dynamic_pointer_cast<ContactTree>(fe_ptr);
  auto &m_field = ep.mField;
 
  using BoundaryEle =
      PipelineManager::ElementsAndOpsByDim<SPACE_DIM>::BoundaryEle;
  using EleOnSide =
      PipelineManager::ElementsAndOpsByDim<SPACE_DIM>::FaceSideEle;
  using SideEleOp = EleOnSide::UserDataOperator;
  using BdyEleOp = BoundaryEle::UserDataOperator;
 
  // First: Iterate over skeleton FEs adjacent to Domain FEs
  // Note:  BoundaryEle, i.e. uses skeleton interation rule
  auto op_loop_skeleton_side = new OpLoopSide<BoundaryEle>(
      m_field, ep.contactElement, SPACE_DIM - 1, Sev::noisy);
 
  auto rule_contact = [](int, int, int o) { return -1; };
  auto refine = Tools::refineTriangle(ep.contactRefinementLevels);
 
  auto set_rule_contact = [refine](
 
                              ForcesAndSourcesCore *fe_raw_ptr, int order_row,
                              int order_col, int order_data
 
                          ) {
    MoFEMFunctionBegin;
    auto rule = 2 * order_data;
    fe_raw_ptr->gaussPts = Tools::refineTriangleIntegrationPts(rule, refine);
    MoFEMFunctionReturn(0);
  };
 
  op_loop_skeleton_side->getSideFEPtr()->getRuleHook = rule_contact;
  op_loop_skeleton_side->getSideFEPtr()->setRuleHook = set_rule_contact;
  CHKERR
  EshelbianPlasticity::AddHOOps<SPACE_DIM - 1, SPACE_DIM, SPACE_DIM>::add(
      op_loop_skeleton_side->getOpPtrVector(), {L2}, ep.materialH1Positions,
      ep.frontAdjEdges);
 
  // Second: Iterate over domain FEs adjacent to skelton, particularly
  // one domain element.
  auto broken_data_ptr = boost::make_shared<std::vector<BrokenBaseSideData>>();
 
  // Data storing contact fields
  auto contact_common_data_ptr = boost::make_shared<ContactOps::CommonData>();
 
  auto add_ops_domain_side = [&](auto &pip) {
    MoFEMFunctionBegin;
    // Note: EleOnSide, i.e. uses on domain projected skeleton rule
    auto op_loop_domain_side = new OpBrokenLoopSide<EleOnSide>(
        m_field, ep.elementVolumeName, SPACE_DIM, Sev::noisy);
    op_loop_domain_side->getSideFEPtr()->getUserPolynomialBase() =
        boost::make_shared<CGGUserPolynomialBase>();
    CHKERR
    EshelbianPlasticity::AddHOOps<SPACE_DIM, SPACE_DIM, SPACE_DIM>::add(
        op_loop_domain_side->getOpPtrVector(), {HDIV, H1, L2},
        ep.materialH1Positions, ep.frontAdjEdges);
    op_loop_domain_side->getOpPtrVector().push_back(
        new OpGetBrokenBaseSideData<SideEleOp>(ep.piolaStress,
                                               broken_data_ptr));
    op_loop_domain_side->getOpPtrVector().push_back(
        new OpCalculateHVecTensorTrace<SPACE_DIM, SideEleOp>(
            ep.piolaStress, contact_common_data_ptr->contactTractionPtr()));
    pip.push_back(op_loop_domain_side);
    MoFEMFunctionReturn(0);
  };
 
  auto add_ops_contact_lhs = [&](auto &pip) {
    MoFEMFunctionBegin;
    pip.push_back(new OpCalculateVectorFieldValues<3>(
        ep.contactDisp, contact_common_data_ptr->contactDispPtr()));
    auto u_h1_ptr = boost::make_shared<MatrixDouble>();
    pip.push_back(
        new OpCalculateVectorFieldValues<3>(ep.spatialH1Disp, u_h1_ptr));
    pip.push_back(new OpTreeSearch(
        contact_tree_ptr, u_h1_ptr,
        contact_common_data_ptr->contactTractionPtr(),
        get_range_from_block(m_field, "CONTACT", SPACE_DIM - 1), nullptr,
        nullptr));
 
    // get body id and SDF range
    auto contact_sfd_map_range_ptr = boost::make_shared<std::map<int, Range>>(
        get_body_range(m_field, "CONTACT_SDF", SPACE_DIM - 1));
 
    pip.push_back(new EshelbianPlasticity::OpConstrainBoundaryL2Lhs_dU(
        ep.contactDisp, ep.contactDisp, contact_common_data_ptr,
        contact_tree_ptr, contact_sfd_map_range_ptr));
    pip.push_back(
        new EshelbianPlasticity::OpConstrainBoundaryL2Lhs_dP<A, GAUSS>(
            ep.contactDisp, broken_data_ptr, contact_common_data_ptr,
            contact_tree_ptr, contact_sfd_map_range_ptr));
    pip.push_back(
        new EshelbianPlasticity::OpConstrainBoundaryHDivLhs_dU<A, GAUSS>(
            broken_data_ptr, ep.contactDisp, contact_common_data_ptr,
            contact_tree_ptr));
 
    MoFEMFunctionReturn(0);
  };
 
  // push ops to face/side pipeline
  CHKERR add_ops_domain_side(op_loop_skeleton_side->getOpPtrVector());
  CHKERR add_ops_contact_lhs(op_loop_skeleton_side->getOpPtrVector());
 
  // Add skeleton to domain pipeline
  pip.push_back(op_loop_skeleton_side);
 
  MoFEMFunctionReturn(0);
};
 
ForcesAndSourcesCore::UserDataOperator *
getOpContactDetection(EshelbianCore &ep,
                      boost::shared_ptr<ForcesAndSourcesCore> fe_ptr,
                      boost::shared_ptr<MatrixDouble> u_h1_ptr,
                      boost::shared_ptr<MatrixDouble> contact_traction_ptr,
                      Range r, moab::Interface *post_proc_mesh_ptr,
                      std::vector<EntityHandle> *map_gauss_pts_ptr) {
 
  auto &m_field = ep.mField;
  auto contact_tree_ptr = boost::dynamic_pointer_cast<ContactTree>(fe_ptr);
  return new OpTreeSearch(
      contact_tree_ptr, u_h1_ptr, contact_traction_ptr,
      get_range_from_block(m_field, "CONTACT", SPACE_DIM - 1),
      post_proc_mesh_ptr, map_gauss_pts_ptr);
}
 
} // namespace EshelbianPlasticity