2_ev_gjk_performance.cpp

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

#include "dem_profiler.hpp"
#include "distribution_uniform.hpp"
#include "particle.hpp"
#include "shape_ellipsoid.hpp"
#include "shape_sphere.hpp"
#include "solver_gjk_pp.hpp"
#include "utils_math.hpp"
#include <filesystem>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
using namespace netdem;
using namespace std;
 
int num_samples = 1000000;
 
auto *ds_q_pos = new Vec3d[num_samples];
auto *ds_q_quat = new Vec4d[num_samples];
 
auto *ds_len_n = new double[num_samples];
auto *ds_dir_n = new Vec3d[num_samples];
auto *ds_pos = new Vec3d[num_samples];
 
Ellipsoid ellipsoid = Ellipsoid(0.5, 1.0, 0.5);
Particle p1 = Particle(&ellipsoid);
Particle p2 = Particle(&ellipsoid);
SolverGJKPP cnt_solver;
 
void GenDataset(double len_n_min, double len_n_max) {
  UniformDistribution uniform_dist;
  for (int i = 0; i < num_samples; i++) {
    // particle rotation
    double rot_axis_theta = uniform_dist.Get() * Math::PI;
    double rot_axis_phi = uniform_dist.Get() * 2.0 * Math::PI;
    Vec3d rot_axis_sph{1.0, rot_axis_theta, rot_axis_phi};
    double rot_angle = uniform_dist.Get() * 2.0 * Math::PI;
    Vec3d rot_axis = Math::SphericalToCartesian(rot_axis_sph);
    ds_q_quat[i] = Math::Quaternion::FromRodrigues(rot_angle, rot_axis);
 
    // target overlap
    ds_len_n[i] = len_n_min + uniform_dist.Get() * (len_n_max - len_n_min);
 
    // target contact normal
    double dir_theta = uniform_dist.Get() * Math::PI;
    double dir_phi = uniform_dist.Get() * 2.0 * Math::PI;
    Vec3d dir_sph{1.0, dir_theta, dir_phi};
    ds_dir_n[i] = Math::SphericalToCartesian(dir_sph);
 
    // get the support points
    Vec3d sup_pos_1, sup_pos_2;
    Vec3d dir_n_inv = -1.0 * ds_dir_n[i];
    sup_pos_1 = ellipsoid.SupportPoint(dir_n_inv);
    sup_pos_2 = sup_pos_1 + ds_len_n[i] * ds_dir_n[i];
 
    ds_pos[i] = sup_pos_1 + 0.5 * ds_len_n[i] * ds_dir_n[i];
 
    Vec4d quat_conj = Math::Quaternion::Conjugate(ds_q_quat[i]);
    Vec3d dir_n_ref = Math::Rotate(ds_dir_n[i], quat_conj);
    Vec3d sup_pos_2_ref = ellipsoid.SupportPoint(dir_n_ref);
    Vec3d sup_pos_2_rot = Math::Rotate(sup_pos_2_ref, ds_q_quat[i]);
 
    // calculate the particle position
    ds_q_pos[i] = sup_pos_2 - sup_pos_2_rot;
 
    if (((i + 1) % 100000) == 0) {
      cout << "data generated: " << i + 1 << " ..." << endl;
    }
  }
}
 
void SaveReferenceResult(string filename) {
  stringbuf buf;
  ostream os(&buf);
  int os_width = 24;
  os.setf(ios::scientific);
  os.precision(15);
 
  for (int i = 0; i < num_samples; i++) {
    os << ds_q_pos[i][0] << ", " << ds_q_pos[i][1] << ", " << ds_q_pos[i][2]
       << ", " << ds_q_quat[i][0] << ", " << ds_q_quat[i][1] << ", "
       << ds_q_quat[i][2] << ", " << ds_q_quat[i][3] << ", " << ds_len_n[i]
       << ", " << ds_dir_n[i][0] << ", " << ds_dir_n[i][1] << ", "
       << ds_dir_n[i][2] << ", " << ds_pos[i][0] << ", " << ds_pos[i][1] << ", "
       << ds_pos[i][2] << endl;
  }
 
  ofstream outfile;
  outfile.open(filename);
  if (!outfile) {
    cout << "cannot open file: " << filename << endl;
  }
  outfile << buf.str();
  outfile.close();
  cout << "data saved to: " << filename << endl;
}
 
