EshelbianContact.cpp

Go to the documentation of this file.

/**
 * @file EshelbianContact.cpp
 * @brief
 * @date 2023-05-13
 *
 * @copyright Copyright (c) 2023
 *
 */
 
extern "C" {
void tricircumcenter3d_tp(double a[3], double b[3], double c[3],
                          double circumcenter[3], double *xi, double *eta);
}
 
template <>
typename MoFEM::OpBaseImpl<A, ContactOps::BoundaryEleOp>::MatSetValuesHook
    MoFEM::OpBaseImpl<A, ContactOps::BoundaryEleOp>::matSetValuesHook =
        [](ForcesAndSourcesCore::UserDataOperator *op_ptr,
           const EntitiesFieldData::EntData &row_data,
           const EntitiesFieldData::EntData &col_data, MatrixDouble &m) {
          return MatSetValues<AssemblyTypeSelector<A>>(
              op_ptr->getKSPA(), row_data, col_data, m, ADD_VALUES);
        };
 
namespace EshelbianPlasticity {
 
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();
 
  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.));
  }
};
 
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);
};
 
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 };
 
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);
 
    FTensor::Tensor1<double, 3> t_normal;
    t_normal(i) = t_master_normal(i) - t_slave_normal(i);
    t_normal.normalize();
 
    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;
 
    auto f_min_gap = [zeta](auto d, auto s) {
      return 0.5 * (d + s - std::sqrt((d - s) * (d - s) + zeta));
    };
    auto f_diff_min_gap = [zeta](auto d, auto s) {
      return 0.5 * (1 - (d - s) / std::sqrt((d - s) * (d - s) + zeta));
    };
 
    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;
    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;
};
 
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_NULL, "", "-cn",
                                          &ContactOps::cn_contact, PETSC_NULL),
                    "get cn failed");
  ContactOps::alpha_contact_const = 1. / ContactOps::cn_contact;
  CHK_THROW_MESSAGE(
      PetscOptionsGetScalar(PETSC_NULL, "", "-alpha_contact_const",
                            &ContactOps::alpha_contact_const, PETSC_NULL),
      "get alpha contact failed");
  CHK_THROW_MESSAGE(
      PetscOptionsGetScalar(PETSC_NULL, "", "-alpha_contact_quadratic",
                            &ContactOps::alpha_contact_quadratic, PETSC_NULL),
      "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;
}
 
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) {
    SETERRQ2(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
             "Wrong number of integration pts %d != %d",
             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
  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 (face_data_ptr) {
        return true;
      }
      return false;
    };
 
    auto tn = t_traction(i) * t_grad_sdf_v(i);
    auto c = ContactOps::constrain(t_sdf_v, tn);
    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 {
 
      auto inv_cn = ContactOps::alpha_contact_const;
      
      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);
    }
 
    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);
}
 
OpConstrainBoundaryHDivRhs::OpConstrainBoundaryHDivRhs(
    const std::string row_field_name,
    boost::shared_ptr<ContactOps::CommonData> common_data_ptr,
    boost::shared_ptr<ContactTree> contact_tree_ptr)
    : ContactOps::AssemblyBoundaryEleOp(row_field_name, row_field_name,
                                        ContactOps::BoundaryEleOp::OPROW),
      commonDataPtr(common_data_ptr), contactTreePtr(contact_tree_ptr) {}
 
MoFEMErrorCode
OpConstrainBoundaryHDivRhs::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) {
    SETERRQ2(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
             "Wrong number of integration pts %d != %d",
             commonDataPtr->contactDisp.size2(), nb_gauss_pts);
  }
#endif // !NDEBUG
 
  auto &nf = locF;
  locF.clear();
 
  auto t_w = getFTensor0IntegrationWeight();
  auto t_disp = getFTensor1FromMat<3>(commonDataPtr->contactDisp);
  auto t_traction = getFTensor1FromMat<3>(commonDataPtr->contactTraction);
  auto t_coords = getFTensor1CoordsAtGaussPts();
  auto t_material_normal = getFTensor1NormalsAtGaussPts();
 
  auto next = [&]() {
    ++t_w;
    ++t_disp;
    ++t_traction;
    ++t_coords;
    ++t_material_normal;
  };
 
  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() / 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 != 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);
}
 
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 (face_data_ptr) {
          return true;
        }
        return false;
      };
 
      FTensor::Tensor2<double, 3, 3> t_res_dU;
 
      auto tn = t_traction(i) * t_grad_sdf_v(i);
      auto c = constrain(t_sdf_v, tn);
      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 {
 
        auto inv_cn = ContactOps::alpha_contact_const;
 
        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));
      }
 
      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);
}
 
OpConstrainBoundaryL2Lhs_dP::OpConstrainBoundaryL2Lhs_dP(
    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_dP::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 = getGaussPts().size2();
 
    auto t_w = getFTensor0IntegrationWeight();
    auto t_disp = getFTensor1FromMat<3>(commonDataPtr->contactDisp);
    auto t_traction = getFTensor1FromMat<3>(commonDataPtr->contactTraction);
    auto t_coords = getFTensor1CoordsAtGaussPts();
    auto t_material_normal = 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(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(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 (face_data_ptr) {
          return true;
        }
        return false;
      };
 
      FTensor::Tensor2<double, 3, 3> t_res_dP;
 
      auto tn = t_traction(i) * t_grad_sdf_v(i);
      auto c = ContactOps::constrain(t_sdf_v, tn);
      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 {
 
        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);
      }
 
      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);
}
 
OpConstrainBoundaryHDivLhs_dU::OpConstrainBoundaryHDivLhs_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)
    : ContactOps::AssemblyBoundaryEleOp(row_field_name, col_field_name,
                                        ContactOps::BoundaryEleOp::OPROWCOL),
      commonDataPtr(common_data_ptr), contactTreePtr(contact_tree_ptr) {
  sYmm = false;
}
 
MoFEMErrorCode
OpConstrainBoundaryHDivLhs_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) {
 
    const size_t nb_gauss_pts = getGaussPts().size2();
 
    auto t_w = getFTensor0IntegrationWeight();
    auto t_disp = getFTensor1FromMat<3>(commonDataPtr->contactDisp);
    auto t_traction = getFTensor1FromMat<3>(commonDataPtr->contactTraction);
    auto t_coords = getFTensor1CoordsAtGaussPts();
    auto t_material_normal = 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(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();
    }
  }
 
  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);
 
  const auto nb_nodes = (refElementsMap.at(MBTRI)->hoNodes) ? 6 : 3;
  const auto nb_bases = NBVOLUMETET_L2(maxOrder);
 
  // 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);
}
 
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);
}
 
OpTreeSearch::OpTreeSearch(
    boost::shared_ptr<ContactTree> contact_tree_ptr,
    boost::shared_ptr<ContactOps::CommonData> common_data_ptr,
    boost::shared_ptr<MatrixDouble> u_h1_ptr, Range r,
 
    moab::Interface *post_proc_mesh_ptr,
    std::vector<EntityHandle> *map_gauss_pts_ptr
 
    )
    : UOP(NOSPACE, UOP::OPSPACE), contactTreePtr(contact_tree_ptr),
      commonDataPtr(common_data_ptr), uH1Ptr(u_h1_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>(commonDataPtr->contactTraction);
 
  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 / 3, 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) * ele_radius;
 
        constexpr double eps = 1e-3;
        auto [faces_out, faces_dist] = get_faces_out(
            &t_ray_point(0), &t_unit_ray(0), 2 * 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 (
 
                body_id != get_body_id(face) &&
                (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);
}
 
} // namespace EshelbianPlasticity