ContactSearchKdTree.hpp

Go to the documentation of this file.

/* This file is part of MoFEM.
 * MoFEM is free software: you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or (at your
 * option) any later version.
 *
 * MoFEM is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
 * License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with MoFEM. If not, see <http://www.gnu.org/licenses/>. */
 
#ifndef __CONTACT_SEARCH_KD_TREE__
#define __CONTACT_SEARCH_KD_TREE__
 
struct ContactSearchKdTree {
 
  MoFEM::Interface &mField;
  ContactSearchKdTree(MoFEM::Interface &m_field)
      : mField(m_field), kdTree(&m_field.get_moab()) {}
 
  EntityHandle kdTreeRootMeshset;
  AdaptiveKDTree kdTree;
  Range nullTriRange;
  // kd-tree from
  MoFEMErrorCode buildTree(Range &range_surf_master) {
    MoFEMFunctionBegin;
    CHKERR kdTree.build_tree(range_surf_master, &kdTreeRootMeshset);
    MoFEMFunctionReturn(0);
  }
 
  // Definitition of multi-index containers consisting of prisms between master
  // and slave trias and their corresponding triangles integration_tris
  struct ContactCommonData {
    EntityHandle pRism;
    Range commonIntegratedTriangle;
    ContactCommonData(EntityHandle _prism, Range _common_integrated_triangle)
        : pRism(_prism), commonIntegratedTriangle(_common_integrated_triangle) {
    }
  };
 
  struct Prism_tag {};
 
  typedef multi_index_container<
      boost::shared_ptr<ContactCommonData>,
      indexed_by<
          ordered_unique<tag<Prism_tag>, member<ContactCommonData, EntityHandle,
                                                &ContactCommonData::pRism>>>>
      ContactCommonData_multiIndex;
 
  // Rotation matrix to transform the 3D triangle to 2D triangle (because
  // clipper library only works in 2D)
  MoFEMErrorCode rotationMatrix(MatrixDouble &m_rot, VectorDouble &v_coords) {
    MoFEMFunctionBegin;
 
    FTensor::Index<'i', 3> i;
    FTensor::Index<'j', 3> j;
    FTensor::Index<'k', 3> k;
 
    VectorDouble3 v_s1(3), v_s2(3), v_n(3);
    double diffN[6];
    CHKERR ShapeDiffMBTRI(diffN);
 
    CHKERR ShapeFaceBaseMBTRI(diffN, &*v_coords.data().begin(),
                              &*v_n.data().begin(), &*v_s1.data().begin(),
                              &*v_s2.data().begin());
 
    FTensor::Tensor1<double *, 3> t_s1(&v_s1[0], &v_s1[1], &v_s1[2], 3);
    FTensor::Tensor1<double *, 3> t_s2(&v_s2[0], &v_s2[1], &v_s2[2], 3);
    FTensor::Tensor1<double *, 3> t_n(&v_n[0], &v_n[1], &v_n[2], 3);
 
    double s1_mag = sqrt(t_s1(i) * t_s1(i));
    double n_mag = sqrt(t_n(i) * t_n(i));
    v_s1 /= s1_mag;
    v_n /= n_mag;
 
    t_s2(i) = FTensor::levi_civita(i, j, k) * t_n(j) * t_s1(k);
 
    for (int ii = 0; ii != 3; ++ii) {
      m_rot(0, ii) = t_s1(ii);
      m_rot(1, ii) = t_s2(ii);
      m_rot(2, ii) = t_n(ii);
    }
 
    MoFEMFunctionReturn(0);
  }
 
  MoFEMErrorCode getNormalVector(VectorDouble &n_vec, VectorDouble &v_coords) {
    MoFEMFunctionBegin;
 
    FTensor::Index<'i', 3> i;
    double s1[3], s2[3];
    double diffN[6];
    CHKERR ShapeDiffMBTRI(diffN);
 
    CHKERR ShapeFaceBaseMBTRI(diffN, &*v_coords.data().begin(),
                              &*n_vec.data().begin(), s1, s2);
 
    FTensor::Tensor1<double *, 3> t_n(&n_vec[0], &n_vec[1], &n_vec[2], 3);
    double n_mag = sqrt(t_n(i) * t_n(i));
    n_vec /= n_mag;
 
    MoFEMFunctionReturn(0);
  }
 
