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());