RigidBodies.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/>. */
 
#pragma once
 
enum RigidBodyTypes {
  PLANE,
  SPHERE,
  CYLINDER,
  CONE,
  TORUS,
  ROLLER,
  STL,
  NURBS,
  LASTBODY
};
 
struct RigidBodyData {
  const int iD;
  Index<'i', 3> i;
  Index<'j', 3> j;
  Index<'k', 3> k;
  VectorDouble originCoords;
  VectorDouble rollerDisp;
  VectorDouble BodyDispScaled;
  double gAp;
  Tensor1<double, 3> tNormal;
  Tensor1<double, 3> dGap;
  Tensor1<double, 3> pointCoords;
  Tensor1<double, 3> closestPoint;
  Tensor1<double, 3> defaultOrientation;
  Tensor1<double, 3> oRientation;
  Tensor2<double, 3, 3> rotationMat;
  Tensor2<double, 3, 3> diffNormal;
 
  string positionDataParamName;
 
  int bodyType;
  array<double, 9> dataForTags;
 
  RigidBodyData(VectorDouble c_coords, VectorDouble roller_disp, int id)
      : originCoords(c_coords), rollerDisp(roller_disp), iD(id),
        defaultOrientation(0., 0., 1.), oRientation(0., 0., 1.) {
    BodyDispScaled = rollerDisp;
    BodyDispScaled.clear();
    rotationMat(i, j) = kronecker_delta(i, j);
    std::fill(dataForTags.begin(), dataForTags.end(), 0);
    bodyType = PLANE;
  }
 
  RigidBodyData() = delete;
  virtual ~RigidBodyData() {}
 
  using TPack3 = PackPtr<double *, 3>; // t_coords
  using TPack1 = PackPtr<double *, 1>; // t_disp
 
  virtual Tensor1<double, 3> getNormal(Tensor1<TPack3, 3> &t_coords,
                                       Tensor1<TPack1, 3> &t_disp) = 0;
  virtual Tensor1<double, 3> getNormal(Tensor1<TPack3, 3> &t_coords,
                                       Tensor1<double, 3> &t_disp) = 0;
  virtual Tensor1<double, 3> getNormal(Tensor1<double, 3> &t_coords,
                                       Tensor1<double, 3> &t_disp) = 0;
 
  virtual Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
                                              Tensor1<TPack1, 3> &t_disp,
                                              Tensor1<TPack1, 3> &t_normal) = 0;
 
  virtual double getGap(Tensor1<TPack3, 3> &t_coords) = 0;
  virtual Tensor1<double, 3> getdGap(Tensor1<TPack3, 3> &t_coords,
                                     Tensor1<TPack1, 3> &t_normal) = 0;
 
  virtual MoFEMErrorCode getBodyOptions() = 0;
 
  inline MoFEMErrorCode computeRotationMatrix() {
    // https://math.stackexchange.com/questions/180418
    MoFEMFunctionBeginHot;
    oRientation.normalize();
    Tensor1<double, 3> v;
    v(i) = levi_civita(i, j, k) * defaultOrientation(j) * oRientation(k);
    const double s = v.l2();
    if (abs(s) < 1e-16)
      MoFEMFunctionReturnHot(0);
    const double c = defaultOrientation(i) * oRientation(i);
    const double coef = 1. / (1. + c);
    Tensor2<double, 3, 3> t_v;
    t_v(i, j) = levi_civita(i, j, k) * v(k);
    rotationMat(i, j) += t_v(i, j) + (coef * t_v(i, k)) * t_v(k, j);
    // Tensor1<double, 3> test;
    // test(i) = rotationMat(j, i) * defaultOrientation(j);
 
    MoFEMFunctionReturnHot(0);
  };
 
  inline Tensor1<double, 3> getBodyOffset() {
    VectorDouble pos = originCoords + BodyDispScaled;
    return Tensor1<double, 3>(pos(0), pos(1), pos(2));
  };
 
  template <typename T1, typename T2>
  inline Tensor1<double, 3> &getPointCoords(Tensor1<T1, 3> &t_coords,
                                            Tensor1<T2, 3> &t_disp) {
    auto t_off = getBodyOffset();
    pointCoords(i) = t_coords(i) - t_off(i) + t_disp(i);
    return pointCoords;
  };
 
  MoFEMErrorCode saveBasicDataOnTag(moab::Interface &moab_debug,
                                    EntityHandle &vertex) {
    MoFEMFunctionBeginHot;
 
    Tag tag0;
    int def_int = 0;
    std::array<double, 9> def_real;
    std::fill(def_real.begin(), def_real.end(), 0);
 
    CHKERR getRollerDataForTag();
 
    CHKERR moab_debug.tag_get_handle("_ROLLER_ID", 1, MB_TYPE_INTEGER, tag0,
                                     MB_TAG_CREAT | MB_TAG_SPARSE, &def_int);
    CHKERR moab_debug.tag_set_data(tag0, &vertex, 1, &this->iD);
 
    CHKERR moab_debug.tag_get_handle("_BODY_TYPE", 1, MB_TYPE_INTEGER, tag0,
                                     MB_TAG_CREAT | MB_TAG_SPARSE, &def_int);
    CHKERR moab_debug.tag_set_data(tag0, &vertex, 1, &this->bodyType);
 
    CHKERR moab_debug.tag_get_handle("_ORIENTATION", 3, MB_TYPE_DOUBLE, tag0,
                                     MB_TAG_CREAT | MB_TAG_SPARSE, &def_real);
    CHKERR moab_debug.tag_set_data(tag0, &vertex, 1, &this->oRientation(0));
 
    CHKERR moab_debug.tag_get_handle("_ROLLER_DATA", 9, MB_TYPE_DOUBLE, tag0,
                                     MB_TAG_CREAT | MB_TAG_SPARSE, &def_real);
    CHKERR moab_debug.tag_set_data(tag0, &vertex, 1, this->dataForTags.data());
 
 
    // tags
    // int roller_id
    // int body_type
    // vec3 orientation
    // tensor9 roller_data
    //  0 radius1 = 5
    //  1 inner_radius = 1
    //  2 height = 1
    //  3 angle = 45
    //  4 angle_a = 45
    //  5 angle_b = 45
    //
 
    MoFEMFunctionReturnHot(0);
  }
  virtual MoFEMErrorCode getRollerDataForTag() = 0;
};
 
struct PlaneRigidBody : public RigidBodyData {
  PetscBool fLip;
  PlaneRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
      : RigidBodyData(c_coords, roller_disp, id), fLip(PETSC_FALSE) {}
 
