bullet_utils.h

Go to the documentation of this file.

 
#ifndef TESSERACT_COLLISION_BULLET_UTILS_H
#define TESSERACT_COLLISION_BULLET_UTILS_H
 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <BulletCollision/CollisionShapes/btCollisionShape.h>
#include <BulletCollision/CollisionDispatch/btManifoldResult.h>
#include <btBulletCollisionCommon.h>
#include <console_bridge/console.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_collision/core/types.h>
#include <tesseract_collision/core/common.h>
#include <tesseract_collision/bullet/bullet_collision_shape_cache.h>
 
namespace tesseract_collision::tesseract_collision_bullet
{
#define METERS
 
const btScalar BULLET_MARGIN = btScalar(0.0);
const btScalar BULLET_SUPPORT_FUNC_TOLERANCE = btScalar(0.01) METERS;
const btScalar BULLET_LENGTH_TOLERANCE = btScalar(0.001) METERS;
const btScalar BULLET_EPSILON = btScalar(1e-3);
const btScalar BULLET_DEFAULT_CONTACT_DISTANCE = btScalar(0.05);
const bool BULLET_COMPOUND_USE_DYNAMIC_AABB = true;
 
btVector3 convertEigenToBt(const Eigen::Vector3d& v);
 
Eigen::Vector3d convertBtToEigen(const btVector3& v);
 
btQuaternion convertEigenToBt(const Eigen::Quaterniond& q);
 
btMatrix3x3 convertEigenToBt(const Eigen::Matrix3d& r);
 
Eigen::Matrix3d convertBtToEigen(const btMatrix3x3& r);
 
btTransform convertEigenToBt(const Eigen::Isometry3d& t);
 
Eigen::Isometry3d convertBtToEigen(const btTransform& t);
 
class CollisionObjectWrapper : public btCollisionObject
{
public:
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  using Ptr = std::shared_ptr<CollisionObjectWrapper>;
  using ConstPtr = std::shared_ptr<const CollisionObjectWrapper>;
 
  CollisionObjectWrapper() = default;
  CollisionObjectWrapper(std::string name,
                         const int& type_id,
                         CollisionShapesConst shapes,
                         tesseract_common::VectorIsometry3d shape_poses);
 
  short int m_collisionFilterGroup{ btBroadphaseProxy::KinematicFilter };
  short int m_collisionFilterMask{ btBroadphaseProxy::StaticFilter | btBroadphaseProxy::KinematicFilter };
  bool m_enabled{ true };
 
  const std::string& getName() const;
  const int& getTypeID() const;
  bool sameObject(const CollisionObjectWrapper& other) const;
 
  const CollisionShapesConst& getCollisionGeometries() const;
 
  const tesseract_common::VectorIsometry3d& getCollisionGeometriesTransforms() const;
 
  void getAABB(btVector3& aabb_min, btVector3& aabb_max) const;
 
  std::shared_ptr<CollisionObjectWrapper> clone();
 
  void manage(const std::shared_ptr<BulletCollisionShape>& t);
 
  void manageReserve(std::size_t s);
 
protected:
  std::string m_name;
  int m_type_id{ -1 };
  /* @brief The shapes that define the collision object */
  CollisionShapesConst m_shapes;
  tesseract_common::VectorIsometry3d m_shape_poses;
  std::vector<std::shared_ptr<BulletCollisionShape>> m_data;
};
 
using COW = CollisionObjectWrapper;
using Link2Cow = std::map<std::string, COW::Ptr>;
using Link2ConstCow = std::map<std::string, COW::ConstPtr>;
 
struct CastHullShape : public btConvexShape
{
public:
  btConvexShape* m_shape;
  btTransform m_t01;
 
  CastHullShape(btConvexShape* shape, const btTransform& t01);
 
  void updateCastTransform(const btTransform& t01);
  btVector3 localGetSupportingVertex(const btVector3& vec) const override;
 
  void getAabb(const btTransform& t_w0, btVector3& aabbMin, btVector3& aabbMax) const override;
 
  const char* getName() const override;
  btVector3 localGetSupportingVertexWithoutMargin(const btVector3& v) const override;
 
  // LCOV_EXCL_START
  // notice that the vectors should be unit length
  void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,
                                                         btVector3* supportVerticesOut,
                                                         int numVectors) const override;
 
  void getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const override;
 
  void setLocalScaling(const btVector3& scaling) override;
 
  const btVector3& getLocalScaling() const override;
 