void SaveGJKResult(string filename, bool use_erosion,
                   double erosion_ratio_initial) {
  stringbuf buf;
  ostream os(&buf);
  int os_width = 24;
  os.setf(ios::scientific);
  os.precision(15);
 
  cnt_solver.use_erosion = use_erosion;
  cnt_solver.erosion_ratio_initial = erosion_ratio_initial;
  for (int i = 0; i < num_samples; i++) {
    p2.SetPosition(ds_q_pos[i][0], ds_q_pos[i][1], ds_q_pos[i][2]);
    p2.SetQuaternion(ds_q_quat[i][0], ds_q_quat[i][1], ds_q_quat[i][2],
                     ds_q_quat[i][3]);
 
    cnt_solver.Init(&p1, &p2);
    if (cnt_solver.Detect()) {
      auto cnt = ContactPP();
      cnt_solver.ResolveInit(&cnt, 1e-4);
      auto cnt_geoms = cnt.collision_entries[0].cnt_geoms;
 
      os << ds_q_pos[i][0] << ", " << ds_q_pos[i][1] << ", " << ds_q_pos[i][2]
         << ", " << ds_q_quat[i][0] << ", " << ds_q_quat[i][1] << ", "
         << ds_q_quat[i][2] << ", " << ds_q_quat[i][3] << ", "
         << cnt_geoms.len_n << ", " << cnt_geoms.dir_n[0] << ", "
         << cnt_geoms.dir_n[1] << ", " << cnt_geoms.dir_n[2] << ", "
         << cnt_geoms.pos[0] << ", " << cnt_geoms.pos[1] << ", "
         << cnt_geoms.pos[2] << endl;
    } else {
      os << ds_q_pos[i][0] << ", " << ds_q_pos[i][1] << ", " << ds_q_pos[i][2]
         << ", " << ds_q_quat[i][0] << ", " << ds_q_quat[i][1] << ", "
         << ds_q_quat[i][2] << ", " << ds_q_quat[i][3] << ", " << 0 << ", " << 0
         << ", " << 0 << ", " << 0 << ", " << 0 << ", " << 0 << ", " << 0
         << endl;
    }
  }
 
  ofstream outfile;
  outfile.open(filename);
  if (!outfile) {
    cout << "cannot open file: " << filename << endl;
  }
  outfile << buf.str();
  outfile.close();
  cout << "data saved to: " << filename << endl;
}
 
void EvaluateGJK(bool use_erosion, double erosion_ratio_initial) {
  DEMProfiler dem_profiler;
  dem_profiler.Clear();
  dem_profiler.StartTimer(DEMProfiler::TimerType::custom);
 
  cnt_solver.use_erosion = use_erosion;
  cnt_solver.erosion_ratio_initial = erosion_ratio_initial;
  for (int i = 0; i < num_samples; i++) {
    p2.SetPosition(ds_q_pos[i][0], ds_q_pos[i][1], ds_q_pos[i][2]);
    p2.SetQuaternion(ds_q_quat[i][0], ds_q_quat[i][1], ds_q_quat[i][2],
                     ds_q_quat[i][3]);
 
    cnt_solver.Init(&p1, &p2);
    if (cnt_solver.Detect()) {
      auto cnt = ContactPP();
      cnt_solver.ResolveInit(&cnt, 1e-4);
 
      /*
      if (abs(cnt.collision_entries[0].cnt_geoms.len_n - ds_len_n[i]) >
          1.0e-4) {
        cout << "debug0: " << cnt.collision_entries[0].cnt_geoms.len_n << ", "
             << ds_len_n[i] << endl;
      }
      // */
    }
  }
  dem_profiler.EndTimer(DEMProfiler::TimerType::custom);
 
  if (use_erosion) {
    printf("erosion: %f s \n",
           dem_profiler.timer_list[DEMProfiler::TimerType::custom] / 1000000.0);
  } else {
    printf("epa: %f s \n",
           dem_profiler.timer_list[DEMProfiler::TimerType::custom] / 1000000.0);
  }
}
 
void ClearDataset() {
  delete[] ds_len_n;
  delete[] ds_dir_n;
  delete[] ds_pos;
 
  delete[] ds_q_quat;
  delete[] ds_q_pos;
}
 
void EvaluateGJKPerformance(bool save_results) {
  string out_dir = "tmp/examples/gjk_erosion_issue/result_data/ev_perf/";
  filesystem::create_directories(out_dir);
 
  // contact case
  double erosion_ratio_initial = 0.05;
  GenDataset(0, erosion_ratio_initial);
  if (save_results) {
    SaveReferenceResult(out_dir + "contact_ref.txt");
    SaveGJKResult(out_dir + "contact_epa.txt", false, erosion_ratio_initial);
    SaveGJKResult(out_dir + "contact_ero.txt", true, erosion_ratio_initial);
  }
 
  EvaluateGJK(false, erosion_ratio_initial);
  EvaluateGJK(true, erosion_ratio_initial);
 
  // non-contact case
  GenDataset(-1, 0);
  if (save_results) {
    SaveReferenceResult(out_dir + "noncontact_ref.txt");
    SaveGJKResult(out_dir + "noncontact_epa.txt", false, erosion_ratio_initial);
    SaveGJKResult(out_dir + "noncontact_ero.txt", true, erosion_ratio_initial);
  }
 
  EvaluateGJK(false, erosion_ratio_initial);
  EvaluateGJK(true, erosion_ratio_initial);
 
  ClearDataset();
}