31_test_dataset_ellipsoid.cpp

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

#include "contact_solver_factory.hpp"
#include "distribution_uniform.hpp"
#include "igl_wrapper.hpp"
#include "model_linear_spring.hpp"
#include "particle.hpp"
#include "shape_ellipsoid.hpp"
#include "shape_plane.hpp"
#include "solver_gjk_pp.hpp"
#include "spherical_voronoi.hpp"
#include "utils_math.hpp"
#include <fstream>
#include <iostream>
#include <random>
#include <sstream>
#include <string>
 
using namespace netdem;
using namespace std;
 
VecXT<VecXT<double>> LoadDatasetEllipsoid(string filename) {
  ifstream file_handle;
  file_handle.open(filename, ios::binary);
  if (!file_handle) {
    cout << "Fail to read file: " << filename << endl;
    return VecXT<VecXT<double>>();
  }
 
  file_handle.seekg(0, ios::end);
  long context_size = file_handle.tellg();
  char *buffer = new char[context_size];
  file_handle.seekg(0);
  file_handle.read(buffer, context_size);
  file_handle.close();
 
  stringstream ss;
  ss.str("");
  ss.clear();
  ss.str(buffer);
  VecXT<VecXT<double>> dataset;
  string str_line, str_num;
  VecXT<double> data_line;
 
  getline(ss, str_line);
  istringstream readstr(str_line);
  while (getline(readstr, str_num, ',')) {
    data_line.push_back(atof(str_num.c_str()));
  }
  getline(readstr, str_num);
  data_line.push_back(atof(str_num.c_str()));
  dataset.push_back(data_line);
  int num_data_per_line = data_line.size();
 
  while (getline(ss, str_line)) {
    if (str_line.find(",") == string::npos)
      continue;
 
    data_line.resize(num_data_per_line);
    istringstream readstr(str_line);
    for (int i = 0; i < num_data_per_line - 1; i++) {
      getline(readstr, str_num, ',');
      data_line[i] = atof(str_num.c_str());
    }
    getline(readstr, str_num);
    data_line[num_data_per_line - 1] = atof(str_num.c_str());
    dataset.push_back(data_line);
  }
  dataset.shrink_to_fit();
 
  delete[] buffer;
  return dataset;
}
 
void TestDatasetEllipsoidDetection() {
  // load particle
  Ellipsoid ellipsoid = Ellipsoid(1, 1, 2);
  ellipsoid.SetSize(1.0);
 
  Particle obj_p1 = Particle(&ellipsoid);
  Particle obj_p2 = Particle(&ellipsoid);
  cout << "particle created ... " << endl;
 
  SolverGJKPP cnt_solver;
  LinearSpring cnt_model;
 
  string root_dir = "local/examples/netdem/ann_models/ellipsoid_ellipsoid/";
  VecXT<VecXT<double>> dataset =
      LoadDatasetEllipsoid(root_dir + "dataset_detection.txt");
 
  int num_samples = dataset.size();
  double(*ds_inputs)[7] = new double[num_samples][7];
  bool *ds_cnt_flags = new bool[num_samples];
 
  for (int i = 0, skip = 1; i < num_samples; i += skip) {
    for (int i_inputs = 0; i_inputs < 7; i_inputs++) {
      ds_inputs[i][i_inputs] = dataset[i][i_inputs];
    }
    ds_cnt_flags[i] = (bool)dataset[i][7];
  }
 
  for (int i = 0; i < num_samples; i++) {
    Vec3d pos;
    Vec4d quat;
 
    // unpack the inputs
    pos[0] = ds_inputs[i][0];
    pos[1] = ds_inputs[i][1];
    pos[2] = ds_inputs[i][2];
    quat[0] = ds_inputs[i][3];
    quat[1] = ds_inputs[i][4];
    quat[2] = ds_inputs[i][5];
    quat[3] = ds_inputs[i][6];
 
    obj_p2.SetPosition(pos[0], pos[1], pos[2]);
    obj_p2.SetQuaternion(quat[0], quat[1], quat[2], quat[3]);
 
    cnt_solver.Init(&obj_p1, &obj_p2);
 
    bool cnt_flag = cnt_solver.Detect();
    cout << "true vs data: " << cnt_flag << ", " << ds_cnt_flags[i] << endl;
  }
 
  delete[] ds_inputs;
  delete[] ds_cnt_flags;
}
 
void TestDatasetEllipsoidResolution() {
  // load particle
  Ellipsoid ellipsoid = Ellipsoid(1, 1, 2);
  ellipsoid.SetSize(1.0);
 
  Particle obj_p1 = Particle(&ellipsoid);
  Particle obj_p2 = Particle(&ellipsoid);
  cout << "particle created ... " << endl;
 
  SolverGJKPP cnt_solver;
  LinearSpring cnt_model;
 
  string root_dir = "local/examples/netdem/ann_models/ellipsoid_ellipsoid/";
  VecXT<VecXT<double>> dataset =
      LoadDatasetEllipsoid(root_dir + "dataset_resolution.txt");
 
  int num_samples = dataset.size();
  double(*ds_inputs)[7] = new double[num_samples][7];
  double(*ds_cnt_feats)[7] = new double[num_samples][7];
 
  for (int i = 0, skip = 1; i < num_samples; i += skip) {
    for (int i_inputs = 0; i_inputs < 7; i_inputs++) {
      ds_inputs[i][i_inputs] = dataset[i][i_inputs];
    }
    for (int i_outputs = 0; i_outputs < 7; i_outputs++) {
      ds_cnt_feats[i][i_outputs] = dataset[i][7 + i_outputs];
    }
  }
 
  for (int i = 0; i < num_samples; i++) {
    Vec3d pos;
    Vec4d quat;
 
    // unpack the inputs
    pos[0] = ds_inputs[i][0];
    pos[1] = ds_inputs[i][1];
    pos[2] = ds_inputs[i][2];
    quat[0] = ds_inputs[i][3];
    quat[1] = ds_inputs[i][4];
    quat[2] = ds_inputs[i][5];
    quat[3] = ds_inputs[i][6];
 
    obj_p2.SetPosition(pos[0], pos[1], pos[2]);
    obj_p2.SetQuaternion(quat[0], quat[1], quat[2], quat[3]);
 
    cnt_solver.Init(&obj_p1, &obj_p2);
 
    // contact detection and resolution
    if (cnt_solver.Detect()) {
      auto cnt = ContactPP(&obj_p1, &obj_p2);
      cnt.SetCollisionModel(&cnt_model);
      cnt_solver.ResolveInit(&cnt, 1.0e-4);
      auto &cnt_geoms = cnt.collision_entries[0].cnt_geoms;
 
      cout << ">> true: " << 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;
 
      cout << ">> data: " << ds_cnt_feats[i][0] << ", " << ds_cnt_feats[i][1]
           << ", " << ds_cnt_feats[i][2] << ", " << ds_cnt_feats[i][3] << ", "
           << ds_cnt_feats[i][4] << ", " << ds_cnt_feats[i][5] << ", "
           << ds_cnt_feats[i][6] << endl;
    }
  }
 
  delete[] ds_inputs;
  delete[] ds_cnt_feats;
}
 
void TestDatasetEllipsoid() {
  TestDatasetEllipsoidDetection();
  TestDatasetEllipsoidResolution();
}