FCL_distance_tests.cpp

#define BOOST_TEST_MODULE "FCL_SHAPE_MESH_CONSISTENCY"
#include <boost/test/unit_test.hpp>

#include "fcl/narrowphase/narrowphase.h"
#include "fcl/shape/geometric_shape_to_BVH_model.h"
#include "fcl/distance.h"
#include "fcl/collision.h"
#include "test_fcl_utility.h"

using namespace fcl;

BOOST_AUTO_TEST_CASE(distance_tests)
{
  Sphere s1(10);
  Sphere s2(40);
  BVHModel<RSS> s1_rss;
  BVHModel<RSS> s2_rss;

  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);

  DistanceRequest request;
  request.enable_nearest_points = true;
  DistanceResult res, res1;

  Transform3f pose;
  FCL_REAL dist;

  res.clear(); res1.clear();

  pose.setTranslation(Vec3f(60, 0, 0));
  res.clear();
  dist = distance(&s1_rss, pose, &s2_rss, Transform3f(), request, res);

  pose.setTranslation(Vec3f(10, 0, 0));
  res.clear();
  dist = distance(&s1_rss, pose, &s2_rss, Transform3f(), request, res);

  pose.setTranslation(Vec3f(60, 0, 0));
  res.clear();
  dist = distance(&s1, pose, &s2, Transform3f(), request, res);

  pose.setTranslation(Vec3f(10, 0, 0));
  res.clear();
  dist = distance(&s1, pose, &s2, Transform3f(), request, res);
}