  MoFEMErrorCode getRollerDataForTag() {
    MoFEMFunctionBeginHot;
    bodyType = PLANE;
    MoFEMFunctionReturnHot(0);
  }
 
  Tensor1<double, 3> getNormal(Tensor1<TPack3, 3> &t_coords,
                               Tensor1<TPack1, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
  Tensor1<double, 3> getNormal(Tensor1<TPack3, 3> &t_coords,
                               Tensor1<double, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
  Tensor1<double, 3> getNormal(Tensor1<double, 3> &t_coords,
                               Tensor1<double, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
 
  Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
                                      Tensor1<TPack1, 3> &t_disp,
                                      Tensor1<TPack1, 3> &t_normal) {
    return getDiffNormalImpl(t_coords, t_disp, t_normal);
  }
 
  inline double getGap(Tensor1<TPack3, 3> &t_coords) {
    return getGapImpl(t_coords);
  }
 
  Tensor1<double, 3> getdGap(Tensor1<TPack3, 3> &t_coords,
                             Tensor1<TPack1, 3> &t_normal) {
    return getdGapImpl(t_coords, t_normal);
  }
 
  template <typename T1, typename T2>
  inline Tensor1<double, 3> &getNormalImpl(Tensor1<T1, 3> &t_coords,
                                           Tensor1<T2, 3> &t_disp) {
    auto t_d = getPointCoords(t_coords, t_disp);
    // FIXME:
    // tNormal(i) = rotationMat(j, i) * defaultOrientation(j);
    tNormal(i) = oRientation(i);
    if (fLip)
      tNormal(i) *= -1;
    // rotationMat(i, j)
    return tNormal;
  };
 
  template <typename T1> double getGapImpl(Tensor1<T1, 3> &t_coords) {
    // test(i) = rotationMat(j, i) * t_off(j);
    // t_d2(i) = t_coords(i) - rotationMat(j, i) * t_off(j);
    gAp = tNormal(i) * pointCoords(i);
    return gAp;
  };
 
  template <typename T1, typename T2, typename T3>
  inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
                                                 Tensor1<T2, 3> &t_disp,
                                                 Tensor1<T3, 3> &t_normal) {
    const Tensor2<double, 3, 3> t_diff_norm(0., 0., 0., 0., 0., 0., 0., 0., 0.);
    return t_diff_norm;
  };
 
  template <typename T1, typename T2>
  inline Tensor1<double, 3> getdGapImpl(Tensor1<T1, 3> &t_coords,
                                        Tensor1<T2, 3> &t_normal) {
    dGap(i) = tNormal(i);
    return dGap;
  };
 
  MoFEMErrorCode getBodyOptions() {
    MoFEMFunctionBegin;
    PetscBool rflg;
    PetscInt nb_dirs = 3;
    CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
    string param = "-direction" + to_string(iD);
    CHKERR PetscOptionsGetRealArray(NULL, NULL, param.c_str(), &oRientation(0),
                                    &nb_dirs, &rflg);
    param = "-flip" + to_string(iD);
    CHKERR PetscOptionsBool(param.c_str(), "flip the plane normal", "", fLip,
                            &fLip, PETSC_NULL);
    if (rflg)
      CHKERR computeRotationMatrix();
    ierr = PetscOptionsEnd();
    CHKERRG(ierr);
    MoFEMFunctionReturn(0);
  }
};
 
struct SphereRigidBody : public RigidBodyData {
  double rAdius;
  SphereRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
      : RigidBodyData(c_coords, roller_disp, id), rAdius(1) {}
 
  MoFEMErrorCode getRollerDataForTag() {
    MoFEMFunctionBeginHot;
    bodyType = SPHERE;
    dataForTags[0] = rAdius;
    MoFEMFunctionReturnHot(0);
  }
 
  Tensor1<double, 3> getNormal(Tensor1<TPack3, 3> &t_coords,
                               Tensor1<TPack1, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
  Tensor1<double, 3> getNormal(Tensor1<TPack3, 3> &t_coords,
                               Tensor1<double, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
  Tensor1<double, 3> getNormal(Tensor1<double, 3> &t_coords,
                               Tensor1<double, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
 
  Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
                                      Tensor1<TPack1, 3> &t_disp,
                                      Tensor1<TPack1, 3> &t_normal) {
    return getDiffNormalImpl(t_coords, t_disp, t_normal);
  }
 
  inline double getGap(Tensor1<TPack3, 3> &t_coords) {
    return getGapImpl(t_coords);
  }
 
  Tensor1<double, 3> getdGap(Tensor1<TPack3, 3> &t_coords,
                             Tensor1<TPack1, 3> &t_normal) {
    return getdGapImpl(t_coords, t_normal);
  }
 
  template <typename T1, typename T2>
  inline Tensor1<double, 3> &getNormalImpl(Tensor1<T1, 3> &t_coords,
                                           Tensor1<T2, 3> &t_disp) {
    auto t_d = getPointCoords(t_coords, t_disp);
    tNormal(i) = t_d(i);
    tNormal.normalize();
 
    return tNormal;
  };
 
  template <typename T1, typename T2, typename T3>
  inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
                                                 Tensor1<T2, 3> &t_disp,
                                                 Tensor1<T3, 3> &t_normal) {
 
    auto t_d = getPointCoords(t_coords, t_disp);
 
    const double norm = t_d.l2();
    const double norm3 = norm * norm * norm;
    diffNormal(i, j) =
        kronecker_delta(i, j) * (1. / norm) - (1. / norm3) * t_d(i) * t_d(j);
 
    return diffNormal;
  };
 
  template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
 
    gAp = tNormal(i) * (pointCoords(i) - tNormal(i) * rAdius);
    return gAp;
  };
 
  template <typename T1, typename T2>
  inline Tensor1<double, 3> getdGapImpl(Tensor1<T1, 3> &t_coords,
                                        Tensor1<T2, 3> &t_normal) {
    // d gap / d normal
    dGap(i) =
        pointCoords(j) * diffNormal(i, j) +
        tNormal(j) * (kronecker_delta(i, j) - 2. * rAdius * diffNormal(i, j));
    return dGap;
  };
 
  MoFEMErrorCode getBodyOptions() {
    MoFEMFunctionBegin;
 
    CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
 
    string param = "-radius" + to_string(iD);
    CHKERR PetscOptionsScalar(param.c_str(), "set roller radius", "", rAdius,
                              &rAdius, PETSC_NULL);
    ierr = PetscOptionsEnd();
    CHKERRG(ierr);
 
    MoFEMFunctionReturn(0);
  }
};
 
struct CylinderRigidBody : public RigidBodyData {
  double rAdius;
  double hEight;
  CylinderRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
      : RigidBodyData(c_coords, roller_disp, id), rAdius(1), hEight(INFINITY) {}
 