  // Searching for contacting triangles with the help of kdtree from MOAB
  // fill the multi-index container (contact_commondata_multi_index) conissting
  // of prism with corresponding integration triangles fill
  // range_slave_master_prisms consisting of all the prisms (to be used for
  // looping over all these finite elements)
  PetscErrorCode
  contactSearchAlgorithm(Range &range_surf_master, Range &range_surf_slave,
                         boost::shared_ptr<ContactCommonData_multiIndex>
                             contact_commondata_multi_index,
                         Range &range_slave_master_prisms) {
    MoFEMFunctionBegin;
 
    FTensor::Index<'i', 3> i;
    FTensor::Index<'j', 3> j;
    auto get_tensor_vec = [](VectorDouble &v) {
      return FTensor::Tensor1<FTensor::PackPtr<double *, 3>, 3>(&v(0), &v(1),
                                                                &v(2));
    };
 
    // clipper library way to define one master and one slave tri and the out
    // put will store in solution
    Paths path_slave(1), path_master(1), solution;
 
    // it is used to convert double to integer to be used in clipper library (as
    // the library only works for integer) so we will convert all dimensions
    // from double to integer and after finding common polygon will convert back
    // to double
    double roundfact = 1e6;
 
    // Ranges of polygons in each intersections and created tris
    Range tris_in_polygon;
 
    VectorDouble v_slave_tri_cent;
    v_slave_tri_cent.resize(3, false);
 
    VectorDouble v_coords_slave_new;
    v_coords_slave_new.resize(9, false);
 
    MatrixDouble m_rot;
    m_rot.resize(3, 3, false);
 
    VectorDouble n_vec;
    n_vec.resize(3, false);
 
    VectorDouble v_coords_slave;
    v_coords_slave.resize(9, false);
 
    // loop over slave triagles (to create prisms)
    int slave_num = 0;
    for (Range::iterator tri_it_slave = range_surf_slave.begin();
         tri_it_slave != range_surf_slave.end(); ++tri_it_slave) {
      ++slave_num;
 
      int num_nodes_slave;
      const EntityHandle *conn_slave = NULL;
      CHKERR mField.get_moab().get_connectivity(*tri_it_slave, conn_slave,
                                                num_nodes_slave);
 
      // insert prisms between master and slave triangles, who have common
      // polygons last three nodes belong to the slave tri and the other three
      // belong to the master tris
      EntityHandle slave_master_1prism;
      EntityHandle prism_nodes[6];
      // slave is the top tri so add it to the top tri of prism [ii+3]
      for (int ii = 0; ii != 3; ++ii) {
        prism_nodes[ii + 3] = conn_slave[ii];
      }
 
      CHKERR mField.get_moab().get_coords(conn_slave, 3,
                                          &*v_coords_slave.data().begin());
 
      auto t_coords_slave = get_tensor_vec(v_coords_slave);
      // rotation matrix (3x3) which will be used to transform the tri to z
      // plane (this is the same for both slave and master tris) so
      // calculate it only once
      CHKERR rotationMatrix(m_rot, v_coords_slave);
      FTensor::Tensor2<double *, 3, 3> t_m_rot{
          &m_rot(0, 0), &m_rot(0, 1), &m_rot(0, 2), &m_rot(1, 0), &m_rot(1, 1),
          &m_rot(1, 2), &m_rot(2, 0), &m_rot(2, 1), &m_rot(2, 2)};
 
      CHKERR getNormalVector(n_vec, v_coords_slave);
      auto t_slave_normal = get_tensor_vec(n_vec);
 
      // as we need to transfer the coordinate to z=0
      double z_shift;
 
      auto t_coords_slave_new = get_tensor_vec(v_coords_slave_new);
 
      auto t_slave_tri_cent = get_tensor_vec(v_slave_tri_cent);
      t_slave_tri_cent(i) = 0;
 
      // transfer the slave tri coordinates (n=3) one by one
      for (int ii = 0; ii != 3; ++ii) {
        t_coords_slave_new(i) = t_m_rot(i, j) * t_coords_slave(j);
        t_slave_tri_cent(i) += t_coords_slave(i) / 3;
        if (ii == 0)
          z_shift = v_coords_slave_new[2];
 
        t_coords_slave_new(2) = 0;
        ++t_coords_slave_new;
        ++t_coords_slave;
      }
 
      // multiply with roundfact to convert to integer
      v_coords_slave_new = roundfact * v_coords_slave_new;
 
      // clear clipper vector for slave tri and fill it to be used in clipper
      path_slave[0].clear();
      path_slave[0] << IntPoint(v_coords_slave_new[0], v_coords_slave_new[1])
                    << IntPoint(v_coords_slave_new[3], v_coords_slave_new[4])
                    << IntPoint(v_coords_slave_new[6], v_coords_slave_new[7]);
 
      // find centroid of triangle based on original coordinates (this is
      // created to search the nearest master tri) it is to be used in kd-tree
 
      // this range will be filled in recursive manner, starting from the
      // nearset one and unitill there is no more intersecting master tris
      Range range_closest_master_tris;
      range_closest_master_tris.clear();
      double closest_point[3];
      EntityHandle closest_master_tri;
      CHKERR kdTree.closest_triangle(kdTreeRootMeshset,
                                     &*v_slave_tri_cent.data().begin(),
                                     closest_point, closest_master_tri);
 
      range_closest_master_tris.insert(closest_master_tri);
 
      // this while loop will continue until there is no more master tri for
      // slave tris
      bool flag_first_master =
          true; // for the first master, we will not find the adjacenency
      int master_num = 0;
      bool flag_end_of_search = false;
      bool flag_temp = true;
      while (flag_end_of_search == false) {
 
        Range range_master_tris_on_surf_new;
        if (flag_first_master == true) { // first master
          range_master_tris_on_surf_new = range_closest_master_tris;
          flag_first_master = false;
        } else { // the rest of master determine from adjacencies
          flag_temp = false;
          Range range_conn;
          CHKERR mField.get_moab().get_connectivity(range_closest_master_tris,
                                                    range_conn);
 
          Range range_adj_entities;
          // in the get_adjacencies, if moab::Interface::INTERSECT it will give
          // only the tri, which is interesect of the adjacency of all nodes
          // moab::Interface::UNION will give all the tri, even the one which
          // are not on the surface, so then need to intersect with the master
          // to get what we want
          CHKERR mField.get_moab().get_adjacencies(
              range_conn, 2, false, range_adj_entities, moab::Interface::UNION);
 
          // triangle on the surface, i.e. removing the tri inside the volume
          Range range_master_tris_on_surf =
              intersect(range_surf_master, range_adj_entities);
 
          // removing the tri from the range_tri_on_surf, which already exists
          // in range_closest_master_tris
          range_master_tris_on_surf_new =
              subtract(range_master_tris_on_surf, range_closest_master_tris);
        }
 
        // all used in master loop
        VectorDouble v_coords_master;
        v_coords_master.resize(9, false);
 
        VectorDouble n_vec_master;
        n_vec_master.resize(3, false);
 
        // fill the master clipper vector
        VectorDouble v_coords_master_new;
        v_coords_master_new.resize(9, false);
 
        double coord[3];
        // looping over all master range_master_tris_on_surf_new and discart
        // those elements, which are not in contact with master (will do it with
        // clipper library)
        int num_intersections = 0;
        for (Range::iterator tri_it_master =
                 range_master_tris_on_surf_new.begin();
             tri_it_master != range_master_tris_on_surf_new.end();
             ++tri_it_master) {
          const EntityHandle *conn_master = NULL;
          int num_nodes_master = 0;
          CHKERR mField.get_moab().get_connectivity(*tri_it_master, conn_master,
                                                    num_nodes_master);
 
          CHKERR mField.get_moab().get_coords(conn_master, 3,
                                              &*v_coords_master.data().begin());
 
          auto t_coords_master = get_tensor_vec(v_coords_master);
 
          CHKERR getNormalVector(n_vec_master, v_coords_master);
 
          auto t_master_normal = get_tensor_vec(n_vec_master);
 
          // 0.035 angle of 88 deg
          if (fabs(t_slave_normal(i) * t_master_normal(i)) < 0.035)
            continue;
 
          auto t_coords_master_new = get_tensor_vec(v_coords_master_new);
 
          // rotation matrix for this master element is the same as for the
          // above slave element
          for (int ii = 0; ii != 3; ++ii) {
            t_coords_master_new(i) = t_m_rot(i, j) * t_coords_master(j);
            t_coords_master_new(2) = 0;
            ++t_coords_master_new;
            ++t_coords_master;
          }
 
          v_coords_master_new = roundfact * v_coords_master_new;
          path_master[0].clear();
          path_master[0]
              << IntPoint(v_coords_master_new[0], v_coords_master_new[1])
              << IntPoint(v_coords_master_new[3], v_coords_master_new[4])
              << IntPoint(v_coords_master_new[6], v_coords_master_new[7]);
 
          // perform intersection in clipper
          Clipper c;
          c.AddPaths(path_master, ptSubject, true);
          c.AddPaths(path_slave, ptClip, true);
          c.Execute(ctIntersection, solution, pftNonZero, pftNonZero);
          
          if (solution.size() != 1 && flag_temp) {
            double ray_dir[3];
            CHKERR Tools::getTriNormal(&v_coords_slave(0), ray_dir);
            double norm = cblas_dnrm2(3, ray_dir, 1);
            cblas_dscal(3, 1. / norm, ray_dir, 1);
            const double tol = 1e-6;
            std::vector<double> distance;
            std::vector<EntityHandle> triangles_out;
            CHKERR kdTree.ray_intersect_triangles(kdTreeRootMeshset, tol, ray_dir,
                                                  &*v_slave_tri_cent.data().begin(),
                                                  triangles_out, distance);
 
            if (!triangles_out.empty()) {
              range_closest_master_tris.clear();
              range_closest_master_tris.insert(triangles_out.front());
              flag_first_master = true;
              num_intersections = -1;
              break;
            }
          }
 
          if (solution.size() == 1) { // if it intersect
            range_closest_master_tris.insert(
                *tri_it_master); // update range_closest_master_tris
            num_intersections += 1;
 
            int n_vertices_polygon = solution[0].size();
            // the first three nodes for the prism are the master tri (insert
            // prisms only if there is intersection between slave and master
            // tris)
            for (int ii = 0; ii != 3; ++ii) {
              prism_nodes[ii] = conn_master[ii];
            }
 
            // creating prism between master and slave tris
            CHKERR mField.get_moab().create_element(MBPRISM, prism_nodes, 6,
                                                    slave_master_1prism);
 
            range_slave_master_prisms.insert(slave_master_1prism);
 
            // coordinates of intersection polygon between master and slave
            // here we have only coordinates of polygon, so we will need to
            // construct vertices and polygon from it
            VectorDouble v_coord_polygon;
            v_coord_polygon.resize(3 * n_vertices_polygon);
 
            // perform transfermation (tansfer all coordinates of polygon)
            // create moab vertices with given coordinates (from clippper) and
            // then create polygon from these vertices
            VectorDouble v_coord_polygon_new;
            v_coord_polygon_new.resize(3 * n_vertices_polygon);
 
            auto t_coord_polygon_new = get_tensor_vec(v_coord_polygon_new);
            auto t_coord_polygon = get_tensor_vec(v_coord_polygon);
 
            std::vector<EntityHandle> conn_polygon(n_vertices_polygon);
            for (int ii = 0; ii != n_vertices_polygon; ++ii) {
              t_coord_polygon(0) = solution[0][ii].X / roundfact;
              t_coord_polygon(1) = solution[0][ii].Y / roundfact;
              t_coord_polygon(2) += z_shift;
 
              t_coord_polygon_new(i) = t_m_rot(j, i) * t_coord_polygon(j);
 
              coord[0] = t_coord_polygon_new(0);
              coord[1] = t_coord_polygon_new(1);
              coord[2] = t_coord_polygon_new(2);
              CHKERR mField.get_moab().create_vertex(coord, conn_polygon[ii]);
 
              ++t_coord_polygon_new;
              ++t_coord_polygon;
            }
 
            EntityHandle polygon;
            CHKERR mField.get_moab().create_element(
                MBPOLYGON, &conn_polygon[0], n_vertices_polygon, polygon);
 
            // triangulate the polygon (if it only have 3 vertices, so it is
            // triangle, so don't need to triangulate)
            tris_in_polygon.clear();
            if (n_vertices_polygon == 3) {
              // tris_in_polygon.insert(polygon);
              EntityHandle new_tri_conn[3];
              new_tri_conn[0] = conn_polygon[0];
              new_tri_conn[1] = conn_polygon[1];
              new_tri_conn[2] = conn_polygon[2];
              EntityHandle new_tri;
              CHKERR mField.get_moab().create_element(MBTRI, new_tri_conn, 3,
                                                      new_tri);
 
              tris_in_polygon.insert(new_tri);
            } else {
              EntityHandle new_tri_conn[3];
              new_tri_conn[0] = conn_polygon[0];
              for (int ii = 0; ii != n_vertices_polygon - 2; ++ii) {
                new_tri_conn[1] = conn_polygon[ii + 1];
                new_tri_conn[2] = conn_polygon[ii + 2];
                // create tri in polygon
                EntityHandle new_tri;
                CHKERR mField.get_moab().create_element(MBTRI, new_tri_conn, 3,
                                                        new_tri);
 
                tris_in_polygon.insert(new_tri);
              }
            }
 
            // fill the multi-index container with pair (prism between slave and
            // master, triangles in the common polygon)
            contact_commondata_multi_index->insert(
                boost::shared_ptr<ContactCommonData>(new ContactCommonData(
                    slave_master_1prism, tris_in_polygon)));
          }
        } // master loop for new layer arrond the previous layer
 
        if (num_intersections == 0)
          flag_end_of_search = true; // this will stop further searching for the
                                     // current slave and will go to next one
      }                              // while loop
    }                                // slave loop
 
    MoFEMFunctionReturn(0);
  }
};
#endif