Add collision group filter for models

Signed-off-by: Jorge Perez <[email protected]>

bullet/src/Base.hh

@@ -124,12 +124,18 @@ inline Eigen::Vector3d convert(btVector3 vec)
 class Base : public Implements3d<FeatureList<Feature>>
 {
   public: std::size_t entityCount = 0;
+  public: std::size_t collisionGroup = 1;
 
   public: inline std::size_t GetNextEntity()
   {
     return entityCount++;
   }
 
+  public: inline std::size_t GetNextCollisionGroup()
+  {
+    return collisionGroup++;
+  }
+
   public: inline Identity InitiateEngine(std::size_t /*_engineID*/) override
   {
     const auto id = this->GetNextEntity();
@@ -150,6 +156,9 @@ class Base : public Implements3d<FeatureList<Feature>>
     const auto id = this->GetNextEntity();
     this->models[id] = std::make_shared<ModelInfo>(_modelInfo);
 
+    // A collision mask for each different model
+    this->collisionGroups[id] = 1 << GetNextCollisionGroup();
+
     return this->GenerateIdentity(id, this->models.at(id));
   }
 
@@ -190,6 +199,7 @@ class Base : public Implements3d<FeatureList<Feature>>
   public: std::unordered_map<std::size_t, LinkInfoPtr> links;
   public: std::unordered_map<std::size_t, CollisionInfoPtr> collisions;
   public: std::unordered_map<std::size_t, JointInfoPtr> joints;
+  public: std::unordered_map<std::size_t, std::size_t> collisionGroups;
 
   public: int internalTicksDivider = 0;
 

bullet/src/SDFFeatures.cc

@@ -115,7 +115,12 @@ Identity SDFFeatures::ConstructSdfLink(
   body->setActivationState(DISABLE_DEACTIVATION);
 
   const auto &world = this->worlds.at(modelInfo->world)->world;
-  world->addRigidBody(body);
+
+  // Models collide with everything except themselves
+  const int modelCollisionGroup = 1 << this->collisionGroups.at(_modelID);
+  const int collisionMask = 0xFFFFFFFF & ~modelCollisionGroup;
+  world->addRigidBody(body, modelCollisionGroup, collisionMask);
+  //world->addRigidBody(body);
 
   // Generate an identity for it
   const auto linkIdentity = this->AddLink({name, body, _modelID, pose, mass, linkInertiaDiag});