  MoFEMErrorCode getRollerDataForTag() {
    MoFEMFunctionBeginHot;
    bodyType = CYLINDER;
    dataForTags[0] = rAdius;
    dataForTags[2] = hEight;
    MoFEMFunctionReturnHot(0);
  }
 
  Tensor1<double, 3> getNormal(Tensor1<TPack3, 3> &t_coords,
                               Tensor1<TPack1, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
  Tensor1<double, 3> getNormal(Tensor1<TPack3, 3> &t_coords,
                               Tensor1<double, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
  Tensor1<double, 3> getNormal(Tensor1<double, 3> &t_coords,
                               Tensor1<double, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
 
  Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
                                      Tensor1<TPack1, 3> &t_disp,
                                      Tensor1<TPack1, 3> &t_normal) {
    return getDiffNormalImpl(t_coords, t_disp, t_normal);
  }
 
  inline double getGap(Tensor1<TPack3, 3> &t_coords) {
    return getGapImpl(t_coords);
  }
 
  Tensor1<double, 3> getdGap(Tensor1<TPack3, 3> &t_coords,
                             Tensor1<TPack1, 3> &t_normal) {
    return getdGapImpl(t_coords, t_normal);
  }
 
  template <typename T1, typename T2>
  inline Tensor1<double, 3> &getNormalImpl(Tensor1<T1, 3> &t_coords,
                                           Tensor1<T2, 3> &t_disp) {
    auto t_d = getPointCoords(t_coords, t_disp);
    pointCoords(i) = rotationMat(i, j) * t_d(j);
    Tensor1<double, 3> c_center(0., 0., 0.);
    Tensor1<double, 3> q_minus_c(0., 0., 0.);
    Tensor1<double, 3> K(0., 0., 0.);
    auto Q = pointCoords;
    auto P_Q = pointCoords;
    auto Q_K = pointCoords;
 
    // https://www.geometrictools.com/Documentation/DistanceToCircle3.pdf
    const double half_height = hEight / 2.;
    const double rad2 = rAdius * rAdius;
    tNormal(i) = 0.;
 
    if (pointCoords(2) > half_height) {
      tNormal(2) = 1;
      if (pow(pointCoords(1), 2) + pow(pointCoords(0), 2) < rad2) {
        gAp = tNormal(i) * pointCoords(i) - half_height;
      } else {
        c_center(2) = half_height;
        Q(2) = c_center(2); // Q
        q_minus_c(i) = Q(i) - c_center(i);
        q_minus_c.normalize();
        K(i) = c_center(i) + rAdius * q_minus_c(i);
        P_Q(i) = pointCoords(i) - Q(i);
        Q_K(i) = Q(i) - K(i);
        gAp = sqrt(P_Q.l2() * P_Q.l2() + Q_K.l2() * Q_K.l2());
      }
    } else if (pointCoords(2) < -half_height) {
      tNormal(2) = -1;
      if (pow(pointCoords(1), 2) + pow(pointCoords(0), 2) < rad2) {
        gAp = tNormal(i) * pointCoords(i) - half_height;
      } else {
        c_center(2) = -half_height;
        Q(2) = c_center(2); // Q
        q_minus_c(i) = Q(i) - c_center(i);
        q_minus_c.normalize();
        K(i) = c_center(i) + rAdius * q_minus_c(i);
        P_Q(i) = pointCoords(i) - Q(i);
        Q_K(i) = Q(i) - K(i);
        gAp = sqrt(P_Q.l2() * P_Q.l2() + Q_K.l2() * Q_K.l2());
      }
    } else {
      pointCoords(2) = 0;
      tNormal(i) = pointCoords(i);
      tNormal.normalize();
      gAp = tNormal(i) * (pointCoords(i) - tNormal(i) * rAdius);
    }
 
    // auto length = [&](auto v) { return sqrt(v(0) * v(0) + v(1) * v(1)); };
 
    // auto sd_cylinder = [&](Tensor1<double, 3> p) {
    //   typedef Tensor1<double, 2> vec2;
    //   Index<'i', 2> i;
    //   vec2 d(length(p) - rAdius, abs(p(2)) - half_height);
    //   vec2 mx_d(max(d(0), 0.0), max(d(1), 0.0));
    //   return min(max(d(0), d(1)), 0.0) + length(mx_d);
    // };
 
    // const double eps = 1e-6;
    // auto cyl_distance = sd_cylinder(pointCoords);
    // gAp = cyl_distance;
 
    // auto get_d = [&](int a) {
    //   auto pt = pointCoords;
    //   pt(a) += eps;
    //   return (sd_cylinder(pt) - cyl_distance) / eps;
    // };
 
    // for (int dd = 0; dd != 3; ++dd)
    //   tNormal(dd) = get_d(dd);
 
    // for debugging
    // auto my_rand_mn = [](double M, double N) {
    //   return M + (rand() / (RAND_MAX / (N - M)));
    // };
    // auto t_off = getBodyOffset();
    // std::ofstream afile("cylinder_pts.csv", std::ios::ate);
    // if (afile.is_open()) {
    //   afile << "X,Y,Z\n";
    //   Tensor1<double, 3> p(0, 0, 0);
    //   for (int n = 0; n != 1e6; n++) {
    //     double x = my_rand_mn(-300, 300);
    //     double y = my_rand_mn(-300, 300);
    //     double z = my_rand_mn(-300, 300);
    //     Tensor1<double, 3> coords(x, y, z);
    //     if (sd_cylinder(coords) < 0) {
 
    //       afile << x + t_off(0) << "," << y + t_off(1) << "," << z + t_off(2)
    //       << "\n";
    //     }
    //   }
    //   afile.close();
    // }
    // cout << "kill process " << endl;
 
    tNormal.normalize();
 
    auto normal_copy = tNormal;
    tNormal(i) = rotationMat(j, i) * normal_copy(j);
 
    return tNormal;
  };
 
  template <typename T1, typename T2, typename T3>
  inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
                                                 Tensor1<T2, 3> &t_disp,
                                                 Tensor1<T3, 3> &t_normal) {
 
    Tensor2<double, 3, 3> diff_normal;
    Tensor1<double, 3> norm_diff;
    const double clean_gap = gAp;
    const double eps = 1e-8;
    for (int ii = 0; ii != 3; ++ii) {
      Tensor1<double, 3> pert_disp{t_disp(0), t_disp(1), t_disp(2)};
      pert_disp(ii) += eps;
      auto pert_normal = getNormal(t_coords, pert_disp);
      norm_diff(i) = (pert_normal(i) - t_normal(i)) / eps;
      dGap(ii) = (gAp - clean_gap) / eps;
 
      for (int jj = 0; jj != 3; ++jj) {
        diff_normal(jj, ii) = norm_diff(jj);
      }
    }
    return diff_normal;
  };
 
  template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
    // gAp = tNormal(i) * (pointCoords(i) - tNormal(i) * rAdius);
    return gAp;
  };
 
