From 55dd0f97ae6165fa59e6f5bfbab8783654b04654 Mon Sep 17 00:00:00 2001
From: Jorge Perez <jjperez@ekumenlabs.com>
Date: Mon, 8 Feb 2021 00:45:43 -0500
Subject: [PATCH] Add collision group filter for models

Signed-off-by: Jorge Perez <jjperez@ekumenlabs.com>
---
 bullet/src/Base.hh        | 10 ++++++++++
 bullet/src/SDFFeatures.cc |  7 ++++++-
 2 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/bullet/src/Base.hh b/bullet/src/Base.hh
index 436976c32..ca7a6bfa7 100644
--- a/bullet/src/Base.hh
+++ b/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;
 
diff --git a/bullet/src/SDFFeatures.cc b/bullet/src/SDFFeatures.cc
index 1c607fa3e..d10faec95 100644
--- a/bullet/src/SDFFeatures.cc
+++ b/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});