Skip to content

Commit

Permalink
use 2 separate sets to keep track of manifolds, update logic to find …
Browse files Browse the repository at this point in the history 
…collision in get contacts from last step

Signed-off-by: Ian Chen <ichen@openrobotics.org>
  • Loading branch information
@iche033
iche033 committed Jul 3, 2024
1 parent 927bf03 commit a0ffccc
Show file tree
Hide file tree
Showing 4 changed files with 64 additions and 57 deletions.
42 changes: 17 additions & 25 deletions bullet-featherstone/src/Base.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,15 +31,12 @@ GzCollisionDispatcher::GzCollisionDispatcher(
/////////////////////////////////////////////////
GzCollisionDispatcher::~GzCollisionDispatcher()
{
// Use a while loop because the releaseManifold call also loops through
// the manifoldsToKeep map
while (!this->manifoldsToKeep.empty())
for (auto& manifold : this->customManifolds)
{
auto manifoldIt = this->manifoldsToKeep.begin();
bool ownsManifold = manifoldIt->second;
if (ownsManifold)
this->releaseManifold(manifoldIt->first);
btCollisionDispatcher::releaseManifold(manifold);
}

this->customManifolds.clear();
this->colPairManifolds.clear();
}

Expand All @@ -48,24 +45,19 @@ void GzCollisionDispatcher::RemoveManifoldByCollisionObject(
btCollisionObject *_colObj)
{
std::unordered_set<btPersistentManifold *> manifoldsToRemove;
for (const auto& manifoldIt : this->manifoldsToKeep)
for (const auto& manifold: this->customManifolds)
{
bool ownsManifold = manifoldIt.second;
btPersistentManifold *manifold = manifoldIt.first;
if (manifold->getBody0() == _colObj ||
manifold->getBody1() == _colObj)
{
if (ownsManifold)
{
manifoldsToRemove.insert(manifold);
}
manifoldsToRemove.insert(manifold);
}
}

for (auto& manifold : manifoldsToRemove)
{
this->releaseManifold(manifold);
this->manifoldsToKeep.erase(manifold);
btCollisionDispatcher::releaseManifold(manifold);
this->customManifolds.erase(manifold);
}
}

Expand Down Expand Up @@ -158,8 +150,8 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs(
dynamic_cast<const btMultiBodyLinkCollider *>(
contactManifold->getBody1());

if (this->manifoldsToKeep.find(contactManifold) !=
this->manifoldsToKeep.end())
if (this->customManifolds.find(contactManifold) !=
this->customManifolds.end())
{
contactManifold->refreshContactPoints(ob0->getWorldTransform(),
ob1->getWorldTransform());
Expand All @@ -184,7 +176,7 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs(
(!this->HasConvexHullChildShapes(colShape0) &&
!this->HasConvexHullChildShapes(colShape1)))
{
this->manifoldsToKeep[contactManifold] = false;
this->manifoldsToClear.insert(contactManifold);
continue;
}

Expand All @@ -196,7 +188,7 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs(
colManifold = this->getNewManifold(ob0, ob1);
this->colPairManifolds[colShape0][colShape1] = colManifold;
this->colPairManifolds[colShape1][colShape0] = colManifold;
this->manifoldsToKeep[colManifold] = true;
this->customManifolds.insert(colManifold);
}
colManifold->addManifoldPoint(pt);
colManifold->refreshContactPoints(ob0->getWorldTransform(),
Expand All @@ -205,18 +197,18 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs(

// clear original manifolds so that bullet will only use the
// new ones
if (this->manifoldsToKeep.find(contactManifold) ==
this->manifoldsToKeep.end())
if (this->manifoldsToClear.find(contactManifold) ==
this->manifoldsToClear.end())
contactManifold->clearManifold();
}
}

/////////////////////////////////////////////////
void GzCollisionDispatcher::releaseManifold(btPersistentManifold *_manifold)
{
auto manifoldIt = this->manifoldsToKeep.find(_manifold);
if (manifoldIt != this->manifoldsToKeep.end())
this->manifoldsToKeep.erase(manifoldIt);
auto manifoldIt = this->manifoldsToClear.find(_manifold);
if (manifoldIt != this->manifoldsToClear.end())
this->manifoldsToClear.erase(manifoldIt);

btCollisionDispatcher::releaseManifold(_manifold);
}
Expand Down
22 changes: 13 additions & 9 deletions bullet-featherstone/src/Base.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -156,30 +156,34 @@ class GzCollisionDispatcher : public btCollisionDispatcher
/// \param[in] _colObject Collision object being removed
public: void RemoveManifoldByCollisionObject(btCollisionObject *_colObj);

/// \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 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.
private: const btCollisionShape *FindCollisionShape(
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 map of contact manifolds and whether we own this manifold
private: std::unordered_map<btPersistentManifold *, bool> manifoldsToKeep;
/// \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 *> manifoldsToClear;

/// \brief A set of custom contact manifolds created and owned by gz-physics.
private: std::unordered_set<btPersistentManifold *> customManifolds;
};

/// Link information is embedded inside the model, so all we need to store here
Expand Down
8 changes: 7 additions & 1 deletion bullet-featherstone/src/SDFFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1368,12 +1368,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;
Expand Down
49 changes: 27 additions & 22 deletions bullet-featherstone/src/SimulationFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,40 +60,45 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const
{
return outContacts;
}
GzCollisionDispatcher *dispatcher =
dynamic_cast<GzCollisionDispatcher *>(world->world->getDispatcher());

int numManifolds = world->world->getDispatcher()->getNumManifolds();
for (int i = 0; i < numManifolds; i++)
{
btPersistentManifold* contactManifold =
world->world->getDispatcher()->getManifoldByIndexInternal(i);
const btMultiBodyLinkCollider* obA =
const btMultiBodyLinkCollider* ob0 =
dynamic_cast<const btMultiBodyLinkCollider*>(contactManifold->getBody0());
const btMultiBodyLinkCollider* obB =
const btMultiBodyLinkCollider* ob1 =
dynamic_cast<const btMultiBodyLinkCollider*>(contactManifold->getBody1());
std::size_t collision1ID = std::numeric_limits<std::size_t>::max();
std::size_t collision2ID = std::numeric_limits<std::size_t>::max();

for (const auto & link : this->links)
{
if (obA == link.second->collider.get())
{
for (const auto &v : link.second->collisionNameToEntityId)
{
collision1ID = v.second;
}
}
if (obB == link.second->collider.get())
{
for (const auto &v : link.second->collisionNameToEntityId)
{
collision2ID = v.second;
}
}
}
const btCompoundShape *compoundShape0 =
dynamic_cast<const btCompoundShape *>(ob0->getCollisionShape());
const btCompoundShape *compoundShape1 =
dynamic_cast<const btCompoundShape *>(ob1->getCollisionShape());

int numContacts = contactManifold->getNumContacts();
for (int j = 0; j < numContacts; j++)
{
btManifoldPoint& pt = contactManifold->getContactPoint(j);

const btCollisionShape *colShape0 = dispatcher->FindCollisionShape(
compoundShape0, pt.m_index0);
const btCollisionShape *colShape1 = dispatcher->FindCollisionShape(
compoundShape1, pt.m_index1);

std::size_t collision0ID = std::numeric_limits<std::size_t>::max();
std::size_t collision1ID = std::numeric_limits<std::size_t>::max();
if (colShape0)
collision0ID = colShape0->getUserIndex();
else if (compoundShape0->getNumChildShapes() > 0)
collision0ID = compoundShape0->getChildShape(0)->getUserIndex();
if (colShape1)
collision1ID = colShape1->getUserIndex();
else if (compoundShape1->getNumChildShapes() > 0)
collision1ID = compoundShape1->getChildShape(0)->getUserIndex();

CompositeData extraData;

// Add normal, depth and wrench to extraData.
Expand All @@ -107,8 +112,8 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const
extraContactData.depth = pt.getDistance();

outContacts.push_back(SimulationFeatures::ContactInternal {
this->GenerateIdentity(collision0ID, this->collisions.at(collision0ID)),
this->GenerateIdentity(collision1ID, this->collisions.at(collision1ID)),
this->GenerateIdentity(collision2ID, this->collisions.at(collision2ID)),
convert(pt.getPositionWorldOnA()), extraData});
}
}
Expand Down

0 comments on commit a0ffccc

Please sign in to comment.