  template <typename T1, typename T2>
  inline Tensor1<double, 3> getdGapImpl(Tensor1<T1, 3> &t_coords,
                                        Tensor1<T2, 3> &t_normal) {
    // dGap(i) =
    //     pointCoords(j) * diffNormal(i, j) +
    //     tNormal(j) * (kronecker_delta(i, j) - 2. * rAdius * diffNormal(i,
    //     j));
    return dGap;
  };
 
  MoFEMErrorCode getBodyOptions() {
    MoFEMFunctionBegin;
    PetscBool rflg;
    PetscInt nb_dirs = 3;
    CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
    string param = "-direction" + to_string(iD);
    CHKERR PetscOptionsGetRealArray(NULL, NULL, param.c_str(), &oRientation(0),
                                    &nb_dirs, &rflg);
    param = "-radius" + to_string(iD);
    CHKERR PetscOptionsScalar(param.c_str(), "set roller radius", "", rAdius,
                              &rAdius, PETSC_NULL);
    param = "-height" + to_string(iD);
    CHKERR PetscOptionsScalar(param.c_str(), "set cylinder height", "", hEight,
                              &hEight, PETSC_NULL);
    if (rflg)
      CHKERR computeRotationMatrix();
    ierr = PetscOptionsEnd();
    CHKERRG(ierr);
    MoFEMFunctionReturn(0);
  }
};
 
struct TorusRigidBody : public RigidBodyData {
  // shared_ptr<ArbitrarySurfaceData> cP;
  double rAdius;
  double innerRadius;
  TorusRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
      : RigidBodyData(c_coords, roller_disp, id), rAdius(1), innerRadius(0.5) {
    // , cP(R, r, 1)
    // compute rotation matrix here
  }
 
  MoFEMErrorCode getRollerDataForTag() {
    MoFEMFunctionBeginHot;
    bodyType = TORUS;
    dataForTags[0] = rAdius;
    dataForTags[1] = innerRadius;
    MoFEMFunctionReturnHot(0);
  }
 
  Tensor1<double, 3> getNormal(Tensor1<TPack3, 3> &t_coords,
                               Tensor1<TPack1, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
  Tensor1<double, 3> getNormal(Tensor1<TPack3, 3> &t_coords,
                               Tensor1<double, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
  Tensor1<double, 3> getNormal(Tensor1<double, 3> &t_coords,
                               Tensor1<double, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
 
  Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
                                      Tensor1<TPack1, 3> &t_disp,
                                      Tensor1<TPack1, 3> &t_normal) {
    return getDiffNormalImpl(t_coords, t_disp, t_normal);
  }
 
  inline double getGap(Tensor1<TPack3, 3> &t_coords) {
    return getGapImpl(t_coords);
  }
  Tensor1<double, 3> getdGap(Tensor1<TPack3, 3> &t_coords,
                             Tensor1<TPack1, 3> &t_normal) {
    return getdGapImpl(t_coords, t_normal);
  }
 
  template <typename T1, typename T2>
  inline Tensor1<double, 3> &getNormalImpl(Tensor1<T1, 3> &t_coords,
                                           Tensor1<T2, 3> &t_disp) {
 
    auto t_d = getPointCoords(t_coords, t_disp);
    pointCoords(i) = rotationMat(i, j) * t_d(j);
    t_d(i) = pointCoords(i);
 
    // cP->tt = 1;
    // cP->rr = 1.;
    // cP->R1 = 0;
    // cP->PrintTorusDebug();
    // cP->PrintTorusAnimDebug();
 
    // auto closest_pt_toroid = cP->getClosestPointToToroid(t_d);
    // closestPoint(i) = closest_pt_toroid(i);
 
    // tNormal(j) = t_d(j) - closestPoint(j);
    // if (cP->isPointInsideTorus(t_d))
    //   tNormal(i) *= -1;
    // tNormal.normalize();
 
    // gAp = tNormal(i) * (t_d(i) - closestPoint(i));
 
    // https: // iquilezles.org/www/articles/distfunctions/distfunctions.htm
    const double x = t_d(0);
    const double y = t_d(1);
    const double z = t_d(2);
    const double RR = rAdius - innerRadius;
    const double prod = sqrt(pow(x, 2) + pow(y, 2));
    const double prod2 = sqrt(pow(-RR + prod, 2) + pow(z, 2));
    gAp = -innerRadius + prod2;
    tNormal(0) = (x * (-RR + prod)) / (prod * prod2);
    tNormal(1) = (y * (-RR + prod)) / (prod * prod2);
    tNormal(2) = z / prod2;
    tNormal.normalize();
 
    auto normal_copy = tNormal;
    tNormal(i) = rotationMat(j, i) * normal_copy(j);
 
    return tNormal;
  };
 
  template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
    return gAp;
  };
 
  template <typename T1, typename T2, typename T3>
  inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
                                                 Tensor1<T2, 3> &t_disp,
                                                 Tensor1<T3, 3> &t_normal) {
    Tensor2<double, 3, 3> diff_normal;
    Tensor1<double, 3> norm_diff;
    const double clean_gap = gAp;
    const double eps = 1e-8;
    for (int ii = 0; ii != 3; ++ii) {
      Tensor1<double, 3> pert_disp{t_disp(0), t_disp(1), t_disp(2)};
      pert_disp(ii) += eps;
      auto pert_normal = getNormal(t_coords, pert_disp);
      norm_diff(i) = (pert_normal(i) - t_normal(i)) / eps;
      dGap(ii) = (gAp - clean_gap) / eps;
 
      for (int jj = 0; jj != 3; ++jj) {
        diff_normal(jj, ii) = norm_diff(jj);
      }
    }
    return diff_normal;
  };
 
  template <typename T1, typename T2>
  inline Tensor1<double, 3> getdGapImpl(Tensor1<T1, 3> &t_coords,
                                        Tensor1<T2, 3> &t_normal) {
    return dGap;
  };
 
