Backport collide bitmask and category bitmask features (backport #804, #884, #885) (backport #900)

bullet-featherstone/src/Base.cc

@@ -25,24 +25,114 @@
 
 #include <utility>
 
+namespace
+{
+
+bool doCollide(uint32_t _categoryBitmask0, uint32_t _collideBitmask0,
+               uint32_t _categoryBitmask1, uint32_t _collideBitmask1)
+{
+  return (_categoryBitmask0 & _collideBitmask1) |
+         (_categoryBitmask1 & _collideBitmask0);
+}
+
+}  // namespace
+
 namespace gz {
 namespace physics {
 namespace bullet_featherstone {
 
+/////////////////////////////////////////////////
+bool GzCollisionFilterCallback::needBroadphaseCollision(
+    btBroadphaseProxy *_proxy0, btBroadphaseProxy *_proxy1) const
+{
+  GzMultiBodyLinkCollider *col0 =
+          static_cast<GzMultiBodyLinkCollider *>(
+          _proxy0->m_clientObject);
+  GzMultiBodyLinkCollider *col1 =
+          static_cast<GzMultiBodyLinkCollider *>(
+          _proxy1->m_clientObject);
+
+  if (col0 && col1)
+  {
+    // For backward compatibility, if category bitmask is not set, it
+    // defaults to the same value as collide bitmask.
+    uint32_t col0CategoryBitmask = col0->categoryBitmask.has_value() ?
+        col0->categoryBitmask.value() : col0->collideBitmask;
+    uint32_t col1CategoryBitmask = col1->categoryBitmask.has_value() ?
+        col1->categoryBitmask.value() : col1->collideBitmask;
+    // Early out if collide bitmask test fails
+    if (!doCollide(col0CategoryBitmask, col0->collideBitmask,
+        col1CategoryBitmask, col1->collideBitmask))
+    {
+      return false;
+    }
+  }
+
+  // Continue filtering collision based on logic in
+  // btOverlappingPairCache::needsBroadphaseCollision
+  bool collides = (_proxy0->m_collisionFilterGroup &
+                   _proxy1->m_collisionFilterMask) != 0;
+  collides = collides && (_proxy1->m_collisionFilterGroup &
+                          _proxy0->m_collisionFilterMask);
+  return collides;
+}
+
+/////////////////////////////////////////////////
+GzCollisionDispatcher::GzCollisionDispatcher(
+    btCollisionConfiguration *_collisionConfiguration)
+    : btCollisionDispatcher(_collisionConfiguration)
+{
+}
+
+/////////////////////////////////////////////////
+bool GzCollisionDispatcher::needsCollision(const btCollisionObject *_body0,
+    const btCollisionObject *_body1)
+{
+  const GzMultiBodyLinkCollider *col0 =
+          static_cast<const GzMultiBodyLinkCollider *>(_body0);
+  const GzMultiBodyLinkCollider *col1 =
+          static_cast<const GzMultiBodyLinkCollider *>(_body1);
+
+  // Collision filtering in narrow phase.
+  if (col0 && col1)
+  {
+    // For backward compatibility, if category bitmask is not set, it
+    // defaults to the same value as collide bitmask.
+    uint32_t col0CategoryBitmask = col0->categoryBitmask.has_value() ?
+        col0->categoryBitmask.value() : col0->collideBitmask;
+    uint32_t col1CategoryBitmask = col1->categoryBitmask.has_value() ?
+        col1->categoryBitmask.value() : col1->collideBitmask;
+    // Early out if collide bitmask test fails
+    if (!doCollide(col0CategoryBitmask, col0->collideBitmask,
+        col1CategoryBitmask, col1->collideBitmask))
+    {
+      return false;
+    }
+  }
+
+  return btCollisionDispatcher::needsCollision(_body0, _body1);
+}
+
 /////////////////////////////////////////////////
 WorldInfo::WorldInfo(std::string name_)
   : name(std::move(name_))
 {
   this->collisionConfiguration =
     std::make_unique<btDefaultCollisionConfiguration>();
   this->dispatcher =
-    std::make_unique<btCollisionDispatcher>(collisionConfiguration.get());
+    std::make_unique<GzCollisionDispatcher>(collisionConfiguration.get());
   this->broadphase = std::make_unique<btDbvtBroadphase>();
   this->solver = std::make_unique<btMultiBodyConstraintSolver>();
   this->world = std::make_unique<btMultiBodyDynamicsWorld>(
     dispatcher.get(), broadphase.get(), solver.get(),
     collisionConfiguration.get());
 
+  // Set custom collision filter callback for filtering based on
+  // surface contact parameters
+  this->collisionFilterCallback = std::make_unique<GzCollisionFilterCallback>();
+  btOverlappingPairCache* pairCache = this->world->getPairCache();
+  pairCache->setOverlapFilterCallback(this->collisionFilterCallback.get());
+
   btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher.get());
 
   // Needed for force-torque sensor

bullet-featherstone/src/Base.hh

@@ -37,6 +37,7 @@
 #include <Eigen/Geometry>
 
 #include <algorithm>
+#include <limits>
 #include <memory>
 #include <string>
 #include <unordered_map>
@@ -53,6 +54,29 @@ namespace gz {
 namespace physics {
 namespace bullet_featherstone {
 
+/// \brief Custom collision filter callback struct for handling
+/// collisions based on surface contact parameters
+struct GzCollisionFilterCallback : public btOverlapFilterCallback
+{
+  /// \brief Overrides base struct's function for additional collision
+  /// filtering based on surface contact parameters
+  bool needBroadphaseCollision(btBroadphaseProxy *_proxy0,
+      btBroadphaseProxy *_proxy1) const override;
+};
+
+/// \brief Custom gz collision dispatcher
+class GzCollisionDispatcher : public btCollisionDispatcher
+{
+  /// \brief Constructor
+  public: explicit GzCollisionDispatcher(
+              btCollisionConfiguration *_collisionConfiguration);
+
+  /// \brief Overrides base struct's function for additional collision
+  /// filtering based on surface contact parameters
+  public: bool needsCollision(const btCollisionObject *_body0,
+                              const btCollisionObject *_body1) override;
+};
+
 /// \brief The Info structs are used for three reasons:
 /// 1) Holding extra information such as the name
 ///    that will be different from the underlying engine
@@ -67,16 +91,18 @@ struct WorldInfo
 {
   std::string name;
   std::unique_ptr<btDefaultCollisionConfiguration> collisionConfiguration;
-  std::unique_ptr<btCollisionDispatcher> dispatcher;
+  std::unique_ptr<GzCollisionDispatcher> dispatcher;
   std::unique_ptr<btBroadphaseInterface> broadphase;
   std::unique_ptr<btMultiBodyConstraintSolver> solver;
   std::unique_ptr<btMultiBodyDynamicsWorld> world;
+  std::unique_ptr<GzCollisionFilterCallback> collisionFilterCallback;
 
   std::unordered_map<int, std::size_t> modelIndexToEntityId;
   std::unordered_map<std::string, std::size_t> modelNameToEntityId;
   int nextModelIndex = 0;
 
   double stepSize = 0.001;
+  bool collisionMasksDirty = false;
 
   explicit WorldInfo(std::string name);
 };
@@ -187,6 +213,12 @@ class GzMultiBodyLinkCollider: public btMultiBodyLinkCollider {
     return btMultiBodyLinkCollider::checkCollideWithOverride(_co) &&
            btCollisionObject::checkCollideWithOverride(_co);
   }
+
+  /// \brief Collision contact surface collide bitmask parameter
+  public: uint16_t collideBitmask = std::numeric_limits<uint16_t>::max();
+
+  /// \brief Collision contact surface category bitmask parameter
+  public: std::optional<uint16_t> categoryBitmask;
 };
 
 /// Link information is embedded inside the model, so all we need to store here

bullet-featherstone/src/EntityManagementFeatures.cc

@@ -17,6 +17,7 @@
 
 #include <btBulletDynamicsCommon.h>
 
+#include <limits>
 #include <memory>
 #include <string>
 #include <unordered_map>
@@ -194,6 +195,87 @@ Identity EntityManagementFeatures::GetLinkOfShape(
   return this->ReferenceInterface<CollisionInfo>(_shapeID)->link;
 }
 
+/////////////////////////////////////////////////
+void EntityManagementFeatures::SetCollisionFilterMask(
+    const Identity &_shapeID, uint16_t _mask)
+{
+  auto *colInfo = this->ReferenceInterface<CollisionInfo>(_shapeID);
+  auto *linkInfo = this->ReferenceInterface<LinkInfo>(colInfo->link);
+
+  if (_mask != linkInfo->collider->collideBitmask)
+  {
+    linkInfo->collider->collideBitmask = _mask;
+
+    // Mark dirty if new collision flags are set so that new contacts are
+    // generated with up-to-date collision flags.
+    auto *modelInfo = this->ReferenceInterface<ModelInfo>(linkInfo->model);
+    auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
+    world->collisionMasksDirty = true;
+  }
+}
+
+/////////////////////////////////////////////////
+uint16_t EntityManagementFeatures::GetCollisionFilterMask(
+    const Identity &_shapeID) const
+{
+  auto *colInfo = this->ReferenceInterface<CollisionInfo>(_shapeID);
+  auto *linkInfo = this->ReferenceInterface<LinkInfo>(colInfo->link);
+  return linkInfo->collider->collideBitmask;
+}
+
+/////////////////////////////////////////////////
+void EntityManagementFeatures::RemoveCollisionFilterMask(
+    const Identity &_shapeID)
+{
+  // Reset to default value
+  this->SetCollisionFilterMask(_shapeID, std::numeric_limits<uint16_t>::max());
+}
+
+/////////////////////////////////////////////////
+void EntityManagementFeatures::SetCategoryFilterMask(
+    const Identity &_shapeID, uint16_t _mask)
+{
+  auto *colInfo = this->ReferenceInterface<CollisionInfo>(_shapeID);
+  auto *linkInfo = this->ReferenceInterface<LinkInfo>(colInfo->link);
+
+  if (_mask != linkInfo->collider->categoryBitmask)
+  {
+    linkInfo->collider->categoryBitmask = _mask;
+
+    // Mark dirty if new collision flags are set so that new contacts are
+    // generated with up-to-date collision flags.
+    auto *modelInfo = this->ReferenceInterface<ModelInfo>(linkInfo->model);
+    auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
+    world->collisionMasksDirty = true;
+  }
+}
+
+/////////////////////////////////////////////////
+uint16_t EntityManagementFeatures::GetCategoryFilterMask(
+    const Identity &_shapeID) const
+{
+  auto *colInfo = this->ReferenceInterface<CollisionInfo>(_shapeID);
+  auto *linkInfo = this->ReferenceInterface<LinkInfo>(colInfo->link);
+  // For backward compatibility, if category bitmask is not set, it
+  // defaults to the same value as collide bitmask.
+  return linkInfo->collider->categoryBitmask.has_value() ?
+      linkInfo->collider->categoryBitmask.value() :
+      linkInfo->collider->collideBitmask;
+}
+
+/////////////////////////////////////////////////
+void EntityManagementFeatures::RemoveCategoryFilterMask(
+    const Identity &_shapeID)
+{
+  auto *colInfo = this->ReferenceInterface<CollisionInfo>(_shapeID);
+  auto *linkInfo = this->ReferenceInterface<LinkInfo>(colInfo->link);
+  linkInfo->collider->categoryBitmask.reset();
+
+  auto *modelInfo = this->ReferenceInterface<ModelInfo>(linkInfo->model);
+  auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
+  world->collisionMasksDirty = true;
+}
+
 /////////////////////////////////////////////////
 Identity EntityManagementFeatures::ConstructEmptyWorld(
     const Identity &/*_engineID*/, const std::string &_name)

bullet-featherstone/src/EntityManagementFeatures.hh

@@ -24,6 +24,7 @@
 #include <gz/physics/GetEntities.hh>
 #include <gz/physics/Implements.hh>
 #include <gz/physics/RemoveEntities.hh>
+#include <gz/physics/Shape.hh>
 
 #include "Base.hh"
 
@@ -32,6 +33,8 @@ namespace physics {
 namespace bullet_featherstone {
 
 struct EntityManagementFeatureList : gz::physics::FeatureList<
+  CategoryFilterMaskFeature,
+  CollisionFilterMaskFeature,
   ConstructEmptyWorldFeature,
   GetEngineInfo,
   GetJointFromModel,
@@ -163,6 +166,24 @@ class EntityManagementFeatures :
   public: bool RemoveNestedModelByName(
       const Identity &_modelID, const std::string &_modelName) override;
 
+  // ----- Manage collision filter masks -----
+  public: void SetCollisionFilterMask(
+      const Identity &_shapeID, uint16_t _mask) override;
+
+  public: uint16_t GetCollisionFilterMask(
+      const Identity &_shapeID) const override;
+
+  public: void RemoveCollisionFilterMask(const Identity &_shapeID) override;
+
+  // ----- Manage category filter masks -----
+  public: void SetCategoryFilterMask(
+      const Identity &_shapeID, uint16_t _mask) override;
+
+  public: uint16_t GetCategoryFilterMask(
+      const Identity &_shapeID) const override;
+
+  public: void RemoveCategoryFilterMask(const Identity &_shapeID) override;
+
   // ----- Construct empty entities -----
   public: Identity ConstructEmptyWorld(
       const Identity &_engineID, const std::string & _name) override;

bullet-featherstone/src/SDFFeatures.cc

@@ -43,6 +43,7 @@
 
 #include <LinearMath/btQuaternion.h>
 
+#include <limits>
 #include <memory>
 #include <unordered_map>
 #include <utility>
@@ -1257,6 +1258,8 @@ bool SDFFeatures::AddSdfCollision(
   double restitution = 0.0;
   double torsionalCoefficient = 1.0;
   double rollingFriction = 0.0;
+  uint16_t collideBitmask = std::numeric_limits<uint16_t>::max();
+  std::optional<uint16_t> categoryBitmask;
   if (const auto *surface = _collision.Surface())
   {
     if (const auto *friction = surface->Friction())
@@ -1291,6 +1294,19 @@ bool SDFFeatures::AddSdfCollision(
         if (const auto r = bounce->FindElement("restitution_coefficient"))
           restitution = r->Get<double>();
       }
+
+      if (const auto contact = surfaceElement->FindElement("contact"))
+      {
+        if (const auto bitmask = contact->FindElement("collide_bitmask"))
+        {
+          // Get only supports uint32_t so cast back to uint16_t
+          collideBitmask = static_cast<uint16_t>(bitmask->Get<uint32_t>());
+        }
+        if (const auto bitmask = contact->FindElement("category_bitmask"))
+        {
+          categoryBitmask = static_cast<uint16_t>(bitmask->Get<uint32_t>());
+        }
+      }
     }
   }
 
@@ -1322,7 +1338,9 @@ bool SDFFeatures::AddSdfCollision(
 
     if (!linkInfo->collider)
     {
-      this->CreateLinkCollider(_linkID, _isStatic, shape.get(),
+
+      this->CreateLinkCollider(_linkID, _isStatic, collideBitmask,
+          categoryBitmask, shape.get(),
           btInertialToCollision);
 
       linkInfo->collider->setRestitution(static_cast<btScalar>(restitution));
@@ -1472,6 +1490,7 @@ Identity SDFFeatures::ConstructSdfJoint(
 
 /////////////////////////////////////////////////
 void SDFFeatures::CreateLinkCollider(const Identity &_linkID, bool _isStatic,
+    uint16_t _collideBitmask, std::optional<uint16_t> _categoryBitmask,
     btCollisionShape *_shape, const btTransform &_shapeTF)
 {
   auto *linkInfo = this->ReferenceInterface<LinkInfo>(_linkID);
@@ -1526,6 +1545,16 @@ void SDFFeatures::CreateLinkCollider(const Identity &_linkID, bool _isStatic,
       isFixed = totalLinkDofs == 0;
     }
   }
+
+  // Set the collideBimask variable in the GzMultiBodyLinkCollider class
+  // instead of calling setCollisionFlags so we don't override bullet's
+  // internal collision flags which are used to indicate whether a collision
+  // is static, dynamic, kinematic, etc
+  // Set these masks before calling addCollisionObject so that
+  // the masks are available during the needBroadPhaseCollision check
+  linkInfo->collider->collideBitmask = _collideBitmask;
+  linkInfo->collider->categoryBitmask = _categoryBitmask;
+
   if (_isStatic || isFixed)
   {
     worldInfo->world->addCollisionObject(