fcl_collision_object_wrapper.cpp

Go to the documentation of this file.

 
#include <tesseract_collision/fcl/fcl_collision_object_wrapper.h>
 
namespace tesseract_collision::tesseract_collision_fcl
{
void FCLCollisionObjectWrapper::setContactDistanceThreshold(double contact_distance)
{
  contact_distance_ = contact_distance;
  updateAABB();
}
 
double FCLCollisionObjectWrapper::getContactDistanceThreshold() const { return contact_distance_; }
 
void FCLCollisionObjectWrapper::updateAABB()
{
  if (t.linear().isIdentity())
  {
    aabb = translate(cgeom->aabb_local, t.translation());
    fcl::Vector3<double> delta = fcl::Vector3<double>::Constant(contact_distance_);
    aabb.min_ -= delta;
    aabb.max_ += delta;
  }
  else
  {
    fcl::Vector3<double> center = t * cgeom->aabb_center;
    fcl::Vector3<double> delta = fcl::Vector3<double>::Constant(cgeom->aabb_radius + contact_distance_);
    aabb.min_ = center - delta;
    aabb.max_ = center + delta;
  }
}
 
void FCLCollisionObjectWrapper::setShapeIndex(int index) { shape_index_ = index; }
 
int FCLCollisionObjectWrapper::getShapeIndex() const
{
  assert(shape_index_ >= 0);
  return shape_index_;
}
 
}  // namespace tesseract_collision::tesseract_collision_fcl