  virtual MoFEMErrorCode getBodyOptions() {
    MoFEMFunctionBegin;
    PetscBool rflg;
    int nb_dirs = 3;
    CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
    string param = "-inner_radius" + to_string(iD);
    CHKERR PetscOptionsScalar(param.c_str(), "set torus small radius", "", innerRadius,
                              &innerRadius, PETSC_NULL);
    param = "-radius" + to_string(iD);
    CHKERR PetscOptionsScalar(param.c_str(), "set torus large Radius", "", rAdius,
                              &rAdius, PETSC_NULL);
    param = "-direction" + to_string(iD);
    CHKERR PetscOptionsGetRealArray(NULL, NULL, param.c_str(), &oRientation(0),
                                    &nb_dirs, &rflg);
    ierr = PetscOptionsEnd();
    CHKERRG(ierr);
 
    if (rflg)
      CHKERR computeRotationMatrix();
 
    // cP = make_shared<ArbitrarySurfaceData>(R, r);
 
    MoFEMFunctionReturn(0);
  }
};
 
struct RollerRigidBody : public RigidBodyData {
  // shared_ptr<ArbitrarySurfaceData> cP;
  double angleA;
  double angleB;
  double rAdius;
  double fIllet;
  double torusRadius;
  double hEight;
 
  double coneRadiusA;
  double coneTopRadiusA;
  double coneOffsetA;
 
  double coneRadiusB;
  double coneTopRadiusB;
  double coneOffsetB;
 
  RollerRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
      : RigidBodyData(c_coords, roller_disp, id) {}
 
  MoFEMErrorCode getRollerDataForTag() {
    MoFEMFunctionBeginHot;
    bodyType = ROLLER;
    dataForTags[0] = rAdius;
    dataForTags[1] = fIllet;
    dataForTags[2] = hEight;
    dataForTags[4] = angleA;
    dataForTags[5] = angleB;
 
    MoFEMFunctionReturnHot(0);
  }
 
  Tensor1<double, 3> getNormal(Tensor1<TPack3, 3> &t_coords,
                               Tensor1<TPack1, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
  Tensor1<double, 3> getNormal(Tensor1<TPack3, 3> &t_coords,
                               Tensor1<double, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
  Tensor1<double, 3> getNormal(Tensor1<double, 3> &t_coords,
                               Tensor1<double, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
 
  Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
                                      Tensor1<TPack1, 3> &t_disp,
                                      Tensor1<TPack1, 3> &t_normal) {
    return getDiffNormalImpl(t_coords, t_disp, t_normal);
  }
 
  inline double getGap(Tensor1<TPack3, 3> &t_coords) {
    return getGapImpl(t_coords);
  }
 
  Tensor1<double, 3> getdGap(Tensor1<TPack3, 3> &t_coords,
                             Tensor1<TPack1, 3> &t_normal) {
    return getdGapImpl(t_coords, t_normal);
  }
 
  MoFEMErrorCode getBodyOptions() {
    MoFEMFunctionBegin;
    PetscBool rflg;
    int nb_dirs = 3;
 
    CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
    string param = "-radius" + to_string(iD);
    CHKERR PetscOptionsScalar(param.c_str(), "set roller radius", "", rAdius,
                              &rAdius, PETSC_NULL);
    param = "-fillet" + to_string(iD);
    CHKERR PetscOptionsScalar(param.c_str(), "set torus small radius", "",
                              fIllet, &fIllet, PETSC_NULL);
    param = "-angle_a" + to_string(iD);
    CHKERR PetscOptionsScalar(param.c_str(), "set roller angle of attack", "",
                              angleA, &angleA, PETSC_NULL);
    param = "-angle_b" + to_string(iD);
    CHKERR PetscOptionsScalar(param.c_str(), "set roller back angle", "",
                              angleB, &angleB, PETSC_NULL);
    hEight = 3 * fIllet;
    param = "-height" + to_string(iD);
    CHKERR PetscOptionsScalar(param.c_str(), "set roller height (optional)", "",
                              hEight, &hEight, PETSC_NULL);
    param = "-direction" + to_string(iD);
    CHKERR PetscOptionsGetRealArray(NULL, NULL, param.c_str(), &oRientation(0),
                                    &nb_dirs, &rflg);
    ierr = PetscOptionsEnd();
    CHKERRG(ierr);
    if (rflg)
      CHKERR computeRotationMatrix();
 
    // cP = make_shared<ArbitrarySurfaceData>(rAdius, fIllet);
 
    torusRadius = rAdius - fIllet;
    angleA = std::tan(angleA * M_PI / 180);
    angleB = std::tan(angleB * M_PI / 180);
 
    coneRadiusA = torusRadius + fIllet / sqrt(1 + angleA * angleA);
    coneTopRadiusA = coneRadiusA - hEight * angleA;
    coneOffsetA = -fIllet * angleA / sqrt(1 + angleA * angleA) - hEight / 2.;
 
    coneTopRadiusB = torusRadius + fIllet / sqrt(1 + angleB * angleB);
    coneRadiusB = coneTopRadiusB - hEight * angleB;
    coneOffsetB = fIllet * angleB / sqrt(1 + angleB * angleB) + hEight / 2.;
 
    MoFEMFunctionReturn(0);
  }
 
  template <typename T1, typename T2>
  inline Tensor1<double, 3> &getNormalImpl(Tensor1<T1, 3> &t_coords,
                                           Tensor1<T2, 3> &t_disp) {
    auto t_d = getPointCoords(t_coords, t_disp);
    pointCoords(i) = rotationMat(i, j) * t_d(j);
    t_d(i) = pointCoords(i);
 
    const double inv_eps = 1e6;
    array<double, 3> gaps;
    const double half_height = hEight / 2.;
 
    double cone_apex_offset_a = coneOffsetA + half_height;
    double cone_apex_offset_b = coneOffsetB - half_height;
 
    // https://computergraphics.stackexchange.com/questions/7485/glsl-shapes-signed-distance-field-implementation-explanation
    auto sd_infinite_cone = [&](Tensor1<double, 3> p, double slope) {
      double c1 = 1. / sqrt(1 + (slope * slope));
      double c2 = c1 * slope;
      auto prod = sqrt(p(0) * p(0) + p(1) * p(1));
      auto prod2 = sqrt(1 + slope * slope);
      tNormal(0) = p(0) / (prod2 * prod);
      tNormal(1) = p(1) / (prod2 * prod);
      tNormal(2) = slope / prod2;
      return c1 * prod + c2 * p(2);
    };
 
    const double full_h_a = -cone_apex_offset_a + coneRadiusA / angleA;
    const double full_h_b = cone_apex_offset_b + coneTopRadiusB / angleB;
 
    Tensor1<double, 3> t_apx_1(t_d(0), t_d(1), t_d(2) - full_h_a);
    Tensor1<double, 3> t_apx_2(t_d(0), t_d(1), t_d(2) + full_h_b);
 
    auto torus_dist = [&]() {
      const double prod = sqrt(pow(t_d(0), 2) + pow(t_d(1), 2));
      const double prod2 = sqrt(pow(-torusRadius + prod, 2) + pow(t_d(2), 2));
      tNormal(0) = (t_d(0) * (-torusRadius + prod)) / (prod * prod2);
      tNormal(1) = (t_d(1) * (-torusRadius + prod)) / (prod * prod2);
      tNormal(2) = t_d(2) / prod2;
      return -fIllet + prod2;
    };
 
    gaps[0] = torus_dist();
    gaps[1] = t_d(2) < -cone_apex_offset_a ? inv_eps
                                           : sd_infinite_cone(t_apx_1, angleA);
    gaps[2] = t_d(2) > -cone_apex_offset_b ? inv_eps
                                           : sd_infinite_cone(t_apx_2, -angleB);
 
    int nb = distance(gaps.begin(), std::min_element(gaps.begin(), gaps.end()));
    gAp = gaps[nb];
 
    switch (nb) {
    case 0: {
      torus_dist();
      break;
    }
    case 1: {
      // compute normal for cone A
      sd_infinite_cone(t_apx_1, angleA);
      break;
    }
    case 2: {
      // compute normal for cone b
      sd_infinite_cone(t_apx_2, -angleB);
    } break;
    }
 
    //    // for debugging
    // auto my_rand_mn = [](double M, double N) {
    //   return M + (rand() / (RAND_MAX / (N - M)));
    // };
    // auto t_off = getBodyOffset();
    // std::ofstream afile("cone_pts.csv", std::ios::ate);
    // if (afile.is_open()) {
    //   afile << "x,y,z\n";
    //   Tensor1<double, 3> p(0, 0, 0);
    //   for (int n = 0; n != 1e6; n++) {
    //     double x = my_rand_mn(-1.5, 1.5);
    //     double y = my_rand_mn(-1.5, 1.5);
    //     double z = my_rand_mn(-1, 1);
    //     Tensor1<double, 3> coords(x, y, z);
 
    //     t_d(i) = coords(i);
    //     t_dc1(i) = coords(i);
    //     t_dc2(i) = coords(i);
    //     t_dc1(2) -= full_h_a;
    //     t_dc2(2) += full_h_b;
 
    //     gaps[0] = torus_dist();
    //     gaps[1] = t_d(2) < -cone_apex_offset_a ? inv_eps :
    //     sd_infinite_cone(t_dc1, angleA); gaps[2] = t_d(2) >
    //     -cone_apex_offset_b ? inv_eps : sd_infinite_cone(t_dc2, -angleB);
 
    //     if (gaps[0] < 0 || gaps[1] < 0 || gaps[2] < 0) {
 
    //       afile << x + t_off(0) << "," << y + t_off(1) << "," << z + t_off(2)
    //             << "\n";
    //     }
    //   }
    //   afile.close();
    // }
 
    tNormal.normalize();
    auto normal_copy = tNormal;
    tNormal(i) = rotationMat(j, i) * normal_copy(j);
 
    return tNormal;
  };
 
  template <typename T1, typename T2, typename T3>
  inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
                                                 Tensor1<T2, 3> &t_disp,
                                                 Tensor1<T3, 3> &t_normal) {
    Tensor2<double, 3, 3> diff_normal;
    Tensor1<double, 3> norm_diff;
    const double clean_gap = gAp;
    const double eps = 1e-8;
    for (int ii = 0; ii != 3; ++ii) {
      Tensor1<double, 3> pert_disp{t_disp(0), t_disp(1), t_disp(2)};
      pert_disp(ii) += eps;
      auto pert_normal = getNormal(t_coords, pert_disp);
      norm_diff(i) = (pert_normal(i) - t_normal(i)) / eps;
      dGap(ii) = (gAp - clean_gap) / eps;
 
      for (int jj = 0; jj != 3; ++jj) {
        diff_normal(jj, ii) = norm_diff(jj);
      }
    }
    return diff_normal;
  };
 
  template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
    return gAp;
  };
 
