[Citadel][TPE] Implement collision filtering using collide bitmasks and add CollisionFilterMaskFeature

tpe/lib/src/Collision.cc

@@ -24,6 +24,9 @@ class ignition::physics::tpelib::CollisionPrivate
 {
   /// \brief Collision's geometry shape
   public: std::shared_ptr<Shape> shape = nullptr;
+
+  /// \brief Collide bitmask
+  public: uint16_t collideBitmask = 0xFF;
 };
 
 using namespace ignition;
@@ -107,3 +110,17 @@ math::AxisAlignedBox Collision::GetBoundingBox(bool /*_force*/) // NOLINT
     return this->dataPtr->shape->GetBoundingBox();
   return math::AxisAlignedBox();
 }
+
+//////////////////////////////////////////////////
+void Collision::SetCollideBitmask(uint16_t _mask)
+{
+  this->dataPtr->collideBitmask = _mask;
+  if (this->GetParent())
+    this->GetParent()->ChildrenChanged();
+}
+
+//////////////////////////////////////////////////
+uint16_t Collision::GetCollideBitmask() const
+{
+  return this->dataPtr->collideBitmask;
+}

tpe/lib/src/Collision.hh

@@ -62,6 +62,13 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Collision : public Entity
   /// \return shape of collision
   public: Shape *GetShape() const;
 
+  /// \brief Set collide bitmask
+  /// \param[in] _mask Bitmask to set
+  public: void SetCollideBitmask(uint16_t _mask);
+
+  // Documentation Inherited
+  public: uint16_t GetCollideBitmask() const override;
+
   // Documentation inherited
   public: math::AxisAlignedBox GetBoundingBox(bool _force) override;
 

tpe/lib/src/CollisionDetector.cc

@@ -15,6 +15,8 @@
  *
 */
 
+#include <unordered_map>
+
 #include "CollisionDetector.hh"
 #include "Utils.hh"
 
@@ -32,13 +34,17 @@ std::vector<Contact> CollisionDetector::CheckCollisions(
   std::vector<Contact> contacts;
 
   // cache of axis aligned box in world frame
-  std::map<std::size_t, math::AxisAlignedBox> worldAabb;
+  std::unordered_map<std::size_t, math::AxisAlignedBox> worldAabb;
 
   for (auto it = _entities.begin(); it != _entities.end(); ++it)
   {
+    std::shared_ptr<Entity> e1 = it->second;
+
+    // Get collide bitmask for enitty 1
+    uint16_t cb1 = e1->GetCollideBitmask();
+
     // Get world axis aligned box for entity 1
     math::AxisAlignedBox wb1;
-    std::shared_ptr<Entity> e1 = it->second;
     auto wb1It = worldAabb.find(e1->GetId());
     if (wb1It == worldAabb.end())
     {
@@ -56,9 +62,17 @@ std::vector<Contact> CollisionDetector::CheckCollisions(
 
     for (auto it2 = std::next(it, 1); it2 != _entities.end(); ++it2)
     {
+      std::shared_ptr<Entity> e2 = it2->second;
+
+      // Get collide bitmask for entity 2
+      uint16_t cb2 = e2->GetCollideBitmask();
+
+      // collision filtering using collide bitmask
+      if ((cb1 & cb2) == 0)
+        continue;
+
       // Get world axis aligned box for entity 2
       math::AxisAlignedBox wb2;
-      std::shared_ptr<Entity> e2 = it2->second;
       auto wb2It = worldAabb.find(e2->GetId());
       if (wb2It == worldAabb.end())
       {

tpe/lib/src/Collision_TEST.cc

@@ -37,6 +37,10 @@ TEST(Collision, BasicAPI)
   collision.SetPose(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3));
   EXPECT_EQ(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3), collision.GetPose());
 
+  EXPECT_EQ(0xFF, collision.GetCollideBitmask());
+  collision.SetCollideBitmask(0x03);
+  EXPECT_EQ(0x03, collision.GetCollideBitmask());
+
   Collision collision2;
   EXPECT_NE(collision.GetId(), collision2.GetId());
 }

tpe/lib/src/Entity.cc

@@ -36,8 +36,17 @@ class ignition::physics::tpelib::EntityPrivate
   /// \brief Bounding Box
   public: math::AxisAlignedBox bbox;
 
+  /// \brief Collide bitmask
+  public: uint16_t collideBitmask = 0xFF;
+
   /// \brief Flag to indicate if bounding box changed
   public: bool bboxDirty = true;
+
+  /// \brief Flag to indicate if collide bitmask changed
+  public: bool collideBitmaskDirty = true;
+
+  /// \brief Parent of this entity
+  public: Entity *parent = nullptr;
 };
 
 using namespace ignition;
@@ -63,6 +72,7 @@ Entity::Entity(const Entity &_other)
   this->dataPtr->pose = _other.dataPtr->pose;
   this->dataPtr->children = _other.dataPtr->children;
   this->dataPtr->bbox = _other.dataPtr->bbox;
+  this->dataPtr->collideBitmask = _other.dataPtr->collideBitmask;
 }
 
 //////////////////////////////////////////////////
@@ -187,6 +197,7 @@ bool Entity::RemoveChildById(std::size_t _id)
   if (it != this->dataPtr->children.end())
   {
     this->dataPtr->children.erase(it);
+    this->ChildrenChanged();
     return true;
   }
 
@@ -202,6 +213,7 @@ bool Entity::RemoveChildByName(const std::string &_name)
     if (it->second->GetName() == _name)
     {
       this->dataPtr->children.erase(it);
+      this->ChildrenChanged();
       return true;
     }
   }
