bullet-featherstone: Use a single contact manifold for each convex decomposed mesh collision

bullet-featherstone/src/Base.cc

@@ -23,14 +23,213 @@ namespace gz {
 namespace physics {
 namespace bullet_featherstone {
 
+/////////////////////////////////////////////////
+GzCollisionDispatcher::GzCollisionDispatcher(
+    btCollisionConfiguration *_collisionConfiguration)
+    : btCollisionDispatcher(_collisionConfiguration)
+{
+}
+
+/////////////////////////////////////////////////
+GzCollisionDispatcher::~GzCollisionDispatcher()
+{
+  for (auto& manifold : this->customManifolds)
+  {
+    btCollisionDispatcher::releaseManifold(manifold);
+  }
+
+  this->customManifolds.clear();
+  this->colPairManifolds.clear();
+}
+
+/////////////////////////////////////////////////
+void GzCollisionDispatcher::RemoveManifoldByCollisionObject(
+    btCollisionObject *_colObj)
+{
+  std::unordered_set<btPersistentManifold *> manifoldsToRemove;
+  for (const auto& manifold : this->customManifolds)
+  {
+    if (manifold->getBody0() == _colObj ||
+        manifold->getBody1() == _colObj)
+    {
+      manifoldsToRemove.insert(manifold);
+    }
+  }
+
+  for (auto& manifold : manifoldsToRemove)
+  {
+    btCollisionDispatcher::releaseManifold(manifold);
+    this->customManifolds.erase(manifold);
+  }
+}
+
+/////////////////////////////////////////////////
+bool GzCollisionDispatcher::HasConvexHullChildShapes(
+    const btCollisionShape *_shape)
+{
+  if (!_shape || !_shape->isCompound())
+    return false;
+
+  const btCompoundShape *compoundShape =
+      static_cast<const btCompoundShape *>(_shape);
+  return (compoundShape->getNumChildShapes() > 0 &&
+      compoundShape->getChildShape(0)->getShapeType() ==
+      CONVEX_HULL_SHAPE_PROXYTYPE);
+}
+
+/////////////////////////////////////////////////
+const btCollisionShape *GzCollisionDispatcher::FindCollisionShape(
+    const btCompoundShape *_compoundShape, int _childIndex)
+{
+  // _childIndex should give us the index of the child shape within
+  // _compoundShape which represents the collision.
+  // One exception is when the collision is a convex decomposed mesh.
+  // In this case, the child shape is another btCompoundShape (nested), and
+  // _childIndex is the index of one of the decomposed convex hulls
+  // in the nested compound shape. The nested compound shape is the collision.
+  int childCount = _compoundShape->getNumChildShapes();
+  if (childCount > 0)
+  {
+    if (_childIndex >= 0 && _childIndex < childCount)
+    {
+      // todo(iche033) We do not have sufficient info to determine which
+      // child shape is the collision if the link has convex decomposed mesh
+      // collisions alongside of other collisions. See following example:
+      // parentLink -> boxShape0
+      //            -> boxShape1
+      //            -> comopundShape -> convexShape0
+      //                             -> convexShape1
+      // A _childIndex of 1 is ambiguous as it could refer to either
+      // boxShape1 or convexShape1
+      // return nullptr in this case to indicate ambiguity
+      if (childCount > 1)
+      {
+        for (int i = 0; i < childCount; ++i)
+        {
+          const btCollisionShape *shape = _compoundShape->getChildShape(i);
+          if (this->HasConvexHullChildShapes(shape))
+            return nullptr;
+        }
+      }
+
+      const btCollisionShape *shape =
+          _compoundShape->getChildShape(_childIndex);
+      return shape;
+    }
+    else
+    {
+      return _compoundShape->getChildShape(0);
+    }
+  }
+  return nullptr;
+}
+
+/////////////////////////////////////////////////
+void GzCollisionDispatcher::dispatchAllCollisionPairs(
+    btOverlappingPairCache* pairCache,
+    const btDispatcherInfo& dispatchInfo,
+    btDispatcher* dispatcher)
+{
+  btCollisionDispatcher::dispatchAllCollisionPairs(
+      pairCache, dispatchInfo, dispatcher);
+
+  // Loop through all the contact manifolds.
+  // Find convex decomposed mesh collision shapes.
+  // Create a shared contact manifold for all decomposed shapes.
+  // This is so that we can limit the number of contact points to 4
+  // for the collision shape. Otherwise it will generate up to
+  // (decomposed col count * 4) contact points.
+  int numManifolds = this->getNumManifolds();
+  for (int i = 0; i < numManifolds; ++i)
+  {
+    btPersistentManifold* contactManifold =
+        this->getManifoldByIndexInternal(i);
+
+    const btMultiBodyLinkCollider* ob0 =
+        dynamic_cast<const btMultiBodyLinkCollider *>(
+        contactManifold->getBody0());
+    const btMultiBodyLinkCollider* ob1 =
+        dynamic_cast<const btMultiBodyLinkCollider *>(
+        contactManifold->getBody1());
+
+    // if it's a custom manifold, just refresh contacts, no need to
+    // loop through and check them.
+    if (this->customManifolds.find(contactManifold) !=
+        this->customManifolds.end())
+    {
+      contactManifold->refreshContactPoints(ob0->getWorldTransform(),
+                                            ob1->getWorldTransform());
+      continue;
+    }
+
+    if (this->manifoldsToKeep.find(contactManifold) !=
+        this->manifoldsToKeep.end())
+    {
+      continue;
+    }
+
+    const btCompoundShape *compoundShape0 =
+        static_cast<const btCompoundShape *>(ob0->getCollisionShape());
+    const btCompoundShape *compoundShape1 =
+        static_cast<const btCompoundShape *>(ob1->getCollisionShape());
+
+    int numContacts = contactManifold->getNumContacts();
+    for (int j = 0; j < numContacts; ++j)
+    {
+      btManifoldPoint& pt = contactManifold->getContactPoint(j);
+      const btCollisionShape *colShape0 = this->FindCollisionShape(
+          compoundShape0, pt.m_index0);
+      const btCollisionShape *colShape1 = this->FindCollisionShape(
+          compoundShape1, pt.m_index1);
+
+      if (!colShape0 || !colShape1 ||
+         (!this->HasConvexHullChildShapes(colShape0) &&
+          !this->HasConvexHullChildShapes(colShape1)))
+      {
+        this->manifoldsToKeep.insert(contactManifold);
+        continue;
+      }
+
+      btPersistentManifold* colManifold =
+          this->colPairManifolds[colShape0][colShape1];
+      if (!colManifold)
+      {
+        // create new custom manifold for the collision pair
+        colManifold = this->getNewManifold(ob0, ob1);
+        this->colPairManifolds[colShape0][colShape1] = colManifold;
+        this->colPairManifolds[colShape1][colShape0] = colManifold;
+        this->customManifolds.insert(colManifold);
+      }
+      colManifold->addManifoldPoint(pt);
+      colManifold->refreshContactPoints(ob0->getWorldTransform(),
+                                        ob1->getWorldTransform());
+    }
+
+    // clear manifolds that are replaced by custom ones
+    if (this->manifoldsToKeep.find(contactManifold) ==
+        this->manifoldsToKeep.end())
+      contactManifold->clearManifold();
+  }
+}
+
+/////////////////////////////////////////////////
+void GzCollisionDispatcher::releaseManifold(btPersistentManifold *_manifold)
+{
+  auto manifoldIt = this->manifoldsToKeep.find(_manifold);
+  if (manifoldIt != this->manifoldsToKeep.end())
+    this->manifoldsToKeep.erase(manifoldIt);
+
+  btCollisionDispatcher::releaseManifold(_manifold);
+}
+
 /////////////////////////////////////////////////
 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>(

bullet-featherstone/src/Base.hh

@@ -53,6 +53,61 @@ namespace gz {
 namespace physics {
 namespace bullet_featherstone {
 
+/// \brief Custom gz collision dispatcher
+class GzCollisionDispatcher : public btCollisionDispatcher
+{
+  /// \brief Constructor
+  public: GzCollisionDispatcher(
+              btCollisionConfiguration *_collisionConfiguration);
+
+  /// \brief Destructor
+  public: ~GzCollisionDispatcher();
+
+  // Documentation Inherited.
+  // Override base function in order to limit the number of contacts for convex
+  // decomposed mesh collisions.
+  public: void dispatchAllCollisionPairs(
+              btOverlappingPairCache* pairCache,
+              const btDispatcherInfo& dispatchInfo,
+              btDispatcher* dispatcher) override;
+
+  // Documentation Inherited.
+  public: void releaseManifold(btPersistentManifold *_manifold) override;
+
+  /// \brief Remove manifolds that hold pointers to the input collision object
+  /// \param[in] _colObject Collision object being removed
+  public: void RemoveManifoldByCollisionObject(btCollisionObject *_colObj);
+
+  /// \brief Helper function to find the btCollisionShape that represents a
+  /// collision
+  /// \param[in] _compoundShape Link collision shape
+  /// \param[in] _childIndex Index of the child shape within the compound shape
+  /// \return The btCollisionShape that represents the collision or null if
+  /// the collision shape could not be found.
+  public: const btCollisionShape *FindCollisionShape(
+               const btCompoundShape *_compoundShape,
+               int _childIndex);
+
+  /// \brief Helper function to check whether or not the input shape has child
+  /// convex hull shapes.
+  /// \param[in] _shape Shape to check
+  /// \return true if the shape has child convex hull shapes, false otherwise
+  private: bool HasConvexHullChildShapes(const btCollisionShape *_shape);
+
+  /// \brief A map of collision object pairs and their contact manifold
+  /// Note one manifold exists per collision object pair
+  private: std::unordered_map<const btCollisionShape *,
+               std::unordered_map<const btCollisionShape *,
+               btPersistentManifold *>> colPairManifolds;
+
+  /// \brief A set of original contact manifolds that need to be cleared
+  /// as they are replaced by a custom contact manifold
+  private: std::unordered_set<btPersistentManifold *> manifoldsToKeep;
+
+  /// \brief A set of custom contact manifolds created and owned by gz-physics.
+  private: std::unordered_set<btPersistentManifold *> customManifolds;
+};
+
 /// \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,7 +122,7 @@ 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;
@@ -500,6 +555,10 @@ class Base : public Implements3d<FeatureList<Feature>>
       if (link->collider)
       {
         world->world->removeCollisionObject(link->collider.get());
+        GzCollisionDispatcher *dispatcher =
+            dynamic_cast<GzCollisionDispatcher *>(
+            world->world->getDispatcher());
+        dispatcher->RemoveManifoldByCollisionObject(link->collider.get());
         for (const auto shapeID : link->collisionEntityIds)
           this->collisions.erase(shapeID);
       }

bullet-featherstone/src/SDFFeatures.cc

@@ -1326,12 +1326,18 @@ bool SDFFeatures::AddSdfCollision(
       // match the existing collider and issue a warning if they don't.
     }
 
-    this->AddCollision(
+    btCollisionShape *shapePtr = shape.get();
+    auto colID = this->AddCollision(
       CollisionInfo{
         _collision.Name(),
         std::move(shape),
         _linkID,
         linkFrameToCollision});
+
+    // use user index to store the collision id in gz-physics
+    // This is used by GetContactsFromLastStep to retrieve the collision id
+    // from btCollisionShape
+    shapePtr->setUserIndex(std::size_t(colID));
   }
 
   return true;

bullet-featherstone/src/SDFFeatures.hh

@@ -83,6 +83,7 @@ class SDFFeatures :
   /// \param[in] _linkID ID of link to create the collider for
   /// \param[in] _isStatic True if the link is static
   /// \param[in] _shape Collision shape to attach to link
+  /// \param[in] _shapeTF Collision shape local transform in link collider frame
   private: void CreateLinkCollider(const Identity &_linkID,
       bool _isStatic, btCollisionShape *_shape = nullptr,
       const btTransform &_shapeTF = btTransform::getIdentity());