  template <typename T1, typename T2>
  inline Tensor1<double, 3> getdGapImpl(Tensor1<T1, 3> &t_coords,
                                        Tensor1<T2, 3> &t_normal) {
    return dGap;
  };
};
 
struct ConeRigidBody : public RigidBodyData {
  shared_ptr<ArbitrarySurfaceData> cP;
  double sLope;
  double hEight;
  double rAdius;
  double smallRadius;
  double oFfset;
 
  double aNgle;
  ConeRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
      : RigidBodyData(c_coords, roller_disp, id) {
    // , cP(R, r, 1)
    sLope = 1;
    hEight = 1;
    rAdius = 1;
    oFfset = 0;
    // compute rotation matrix here
  }
 
  MoFEMErrorCode getRollerDataForTag() {
    MoFEMFunctionBeginHot;
    bodyType = CONE;
    dataForTags[0] = rAdius;
    dataForTags[2] = hEight;
    dataForTags[3] = aNgle;
    MoFEMFunctionReturnHot(0);
  }
 
  Tensor1<double, 3> getNormal(Tensor1<TPack3, 3> &t_coords,
                               Tensor1<TPack1, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
  Tensor1<double, 3> getNormal(Tensor1<TPack3, 3> &t_coords,
                               Tensor1<double, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
  Tensor1<double, 3> getNormal(Tensor1<double, 3> &t_coords,
                               Tensor1<double, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
 
  Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
                                      Tensor1<TPack1, 3> &t_disp,
                                      Tensor1<TPack1, 3> &t_normal) {
    return getDiffNormalImpl(t_coords, t_disp, t_normal);
  }
 
  inline double getGap(Tensor1<TPack3, 3> &t_coords) {
    return getGapImpl(t_coords);
  }
  Tensor1<double, 3> getdGap(Tensor1<TPack3, 3> &t_coords,
                             Tensor1<TPack1, 3> &t_normal) {
    return getdGapImpl(t_coords, t_normal);
  }
 
  template <typename T1, typename T2>
  inline Tensor1<double, 3> &getNormalImpl(Tensor1<T1, 3> &t_coords,
                                           Tensor1<T2, 3> &t_disp) {
 
    auto t_d = getPointCoords(t_coords, t_disp);
    pointCoords(i) = rotationMat(i, j) * t_d(j);
    t_d(i) = pointCoords(i);
 
    // Tensor1<double, 3> c_center(0., 0., 0.);
    // Tensor1<double, 3> q_minus_c(0., 0., 0.);
    // Tensor1<double, 3> K(0., 0., 0.);
    // auto Q = pointCoords;
    // auto P_Q = pointCoords;
    // auto Q_K = pointCoords;
 
    const double half_height = hEight / 2.;
    // const double rad2 = rAdius * rAdius;
    // const double small_rad2 = smallRadius * smallRadius;
 
    // tNormal(i) = 0.;
    // pointCoords(2) += (oFfset + half_height);
    // t_d(2) += oFfset + half_height;
    // if (t_d(2) > half_height) {
    //   tNormal(2) = 1;
    //   if (pow(t_d(1), 2) + pow(t_d(0), 2) < rad2) {
    //     gAp = tNormal(i) * t_d(i) - half_height;
    //   } else {
    //     c_center(2) = half_height;
    //     Q(2) = c_center(2); // Q
    //     q_minus_c(i) = Q(i) - c_center(i);
    //     q_minus_c.normalize();
    //     K(i) = c_center(i) + rAdius * q_minus_c(i);
    //     P_Q(i) = t_d(i) - Q(i);
    //     Q_K(i) = Q(i) - K(i);
    //     gAp = sqrt(P_Q.l2() * P_Q.l2() + Q_K.l2() * Q_K.l2());
    //   }
    // } else if (t_d(2) < -half_height) {
    //   tNormal(2) = -1;
    //   if (pow(t_d(1), 2) + pow(t_d(0), 2) < small_rad2) {
    //     gAp = tNormal(i) * t_d(i) - half_height;
    //   } else {
    //     c_center(2) = -half_height;
    //     Q(2) = c_center(2); // Q
    //     q_minus_c(i) = Q(i) - c_center(i);
    //     q_minus_c.normalize();
    //     K(i) = c_center(i) + smallRadius * q_minus_c(i);
    //     P_Q(i) = t_d(i) - Q(i);
    //     Q_K(i) = Q(i) - K(i);
    //     gAp = sqrt(P_Q.l2() * P_Q.l2() + Q_K.l2() * Q_K.l2());
    //   }
    // } else {
    //   auto closest_pt_cone = cP->getClosestPointToCone(t_d);
    //   closestPoint(i) = closest_pt_cone(i);
 
    //   tNormal(j) = t_d(j) - closestPoint(j);
    //   auto test_normal = tNormal.l2();
    //   if (cP->isPointInsideCone(t_d))
    //     tNormal(i) *= -1;
    //   tNormal.normalize();
 
    //   gAp = tNormal(i) * (t_d(i) - closestPoint(i));
    // }
 
    auto clamp = [](auto x, auto min, auto max) {
      if (x < min)
        x = min;
      else if (x > max)
        x = max;
      return x;
    };
 
    auto sd_capped_cone = [&](Tensor1<double, 3> p, double h, double r1,
                              double r2) {
      typedef Tensor1<double, 2> vec2;
      Index<'i', 2> i;
      vec2 cb;
      vec2 q(sqrt(p(i) * p(i)), p(2));
      vec2 k1(r2, h);
      vec2 k2(r2 - r1, 2.0 * h);
      vec2 ca(q(0) - std::min(q(0), (q(1) < 0.0) ? r1 : r2), abs(q(1)) - h);
      vec2 k1_q(k1(0) - q(0), k1(1) - q(1));
      cb(i) = q(i) - k1(i) +
              k2(i) * clamp((k1_q(i) * k2(i)) / (k2(i) * k2(i)), 0.0, 1.0);
      double s = (cb(0) < 0.0 && ca(1) < 0.0) ? -1.0 : 1.0;
      return s * sqrt(std::min((ca(i) * ca(i)), (cb(i) * cb(i))));
    };
 
    const double eps = 1e-6;
    auto cone_distance =
        sd_capped_cone(pointCoords, half_height, rAdius, smallRadius);
 
    auto get_d = [&](int a) {
      auto pt = pointCoords;
      pt(a) += eps;
      return (sd_capped_cone(pt, half_height, rAdius, smallRadius) -
              cone_distance) /
             eps;
    };
 
    // for debugging
    // auto my_rand_mn = [](double M, double N) {
    //   return M + (rand() / (RAND_MAX / (N - M)));
    // };
    // auto t_off = getBodyOffset();
    // std::ofstream afile("cone_pts.csv", std::ios::ate);
    // if (afile.is_open()) {
    //   afile << "X,Y,Z\n";
    //   Tensor1<double, 3> p(0, 0, 0);
    //   for (int n = 0; n != 1e6; n++) {
    //     double x = my_rand_mn(-3, 3);
    //     double y = my_rand_mn(-3, 3);
    //     double z = my_rand_mn(-3, 3);
    //     Tensor1<double, 3> coords(x, y, z);
    //     if (sd_capped_cone(coords, half_height, rAdius, smallRadius) < 0) {
 
    //       afile << x + t_off(0) << "," << y + t_off(1) << "," << z + t_off(2)
    //       << "\n";
    //     }
    //   }
    //   afile.close();
    // }
 
    Tensor1<double, 3> my_normal(get_d(0), get_d(1), get_d(2));
    my_normal.normalize();
 
    gAp = cone_distance;
    tNormal(i) = rotationMat(j, i) * my_normal(j);
 
    return tNormal;
  };
 
  template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
    return gAp;
  };
 
  template <typename T1, typename T2, typename T3>
  inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
                                                 Tensor1<T2, 3> &t_disp,
                                                 Tensor1<T3, 3> &t_normal) {
    Tensor2<double, 3, 3> diff_normal;
    Tensor1<double, 3> norm_diff;
    const double clean_gap = gAp;
    const double eps = 1e-8;
    for (int ii = 0; ii != 3; ++ii) {
      Tensor1<double, 3> pert_disp{t_disp(0), t_disp(1), t_disp(2)};
      pert_disp(ii) += eps;
      auto pert_normal = getNormal(t_coords, pert_disp);
      norm_diff(i) = (pert_normal(i) - t_normal(i)) / eps;
      dGap(ii) = (gAp - clean_gap) / eps;
 
      for (int jj = 0; jj != 3; ++jj) {
        diff_normal(jj, ii) = norm_diff(jj);
      }
    }
    return diff_normal;
  };
 