@@ -240,6 +252,23 @@ void Entity::UpdateBoundingBox(bool _force)
   this->dataPtr->bbox = box;
 }
 
+//////////////////////////////////////////////////
+uint16_t Entity::GetCollideBitmask() const
+{
+  if (this->dataPtr->collideBitmaskDirty)
+  {
+    uint16_t mask = 0u;
+    for (auto &it : this->dataPtr->children)
+    {
+      mask |= it.second->GetCollideBitmask();
+    }
+    this->dataPtr->collideBitmask = mask;
+    this->dataPtr->collideBitmaskDirty = false;
+  }
+
+  return this->dataPtr->collideBitmask;
+}
+
 //////////////////////////////////////////////////
 std::map<std::size_t, std::shared_ptr<Entity>> &Entity::GetChildren() const
 {
@@ -251,3 +280,25 @@ std::size_t Entity::GetNextId()
 {
   return nextId++;
 }
+
+//////////////////////////////////////////////////
+void Entity::ChildrenChanged()
+{
+  this->dataPtr->bboxDirty = true;
+  this->dataPtr->collideBitmaskDirty= true;
+
+  if (this->dataPtr->parent)
+    this->dataPtr->parent->ChildrenChanged();
+}
+
+//////////////////////////////////////////////////
+void Entity::SetParent(Entity *_parent)
+{
+  this->dataPtr->parent = _parent;
+}
+
+//////////////////////////////////////////////////
+Entity *Entity::GetParent() const
+{
+  return this->dataPtr->parent;
+}

tpe/lib/src/Entity.hh

@@ -131,6 +131,25 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Entity
   /// \return Entity bounding box
   public: virtual math::AxisAlignedBox GetBoundingBox(bool _force = false);
 
+  /// \brief Get collide bitmask
+  /// \return Collision's collide bitmask
+  public: virtual uint16_t GetCollideBitmask() const;
+
+  /// \internal
+  /// \brief Set the parent of this entity.
+  /// \param[in] _parent Parent to set to
+  public: void SetParent(Entity *_parent);
+
+  /// \internal
+  /// \brief Get the parent of this entity.
+  /// \return Parent of this entity
+  public: Entity *GetParent() const;
+
+  /// \internal
+  /// \brief Mark that the children of the entity has changed, e.g. a child
+  /// entity is added or removed, or child entity properties changed.
+  public: void ChildrenChanged();
+
   /// \brief Get number of children
   /// \return Map of child id's to child entities
   protected: std::map<std::size_t, std::shared_ptr<Entity>> &GetChildren()

tpe/lib/src/Link.cc

@@ -38,5 +38,7 @@ Entity &Link::AddCollision()
   std::size_t collisionId = Entity::GetNextId();
   const auto[it, success] = this->GetChildren().insert(
     {collisionId, std::make_shared<Collision>(collisionId)});
+  it->second->SetParent(this);
+  this->ChildrenChanged();
   return *it->second.get();
 }

