bench.cc

BENCHMARK_DEFINE_F(NarrowPhaseFixture, fcl_gjk_indep_distance_only_3d)(benchmark::State& st) {
  static bool fcl_gjk_indep_distance_only_setup = true;
  real distance;
  fcl::DistanceRequest<real> drequest;
  fcl::DistanceResult<real> dresult;
  drequest.gjk_solver_type = fcl::GST_INDEP;
  drequest.enable_nearest_points = false;

  auto rect_vv = std::make_shared<std::vector<cdl::Vector3r>>(rect_vertices);
  auto rect_ff = std::make_shared<std::vector<int>>();
  rect_ff->push_back(rect_vv->size() + 1);
  for (std::size_t i = 0; i < rect_vv->size(); ++i) {
    rect_ff->push_back(i);
  }
  auto rect_conv = std::make_shared<fcl::Convex<real>>(rect_vv, 1, rect_ff);

  auto panta_vv = std::make_shared<std::vector<cdl::Vector3r>>(panta_vertices);
  auto panta_ff = std::make_shared<std::vector<int>>();
  panta_ff->push_back(panta_vv->size() + 1);
  for (std::size_t i = 0; i < panta_vv->size(); ++i) {
    panta_ff->push_back(i);
  }
  auto panta_conv = std::make_shared<fcl::Convex<real>>(panta_vv, 1, panta_ff);

  using CO = fcl::CollisionObject<real>;

  std::unique_ptr<CO> obj1(new CO(rect_conv, rect_tf));
  std::unique_ptr<CO> obj2(new CO(panta_conv, panta_tf));

  for (auto _ : st) {
    dresult.clear();
    distance = fcl::distance<real>(obj1.get(), obj2.get(), drequest, dresult);
  }
  if (fcl_gjk_indep_distance_only_setup) {
    std::cout << "fcl independent gjk distance only:" << distance << std::endl;
    fcl_gjk_indep_distance_only_setup = false;
  }
}

BENCHMARK_DEFINE_F(NarrowPhaseFixture, fcl_gjk_indep_nearest_points_3d)(benchmark::State& st) {
  static bool fcl_gjk_indep_nearest_points_setup = true;
  real distance;
  fcl::DistanceRequest<real> drequest;
  fcl::DistanceResult<real> dresult;
  drequest.gjk_solver_type = fcl::GST_INDEP;
  drequest.enable_nearest_points = true;

  auto rect_vv = std::make_shared<std::vector<cdl::Vector3r>>(rect_vertices);
  auto rect_ff = std::make_shared<std::vector<int>>();
  rect_ff->push_back(rect_vv->size() + 1);
  for (std::size_t i = 0; i < rect_vv->size(); ++i) {
    rect_ff->push_back(i);
  }
  auto rect_conv = std::make_shared<fcl::Convex<real>>(rect_vv, 1, rect_ff);

  auto panta_vv = std::make_shared<std::vector<cdl::Vector3r>>(panta_vertices);
  auto panta_ff = std::make_shared<std::vector<int>>();
  panta_ff->push_back(panta_vv->size() + 1);
  for (std::size_t i = 0; i < panta_vv->size(); ++i) {
    panta_ff->push_back(i);
  }
  auto panta_conv = std::make_shared<fcl::Convex<real>>(panta_vv, 1, panta_ff);

  using CO = fcl::CollisionObject<real>;

  std::unique_ptr<CO> obj1(new CO(rect_conv, rect_tf));
  std::unique_ptr<CO> obj2(new CO(panta_conv, panta_tf));

  for (auto _ : st) {
    dresult.clear();
    distance = fcl::distance<real>(obj1.get(), obj2.get(), drequest, dresult);
  }

  if (fcl_gjk_indep_nearest_points_setup) {
    std::cout << "fcl independent gjk distance:" << distance << std::endl;
    fcl_gjk_indep_nearest_points_setup = false;
  }
}


BENCHMARK_DEFINE_F(NarrowPhaseFixture, fcl_gjk_ccd_distance_only_3d)(benchmark::State& st) {
  real distance;
  fcl::DistanceRequest<real> drequest;
  fcl::DistanceResult<real> dresult;
  drequest.gjk_solver_type = fcl::GST_LIBCCD;
  drequest.enable_nearest_points = false;

  auto rect_vv = std::make_shared<std::vector<cdl::Vector3r>>(rect_vertices);
  auto rect_ff = std::make_shared<std::vector<int>>();
  rect_ff->push_back(rect_vv->size() + 1);
  for (std::size_t i = 0; i < rect_vv->size(); ++i) {
    rect_ff->push_back(i);
  }
  auto rect_conv = std::make_shared<fcl::Convex<real>>(rect_vv, 1, rect_ff);

  auto panta_vv = std::make_shared<std::vector<cdl::Vector3r>>(panta_vertices);
  auto panta_ff = std::make_shared<std::vector<int>>();
  panta_ff->push_back(panta_vv->size() + 1);
  for (std::size_t i = 0; i < panta_vv->size(); ++i) {
    panta_ff->push_back(i);
  }
  auto panta_conv = std::make_shared<fcl::Convex<real>>(panta_vv, 1, panta_ff);

  using CO = fcl::CollisionObject<real>;

  std::unique_ptr<CO> obj1(new CO(rect_conv, rect_tf));
  std::unique_ptr<CO> obj2(new CO(panta_conv, panta_tf));

  for (auto _ : st) {
    dresult.clear();
    distance = fcl::distance<real>(obj1.get(), obj2.get(), drequest, dresult);
  }
}

BENCHMARK_DEFINE_F(NarrowPhaseFixture, fcl_gjk_ccd_nearest_points_3d)(benchmark::State& st) {
  real distance;
  fcl::DistanceRequest<real> drequest;
  fcl::DistanceResult<real> dresult;
  drequest.gjk_solver_type = fcl::GST_LIBCCD;
  drequest.enable_nearest_points = true;

  auto rect_vv = std::make_shared<std::vector<cdl::Vector3r>>(rect_vertices);
  auto rect_ff = std::make_shared<std::vector<int>>();
  rect_ff->push_back(rect_vv->size() + 1);
  for (std::size_t i = 0; i < rect_vv->size(); ++i) {
    rect_ff->push_back(i);
  }
  auto rect_conv = std::make_shared<fcl::Convex<real>>(rect_vv, 1, rect_ff);

  auto panta_vv = std::make_shared<std::vector<cdl::Vector3r>>(panta_vertices);
  auto panta_ff = std::make_shared<std::vector<int>>();
  panta_ff->push_back(panta_vv->size() + 1);
  for (std::size_t i = 0; i < panta_vv->size(); ++i) {
    panta_ff->push_back(i);
  }
  auto panta_conv = std::make_shared<fcl::Convex<real>>(panta_vv, 1, panta_ff);

  using CO = fcl::CollisionObject<real>;

  std::unique_ptr<CO> obj1(new CO(rect_conv, rect_tf));
  std::unique_ptr<CO> obj2(new CO(panta_conv, panta_tf));

  for (auto _ : st) {
    dresult.clear();
    distance = fcl::distance<real>(obj1.get(), obj2.get(), drequest, dresult);
  }
}