  template <typename T1, typename T2>
  inline Tensor1<double, 3> getdGapImpl(Tensor1<T1, 3> &t_coords,
                                        Tensor1<T2, 3> &t_normal) {
    return dGap;
  };
 
  virtual MoFEMErrorCode getBodyOptions() {
    MoFEMFunctionBegin;
    PetscBool rflg;
    int nb_dirs = 3;
    double angle = 45;
    CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
    string param = "-radius" + to_string(iD);
    CHKERR PetscOptionsScalar(param.c_str(), "set cone radius", "", rAdius,
                              &rAdius, PETSC_NULL);
    param = "-height" + to_string(iD);
    CHKERR PetscOptionsScalar(param.c_str(), "set cone height", "", hEight,
                              &hEight, PETSC_NULL);
    param = "-angle" + to_string(iD);
    CHKERR PetscOptionsScalar(param.c_str(), "set cone half angle", "", angle,
                              &angle, PETSC_NULL);
    param = "-direction" + to_string(iD);
    CHKERR PetscOptionsGetRealArray(NULL, NULL, param.c_str(), &oRientation(0),
                                    &nb_dirs, &rflg);
    ierr = PetscOptionsEnd();
    CHKERRG(ierr);
    aNgle = angle;
    sLope = std::tan(aNgle * M_PI / 180);
    oFfset = (rAdius / sLope) - hEight;
    smallRadius = rAdius * oFfset / (oFfset + hEight);
 
    if (rflg)
      CHKERR computeRotationMatrix();
 
    cP = make_shared<ArbitrarySurfaceData>(1, sLope);
 
    MoFEMFunctionReturn(0);
  }
};
 
struct STLRigidBody : public RigidBodyData {
  moab::Core mbInstance;
  moab::Interface *bodyMesh;
  shared_ptr<OrientedBoxTreeTool> orientedBox;
  EntityHandle treeMeshset;
  Tag tagNormal;
  EntityHandle triangle;
 
  STLRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
      : RigidBodyData(c_coords, roller_disp, id) {
    bodyMesh = &mbInstance;
  }
 
  MoFEMErrorCode getRollerDataForTag() {
    MoFEMFunctionBeginHot;
    bodyType = STL;
    MoFEMFunctionReturnHot(0);
  }
 
  Tensor1<double, 3> getNormal(Tensor1<TPack3, 3> &t_coords,
                               Tensor1<TPack1, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
  Tensor1<double, 3> getNormal(Tensor1<TPack3, 3> &t_coords,
                               Tensor1<double, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
  Tensor1<double, 3> getNormal(Tensor1<double, 3> &t_coords,
                               Tensor1<double, 3> &t_disp) {
    return getNormalImpl(t_coords, t_disp);
  }
 
  Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
                                      Tensor1<TPack1, 3> &t_disp,
                                      Tensor1<TPack1, 3> &t_normal) {
    return getDiffNormalImpl(t_coords, t_disp, t_normal);
  }
 
  inline double getGap(Tensor1<TPack3, 3> &t_coords) {
    return getGapImpl(t_coords);
  }
 
  Tensor1<double, 3> getdGap(Tensor1<TPack3, 3> &t_coords,
                             Tensor1<TPack1, 3> &t_normal) {
    return getdGapImpl(t_coords, t_normal);
  }
 
  template <typename T1, typename T2>
  inline Tensor1<double, 3> &getNormalImpl(Tensor1<T1, 3> &t_coords,
                                           Tensor1<T2, 3> &t_disp) {
    auto t_d = getPointCoords(t_coords, t_disp);
 
    CHKERR orientedBox->closest_to_location(&t_d(0), treeMeshset,
                                            &closestPoint(0), triangle);
    CHKERR bodyMesh->tag_get_data(tagNormal, &triangle, 1, &tNormal(0));
    gAp = tNormal(i) * (pointCoords(i) - closestPoint(i));
    return tNormal;
  };
 
  template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
    return gAp;
  };
 
  template <typename T1, typename T2>
  inline Tensor1<double, 3> getdGapImpl(Tensor1<T1, 3> &t_coords,
                                        Tensor1<T2, 3> &t_normal) {
    return dGap;
  };
 
  template <typename T1, typename T2, typename T3>
  inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
                                                 Tensor1<T2, 3> &t_disp,
                                                 Tensor1<T3, 3> &t_normal) {
    Tensor2<double, 3, 3> diff_normal;
    Tensor1<double, 3> norm_diff;
    const double clean_gap = gAp;
    const double eps = 1e-8;
    for (int ii = 0; ii != 3; ++ii) {
      Tensor1<double, 3> pert_disp{t_disp(0), t_disp(1), t_disp(2)};
      pert_disp(ii) += eps;
      auto pert_normal = getNormal(t_coords, pert_disp);
      norm_diff(i) = (pert_normal(i) - t_normal(i)) / eps;
      dGap(ii) = (gAp - clean_gap) / eps;
 
      for (int jj = 0; jj != 3; ++jj) {
        diff_normal(jj, ii) = norm_diff(jj);
      }
    }
    return diff_normal;
  };
 
  MoFEMErrorCode getBodyOptions() {
    MoFEMFunctionBegin;
    CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
    PetscBool rflg;
    string param = "-body_file_name" + to_string(iD);
    char mesh_file_name[255];
    CHKERR PetscOptionsString(param.c_str(), "file name", "", "mesh.vtk",
                              mesh_file_name, 255, &rflg);
    if (string(mesh_file_name).substr(string(mesh_file_name).length() - 3) !=
        "vtk")
      SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
              "body surface should be vtk surface");
    bodyMesh->load_file(mesh_file_name, 0, "");
    Range tris;
    CHKERR bodyMesh->get_entities_by_dimension(0, 2, tris, false);
    orientedBox =
        make_shared<OrientedBoxTreeTool>(bodyMesh, "ROOTSETSURF", true);
    CHKERR orientedBox->build(tris, treeMeshset);
    char normal_tag_name[255] = "Normals";
    if (bodyMesh->tag_get_handle(normal_tag_name, tagNormal) != MB_SUCCESS) {
      SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
              "Normal tag name %s cannot be found. Please "
              "provide the correct name. or run ./calculate_normals");
    }
 
    ierr = PetscOptionsEnd();
    CHKERRG(ierr);
    MoFEMFunctionReturn(0);
  }
};
//  struct NurbsRigidBody : public RigidBodyData {
//     NurbsRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
//       : RigidBodyData(c_coords, roller_disp, id) {}
//  };