tpe/lib/src/Model.cc

@@ -43,6 +43,9 @@ Entity &Model::AddLink()
   std::size_t linkId = Entity::GetNextId();
   const auto[it, success]  = this->GetChildren().insert(
       {linkId, std::make_shared<Link>(linkId)});
+
+  it->second->SetParent(this);
+  this->ChildrenChanged();
   return *it->second.get();
 }
 

tpe/lib/src/Model_TEST.cc

@@ -133,11 +133,11 @@ TEST(Model, BoundingBox)
 
   math::AxisAlignedBox expectedBoxLinkFrame(
       math::Vector3d(-2, -2, -2), math::Vector3d(2, 2, 2));
-  EXPECT_EQ(expectedBoxLinkFrame, linkEnt.GetBoundingBox(true));
+  EXPECT_EQ(expectedBoxLinkFrame, linkEnt.GetBoundingBox());
 
   math::AxisAlignedBox expectedBoxModelFrame(
       math::Vector3d(-2, -2, -1), math::Vector3d(2, 2, 3));
-  EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox(true));
+  EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox());
 
   // add another link with box collision shape
   Entity &linkEnt2 = model.AddLink();
@@ -154,9 +154,48 @@ TEST(Model, BoundingBox)
 
   expectedBoxLinkFrame = math::AxisAlignedBox(
       math::Vector3d(-1.5, -2, -2.5), math::Vector3d(1.5, 2, 2.5));
-  EXPECT_EQ(expectedBoxLinkFrame, linkEnt2.GetBoundingBox(true));
+  EXPECT_EQ(expectedBoxLinkFrame, linkEnt2.GetBoundingBox());
 
   expectedBoxModelFrame = math::AxisAlignedBox(
       math::Vector3d(-2, -2, -2.5), math::Vector3d(2, 3, 3));
-  EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox(true));
+  EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox());
+}
+
+/////////////////////////////////////////////////
+TEST(Model, CollideBitmask)
+{
+  Model model;
+  EXPECT_EQ(0x00, model.GetCollideBitmask());
+
+  // add link and verify bitmask is still empty
+  Entity &linkEnt = model.AddLink();
+  EXPECT_EQ(0x00, linkEnt.GetCollideBitmask());
+  EXPECT_EQ(0x00, model.GetCollideBitmask());
+
+  // add a collision and verify the model has the same collision bitmask
+  Link *link = static_cast<Link *>(&linkEnt);
+  Entity &collisionEnt = link->AddCollision();
+  Collision *collision = static_cast<Collision *>(&collisionEnt);
+  collision->SetCollideBitmask(0x01);
+  EXPECT_EQ(0x01, collision->GetCollideBitmask());
+  EXPECT_EQ(0x01, linkEnt.GetCollideBitmask());
+  EXPECT_EQ(0x01, model.GetCollideBitmask());
+
+  // add another collision and verify bitmasks are bitwise OR'd.
+  Entity &collisionEnt2 = link->AddCollision();
+  Collision *collision2 = static_cast<Collision *>(&collisionEnt2);
+  collision2->SetCollideBitmask(0x04);
+  EXPECT_EQ(0x04, collision2->GetCollideBitmask());
+  EXPECT_EQ(0x05, linkEnt.GetCollideBitmask());
+  EXPECT_EQ(0x05, model.GetCollideBitmask());
+
+  // add another link with collision and verify
+  Entity &linkEnt2 = model.AddLink();
+  Link *link2 = static_cast<Link *>(&linkEnt2);
+  Entity &collisionEnt3 = link2->AddCollision();
+  Collision *collision3 = static_cast<Collision *>(&collisionEnt3);
+  collision3->SetCollideBitmask(0x09);
+  EXPECT_EQ(0x09, collision3->GetCollideBitmask());
+  EXPECT_EQ(0x09, linkEnt2.GetCollideBitmask());
+  EXPECT_EQ(0x0D, model.GetCollideBitmask());
 }