  void setMargin(btScalar margin) override;
 
  btScalar getMargin() const override;
 
  int getNumPreferredPenetrationDirections() const override;
 
  void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const override;
 
  void calculateLocalInertia(btScalar, btVector3&) const override;
  // LCOV_EXCL_STOP
};
 
void GetAverageSupport(const btConvexShape* shape,
                       const btVector3& localNormal,
                       btScalar& outsupport,
                       btVector3& outpt);
 
btTransform getLinkTransformFromCOW(const btCollisionObjectWrapper* cow);
 
bool needsCollisionCheck(const COW& cow1,
                         const COW& cow2,
                         const std::shared_ptr<const tesseract_common::ContactAllowedValidator>& validator,
                         bool verbose = false);
 
btScalar addDiscreteSingleResult(btManifoldPoint& cp,
                                 const btCollisionObjectWrapper* colObj0Wrap,
                                 const btCollisionObjectWrapper* colObj1Wrap,
                                 ContactTestData& collisions);
 
void calculateContinuousData(ContactResult* col,
                             const btCollisionObjectWrapper* cow,
                             const btVector3& pt_world,
                             const btVector3& normal_world,
                             const btTransform& link_tf_inv,
                             size_t link_index);
 
btScalar addCastSingleResult(btManifoldPoint& cp,
                             const btCollisionObjectWrapper* colObj0Wrap,
                             int index0,
                             const btCollisionObjectWrapper* colObj1Wrap,
                             int index1,
                             ContactTestData& collisions);
 
struct TesseractBridgedManifoldResult : public btManifoldResult
{
  btCollisionWorld::ContactResultCallback&
      m_resultCallback;  // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members)
 
  TesseractBridgedManifoldResult(const btCollisionObjectWrapper* obj0Wrap,
                                 const btCollisionObjectWrapper* obj1Wrap,
                                 btCollisionWorld::ContactResultCallback& resultCallback);
 
  void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) override;
};
 
struct BroadphaseContactResultCallback
{
  ContactTestData& collisions_;  // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members)
  double contact_distance_;
  bool verbose_;
 
  BroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, bool verbose = false);
 
  virtual ~BroadphaseContactResultCallback() = default;
  BroadphaseContactResultCallback(const BroadphaseContactResultCallback&) = default;
  BroadphaseContactResultCallback& operator=(const BroadphaseContactResultCallback&) = delete;
  BroadphaseContactResultCallback(BroadphaseContactResultCallback&&) = default;
  BroadphaseContactResultCallback& operator=(BroadphaseContactResultCallback&&) = delete;
 
  virtual bool needsCollision(const CollisionObjectWrapper* cow0, const CollisionObjectWrapper* cow1) const;
 
  virtual btScalar addSingleResult(btManifoldPoint& cp,
                                   const btCollisionObjectWrapper* colObj0Wrap,
                                   int partId0,
                                   int index0,
                                   const btCollisionObjectWrapper* colObj1Wrap,
                                   int partId1,
                                   int index1) = 0;
};
 
struct DiscreteBroadphaseContactResultCallback : public BroadphaseContactResultCallback
{
  DiscreteBroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, bool verbose = false);
 
  btScalar addSingleResult(btManifoldPoint& cp,
                           const btCollisionObjectWrapper* colObj0Wrap,
                           int partId0,
                           int index0,
                           const btCollisionObjectWrapper* colObj1Wrap,
                           int partId1,
                           int index1) override;
};
 
struct CastBroadphaseContactResultCallback : public BroadphaseContactResultCallback
{
  CastBroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, bool verbose = false);
 
  btScalar addSingleResult(btManifoldPoint& cp,
                           const btCollisionObjectWrapper* colObj0Wrap,
                           int partId0,
                           int index0,
                           const btCollisionObjectWrapper* colObj1Wrap,
                           int partId1,
                           int index1) override;
};
 
struct TesseractBroadphaseBridgedManifoldResult : public btManifoldResult
{
  BroadphaseContactResultCallback& result_callback_;  // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members)
 
  TesseractBroadphaseBridgedManifoldResult(const btCollisionObjectWrapper* obj0Wrap,
                                           const btCollisionObjectWrapper* obj1Wrap,
                                           BroadphaseContactResultCallback& result_callback);
 
  void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) override;
};
 
