06_test_ann_vs_geom_trimesh_plane.cpp

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

#include "contact_solver_factory.hpp"
#include "distribution_uniform.hpp"
#include "general_net.hpp"
#include "igl_wrapper.hpp"
#include "mlpack_utils.hpp"
#include "model_volume_based.hpp"
#include "particle.hpp"
#include "regression_net.hpp"
#include "shape_trimesh.hpp"
#include "solver_boolean_pp.hpp"
#include "solver_boolean_pw.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;
 
void TestANNvsGeometricTrimeshPlane() {
  TriMesh tri_mesh_1;
  tri_mesh_1.InitFromSTL("data/particle_template.stl");
  tri_mesh_1.Decimate(200);
  tri_mesh_1.AlignAxes();
  tri_mesh_1.SetSize(0.5);
  Particle obj_p = Particle(&tri_mesh_1);
  obj_p.need_update_stl_model = true;
  cout << "particle created ... " << endl;
 
  TriMesh tri_mesh_2;
  tri_mesh_2.InitFromSTL("data/plate_z0.stl");
  Wall obj_w = Wall(&tri_mesh_2);
  obj_w.need_update_stl_model = true;
  cout << "wall created ... " << endl;
 
  SolverBooleanPW cnt_solver;
  VolumeBased cnt_model;
 
  string root_dir = "local/examples/netdem/ann_models/trimesh_plane/";
  GeneralNet classifier;
  classifier.Load(root_dir + "ann_classifier.xml", "detection");
  RegressionNet regressor;
  regressor.Load(root_dir + "ann_regressor.xml", "resolution");
 
  // use bound sphere to narrow down the random space
  double dist_max = obj_p.shape->GetBoundSphereRadius() * 1.1;
  double dist_min{dist_max};
  for (auto &vert : tri_mesh_1.vertices) {
    dist_min = min(Math::NormL2(vert), dist_min);
  }
  dist_min *= 0.9;
  double dist_range = dist_max - dist_min;
 
  // random generator
  UniformDistribution uniform_dist(0.0, 1.0);
 
  // use spherical centroidal voronoi to sample uniform unit VecXT
  VecXT<Vec3d> vertices = SphericalVoronoi::Solve(1000, 10000, 1.0e-4);
  VecXT<Vec3i> facets;
  IGLWrapper::ConvexHull(vertices, &vertices, &facets);
 
  for (int trial = 0; trial < 100; trial++) {
    // random direction
    int id_facet = floor(uniform_dist.Get() * facets.size());
    auto vert_0 = vertices[facets[id_facet][0]];
    auto vert_1 = vertices[facets[id_facet][1]];
    auto vert_2 = vertices[facets[id_facet][2]];
 
    double u_vert = uniform_dist.Get();
    double v_vert = uniform_dist.Get() * (1 - u_vert);
    double w_vert = 1 - u_vert - v_vert;
 
    Vec3d dir_n;
    dir_n[0] = u_vert * vert_0[0] + v_vert * vert_1[0] + w_vert * vert_2[0];
    dir_n[1] = u_vert * vert_0[1] + v_vert * vert_1[1] + w_vert * vert_2[1];
    dir_n[2] = u_vert * vert_0[2] + v_vert * vert_1[2] + w_vert * vert_2[2];
    Math::Normalize(&dir_n);
 
    // random position
    double dist_pc_to_plane = dist_min + uniform_dist.Get() * dist_range;
 
    // obtain the rotation quaternion for the wall (currently, the wall is
    // represented by trimesh, thus need to convert dir_n and dist to the
    // rotation quaternion of the wall. The benchmark solution is based on
    // boolean operation.)
    Vec3d dir_n_ref{0, 0, 1};
    Vec3d rot_axis = Math::Cross(dir_n_ref, dir_n);
 
    Vec4d quat;
    quat[0] = 1 + Math::Dot(dir_n, dir_n_ref);
    quat[1] = rot_axis[0];
    quat[2] = rot_axis[1];
    quat[3] = rot_axis[2];
    Math::Quaternion::Normalize(&quat);
 
    // update the wall with random position and rotation
    obj_w.SetPosition(-dist_pc_to_plane * dir_n[0],
                      -dist_pc_to_plane * dir_n[1],
                      -dist_pc_to_plane * dir_n[2]);
    obj_w.SetQuaternion(quat[0], quat[1], quat[2], quat[3]);
 
    // contact detection and resolution
    double scale = 1.0 / obj_p.shape->GetSize();
    arma::mat input(4, 1, arma::fill::zeros);
    input(0, 0) = dist_pc_to_plane * scale;
    input(1, 0) = dir_n[0];
    input(2, 0) = dir_n[1];
    input(3, 0) = dir_n[2];
    auto output = classifier.Classify(input);
 
    cnt_solver.Init(&obj_p, &obj_w);
    auto cnt_flag_geo = cnt_solver.Detect();
    cout << "ann vs geometric: " << output(0) << ", " << cnt_flag_geo << endl;
 
    if (cnt_flag_geo) {
      auto cnt = ContactPW(&obj_p, &obj_w);
      cnt.SetCollisionModel(&cnt_model);
      cnt_solver.ResolveInit(&cnt, 1.0e-4);
      auto &cnt_geoms = cnt.collision_entries[0].cnt_geoms;
 
      // skip the contact if overlap is too large
      if (cnt_geoms.vol * cnt_geoms.sn > 6.0e-4)
        continue;
 
      auto output = regressor.Predict(input);
      cout << ">> ann: "
           << pow(output(0, 0) / 40.0, 2.0) / scale / scale / scale << ", "
           << output(1, 0) / 40.0 / scale / scale << ", "
           << output(2, 0) / scale << ", " << output(3, 0) / scale << ", "
           << output(4, 0) / scale << endl;
 
      cout << ">> geo: " << cnt_geoms.vol << ", " << cnt_geoms.sn << ", "
           << cnt_geoms.pos[0] << ", " << cnt_geoms.pos[1] << ", "
           << cnt_geoms.pos[2] << endl;
    }
  }
}