tpe/plugin/src/EntityManagementFeatures.cc

@@ -420,3 +420,31 @@ Identity EntityManagementFeatures::ConstructEmptyLink(
   }
   return this->GenerateInvalidId();
 }
+
+/////////////////////////////////////////////////
+void EntityManagementFeatures::SetCollisionFilterMask(
+    const Identity &_shapeID, const uint16_t _mask)
+{
+  auto collision =
+      this->ReferenceInterface<CollisionInfo>(_shapeID)->collision;
+  collision->SetCollideBitmask(_mask);
+}
+
+/////////////////////////////////////////////////
+uint16_t EntityManagementFeatures::GetCollisionFilterMask(
+    const Identity &_shapeID) const
+{
+  const auto collision =
+      this->ReferenceInterface<CollisionInfo>(_shapeID)->collision;
+  return collision->GetCollideBitmask();
+}
+
+/////////////////////////////////////////////////
+void EntityManagementFeatures::RemoveCollisionFilterMask(
+    const Identity &_shapeID)
+{
+  auto collision =
+      this->ReferenceInterface<CollisionInfo>(_shapeID)->collision;
+  // remove = reset to default bitmask
+  collision->SetCollideBitmask(0xFF);
+}

tpe/plugin/src/EntityManagementFeatures.hh

@@ -21,6 +21,7 @@
 #include <string>
 
 #include <ignition/physics/ConstructEmpty.hh>
+#include <ignition/physics/Shape.hh>
 #include <ignition/physics/GetEntities.hh>
 #include <ignition/physics/RemoveEntities.hh>
 #include <ignition/physics/Implements.hh>
@@ -40,7 +41,8 @@ struct EntityManagementFeatureList : FeatureList<
   RemoveEntities,
   ConstructEmptyWorldFeature,
   ConstructEmptyModelFeature,
-  ConstructEmptyLinkFeature
+  ConstructEmptyLinkFeature,
+  CollisionFilterMaskFeature
 > { };
 
 class EntityManagementFeatures :
@@ -135,6 +137,15 @@ class EntityManagementFeatures :
 
   public: Identity ConstructEmptyLink(
     const Identity &_modelID, const std::string &_name) override;
+
+  // ----- Manage collision filter masks -----
+  public: void SetCollisionFilterMask(
+      const Identity &_shapeID, const uint16_t _mask) override;
+
+  public: uint16_t GetCollisionFilterMask(
+      const Identity &_shapeID) const override;
+
+  public: void RemoveCollisionFilterMask(const Identity &_shapeID) override;
 };
 
 }

tpe/plugin/src/SDFFeatures.cc

@@ -168,5 +168,27 @@ Identity SDFFeatures::ConstructSdfCollision(
   // and passed in as argument as there is no logic for searching resources
   // in ign-physics
   const auto collisionIdentity = this->AddCollision(link->GetId(), *collision);
+
+  // set collide bitmask
+  uint16_t collideBitmask = 0xFF;
+  if (_sdfCollision.Element())
+  {
+    // TODO(anyone) add category_bitmask as well
+    auto elem = _sdfCollision.Element();
+    if (elem->HasElement("surface"))
+    {
+      elem = elem->GetElement("surface");
+      if (elem->HasElement("contact"))
+      {
+        elem = elem->GetElement("contact");
+        if (elem->HasElement("collide_bitmask"))
+        {
+          collideBitmask = elem->Get<unsigned int>("collide_bitmask");
+          this->SetCollisionFilterMask(collisionIdentity, collideBitmask);
+        }
+      }
+    }
+  }
+
   return collisionIdentity;
 }