class TesseractCollisionPairCallback : public btOverlapCallback
{
  const btDispatcherInfo& dispatch_info_;  // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members)
  btCollisionDispatcher* dispatcher_;
  BroadphaseContactResultCallback& results_callback_;  // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members)
 
public:
  TesseractCollisionPairCallback(const btDispatcherInfo& dispatchInfo,
                                 btCollisionDispatcher* dispatcher,
                                 BroadphaseContactResultCallback& results_callback);
 
  ~TesseractCollisionPairCallback() override = default;
  TesseractCollisionPairCallback(const TesseractCollisionPairCallback&) = default;
  TesseractCollisionPairCallback& operator=(const TesseractCollisionPairCallback&) = delete;
  TesseractCollisionPairCallback(TesseractCollisionPairCallback&&) = default;
  TesseractCollisionPairCallback& operator=(TesseractCollisionPairCallback&&) = delete;
 
  bool processOverlap(btBroadphasePair& pair) override;
};
 
class TesseractOverlapFilterCallback : public btOverlapFilterCallback
{
public:
  TesseractOverlapFilterCallback(bool verbose = false);
 
  bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const override;
 
private:
  bool verbose_{ false };
};
 
std::shared_ptr<BulletCollisionShape> createShapePrimitive(const CollisionShapeConstPtr& geom);
 
void updateCollisionObjectFilters(const std::vector<std::string>& active, const COW::Ptr& cow);
 
COW::Ptr createCollisionObject(const std::string& name,
                               const int& type_id,
                               const CollisionShapesConst& shapes,
                               const tesseract_common::VectorIsometry3d& shape_poses,
                               bool enabled = true);
 
struct DiscreteCollisionCollector : public btCollisionWorld::ContactResultCallback
{
  ContactTestData& collisions_;  // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members)
  const COW::Ptr cow_;           // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members)
  double contact_distance_;
  bool verbose_;
 
  DiscreteCollisionCollector(ContactTestData& collisions,
                             COW::Ptr cow,
                             btScalar contact_distance,
                             bool verbose = false);
 
  btScalar addSingleResult(btManifoldPoint& cp,
                           const btCollisionObjectWrapper* colObj0Wrap,
                           int partId0,
                           int index0,
                           const btCollisionObjectWrapper* colObj1Wrap,
                           int partId1,
                           int index1) override;
 
  bool needsCollision(btBroadphaseProxy* proxy0) const override;
};
 
struct CastCollisionCollector : public btCollisionWorld::ContactResultCallback
{
  ContactTestData& collisions_;  // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members)
  const COW::Ptr cow_;           // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members)
  double contact_distance_;
  bool verbose_;
 
  CastCollisionCollector(ContactTestData& collisions, COW::Ptr cow, double contact_distance, bool verbose = false);
 
  btScalar addSingleResult(btManifoldPoint& cp,
                           const btCollisionObjectWrapper* colObj0Wrap,
                           int partId0,
                           int index0,
                           const btCollisionObjectWrapper* colObj1Wrap,
                           int partId1,
                           int index1) override;
 
  bool needsCollision(btBroadphaseProxy* proxy0) const override;
};
 
COW::Ptr makeCastCollisionObject(const COW::Ptr& cow);
 
void updateBroadphaseAABB(const COW::Ptr& cow,
                          const std::unique_ptr<btBroadphaseInterface>& broadphase,
                          const std::unique_ptr<btCollisionDispatcher>& dispatcher);
 
void removeCollisionObjectFromBroadphase(const COW::Ptr& cow,
                                         const std::unique_ptr<btBroadphaseInterface>& broadphase,
                                         const std::unique_ptr<btCollisionDispatcher>& dispatcher);
 
void addCollisionObjectToBroadphase(const COW::Ptr& cow,
                                    const std::unique_ptr<btBroadphaseInterface>& broadphase,
                                    const std::unique_ptr<btCollisionDispatcher>& dispatcher);
 
void updateCollisionObjectFilters(const std::vector<std::string>& active,
                                  const COW::Ptr& cow,
                                  const std::unique_ptr<btBroadphaseInterface>& broadphase,
                                  const std::unique_ptr<btCollisionDispatcher>& dispatcher);
 
void refreshBroadphaseProxy(const COW::Ptr& cow,
                            const std::unique_ptr<btBroadphaseInterface>& broadphase,
                            const std::unique_ptr<btCollisionDispatcher>& dispatcher);
}  // namespace tesseract_collision::tesseract_collision_bullet
#endif  // TESSERACT_COLLISION_BULLET_UTILS_H