0_demo_erosion_issue.cpp

This is an example of how to use the netdem library.

#include "contact_solver_factory.hpp"
#include "particle.hpp"
#include "shape_ellipsoid.hpp"
#include "shape_plane.hpp"
#include "solver_gjk_pp.hpp"
#include "solver_gjk_pw.hpp"
#include "utils_math.hpp"
#include <filesystem>
#include <string>
 
using namespace netdem;
using namespace std;
 
class SolverGJKPP_TEST : public SolverGJKPP {
public:
  Vec3d MinkowskiDiff(Vec3d const &dir, double erosion_ratio = 0) {
    auto pos_1 = shape_1->SupportPoint(-1.0 * dir);
    auto pos_2 = Math::Rotate(
        shape_2->SupportPoint(Math::Rotate(dir, dquat_12_conj)), dquat_12);
 
    return (1 - erosion_ratio) * (pos_2 - pos_1) + dpos_12_ref;
  }
 
  tuple<double, Vec3d, Vec3d> EPA() { return SolverGJKPP::EPA(); }
 
  tuple<double, Vec3d, Vec3d> GJK_EROSION() {
    return SolverGJKPP::GJK_EROSION();
  }
 
  tuple<double, Vec3d, Vec3d> GJK_EROSION(double erosion_ratio) {
    Vec3d sup_point;
    Vec3d sup_dir = particle_1->pos - particle_2->pos;
    Math::Normalize(&sup_dir);
 
    sup_point = MinkowskiDiff(sup_dir, erosion_ratio);
 
    auto simplex = Simplex();
    simplex.PushBack(sup_point);
 
    sup_dir = -1.0 * sup_point;
    Math::Normalize(&sup_dir);
 
    Vec3d cnt_dir_n, cnt_pos;
    double cnt_len_n;
 
    bool is_collide{false};
    int iter, iter_max = 100;
    double min_dist_mink = -1e8, min_dist_simp = Math::NormL2(sup_point);
    for (iter = 0; iter < iter_max; iter++) {
      sup_point = MinkowskiDiff(sup_dir, erosion_ratio);
      min_dist_mink = max(-Math::Dot(sup_point, sup_dir), min_dist_mink);
 
      // cout << "debug min_dist pp: " << iter << ", " << min_dist_mink << ", "
      //      << min_dist_simp - min_dist_mink << ", " << sup_point << endl;
 
      if (min_dist_simp - min_dist_mink < 1e-10) {
        // gather contact geometric features
        cnt_dir_n = sup_dir;
 
        sup_point = MinkowskiDiff(cnt_dir_n);
        cnt_len_n = Math::Dot(sup_point, cnt_dir_n);
        bool find_cnt_pos;
        tie(cnt_pos, find_cnt_pos) = GetContactPoint(cnt_dir_n);
 
        if (cnt_len_n < 0 || !find_cnt_pos) {
          cnt_len_n = 0;
          cnt_dir_n[0] = 0;
          cnt_dir_n[1] = 0;
          cnt_dir_n[2] = 0;
        }
        break;
      }
 
      simplex.PushBack(sup_point);
      UpdateSimplex(&simplex, &sup_dir, &min_dist_simp, &is_collide);
      if (is_collide) {
        cnt_len_n = 0;
        cnt_dir_n[0] = 0;
        cnt_dir_n[1] = 0;
        cnt_dir_n[2] = 0;
        break;
      }
    }
 
    if (iter >= iter_max - 1) {
      cout << "warning: git_erosion pp not converged" << endl;
    }
 
    return tuple<double, Vec3d, Vec3d>(cnt_len_n, cnt_dir_n, cnt_pos);
  }
};
 
void DemoErosionIssue() {
  Ellipsoid ellipsoid = Ellipsoid(0.5, 1, 0.5);
  Particle obj_1 = Particle(&ellipsoid);
  Particle obj_2 = Particle(&ellipsoid);
 
  auto cnt_solver = SolverGJKPP_TEST();
  auto cnt_geoms_epa = CollisionGeometries();
 
  double pos_y, pos_y_upper{ellipsoid.axis_b * 2}, pos_y_lower{0},
      len_n_target{0.1};
  for (int i = 0; i < 100; i++) {
    pos_y = 0.5 * (pos_y_lower + pos_y_upper);
    obj_2.SetPosition(0, pos_y, 0.3);
 
    cnt_solver.Init(&obj_1, &obj_2);
    if (cnt_solver.Detect()) {
      tie(cnt_geoms_epa.len_n, cnt_geoms_epa.dir_n, cnt_geoms_epa.pos) =
          cnt_solver.EPA();
 
      cout << "iter: " << i << ", " << pos_y << ", " << cnt_geoms_epa.len_n
           << endl;
 
      if (abs(cnt_geoms_epa.len_n - len_n_target) < 1e-8) {
        break;
      } else if (cnt_geoms_epa.len_n < len_n_target) {
        pos_y_upper = pos_y;
      } else {
        pos_y_lower = pos_y;
      }
    } else {
      pos_y_upper = pos_y;
    }
  }
 
  Vec3d dir_sup = -1.0 * cnt_geoms_epa.dir_n;
  Vec3d pos_sup = cnt_solver.MinkowskiDiff(dir_sup);
  double pos_sup_norm = Math::NormL2(pos_sup);
  cout << "result: " << pos_sup_norm << ": " << pos_sup / pos_sup_norm << ": "
       << cnt_geoms_epa.dir_n << endl;
 
  // filesystem::create_directories("tmp/tests/");
  // obj_1.SaveAsVTK("tmp/tests/obj_1.vtk");
  // obj_2.SaveAsVTK("tmp/tests/obj_2.vtk");
 
  double erosion_ratio{0};
  auto cnt_geoms_erosion = CollisionGeometries();
  for (int i = 0; i < 200; i++) {
    erosion_ratio += len_n_target / 100;
    tie(cnt_geoms_erosion.len_n, cnt_geoms_erosion.dir_n,
        cnt_geoms_erosion.pos) = cnt_solver.GJK_EROSION(erosion_ratio);
 
    cout << "iter: " << i << "; erosion rate: " << erosion_ratio
         << "; overlap: " << cnt_geoms_erosion.len_n << endl
         << "  "
         << "erosion dir_n: " << cnt_geoms_erosion.dir_n
         << "; epa dir_n: " << cnt_geoms_epa.dir_n << endl;
  }
}