From 5a05b5db3a04dc8e54327a14baaae6977a2b9ceb Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 3 Jul 2024 02:10:51 +0000 Subject: [PATCH 01/26] Use 1 contact manifold for each convex decomposed mesh collision Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 202 +++++++++++++++++++++++++++++++- bullet-featherstone/src/Base.hh | 56 +++++++++ test/common_test/collisions.cc | 141 +++++++++++++++++++++- 3 files changed, 396 insertions(+), 3 deletions(-) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index ac66f5746..98fda516c 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -23,6 +23,206 @@ namespace gz { namespace physics { namespace bullet_featherstone { +///////////////////////////////////////////////// +GzCollisionDispatcher::GzCollisionDispatcher( + btCollisionConfiguration *_collisionConfiguration) + : btCollisionDispatcher(_collisionConfiguration) {}; + +///////////////////////////////////////////////// +GzCollisionDispatcher::~GzCollisionDispatcher() +{ + // Use a while loop because the releaseManifold call also loops through + // the manifoldsToKeep map + while (!this->manifoldsToKeep.empty()) + { + auto manifoldIt = this->manifoldsToKeep.begin(); + bool ownsManifold = manifoldIt->second; + if (ownsManifold) + this->releaseManifold(manifoldIt->first); + } + this->colPairManifolds.clear(); +} + +///////////////////////////////////////////////// +void GzCollisionDispatcher::RemoveManifoldByCollisionObject( + btCollisionObject *_colObj) +{ + std::unordered_set manifoldsToRemove; + for (const auto& manifoldIt : this->manifoldsToKeep) + { + bool ownsManifold = manifoldIt.second; + btPersistentManifold *manifold = manifoldIt.first; + if (manifold->getBody0() == _colObj || + manifold->getBody1() == _colObj) + { + if (ownsManifold) + { + manifoldsToRemove.insert(manifold); + } + } + } + + for (auto& manifold : manifoldsToRemove) + { + this->releaseManifold(manifold); + this->manifoldsToKeep.erase(manifold); + } +} + +///////////////////////////////////////////////// +bool GzCollisionDispatcher::HasConvexHullChildShapes( + const btCollisionShape *_shape) +{ + if (!_shape || !_shape->isCompound()) + return false; + + const btCompoundShape *compoundShape = + dynamic_cast(_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( + contactManifold->getBody0()); + const btMultiBodyLinkCollider* ob1 = + dynamic_cast( + contactManifold->getBody1()); + + if (this->manifoldsToKeep.find(contactManifold) != + this->manifoldsToKeep.end()) + { + contactManifold->refreshContactPoints(ob0->getWorldTransform(), + ob1->getWorldTransform()); + continue; + } + + int numContacts = contactManifold->getNumContacts(); + totalContacts += numContacts; + + const btCompoundShape *compoundShape0 = + dynamic_cast(ob0->getCollisionShape()); + const btCompoundShape *compoundShape1 = + dynamic_cast(ob1->getCollisionShape()); + + 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[contactManifold] = false; + 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->manifoldsToKeep[colManifold] = true; + } + colManifold->addManifoldPoint(pt); + colManifold->refreshContactPoints(ob0->getWorldTransform(), + ob1->getWorldTransform()); + } + + // clear original manifolds so that bullet will only use the + // new 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_)) @@ -30,7 +230,7 @@ WorldInfo::WorldInfo(std::string name_) this->collisionConfiguration = std::make_unique(); this->dispatcher = - std::make_unique(collisionConfiguration.get()); + std::make_unique(collisionConfiguration.get()); this->broadphase = std::make_unique(); this->solver = std::make_unique(); this->world = std::make_unique( diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 8df941a4c..8f186288a 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -130,6 +130,58 @@ class GzMultiBodyLinkCollider: public btMultiBodyLinkCollider { } }; +/// \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: virtual void dispatchAllCollisionPairs( + btOverlappingPairCache* pairCache, + const btDispatcherInfo& dispatchInfo, + btDispatcher* dispatcher) override; + + // Documentation Inherited. + public: virtual 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 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( + const btCompoundShape *_compoundShape, + int _childIndex); + + /// \brief A map of collision object pairs and their contact manifold + /// Note one manifold exists per collision object pair + private: std::unordered_map> colPairManifolds; + + /// \brief A map of contact manifolds and whether we own this manifold + private: std::unordered_map manifoldsToKeep; +}; + /// Link information is embedded inside the model, so all we need to store here /// is a reference to the model and the index of this link inside of it. struct LinkInfo @@ -490,6 +542,10 @@ class Base : public Implements3d> if (link->collider) { world->world->removeCollisionObject(link->collider.get()); + GzCollisionDispatcher *dispatcher = + dynamic_cast( + world->world->getDispatcher()); + dispatcher->RemoveManifoldByCollisionObject(link->collider.get()); for (const auto shapeID : link->collisionEntityIds) this->collisions.erase(shapeID); } diff --git a/test/common_test/collisions.cc b/test/common_test/collisions.cc index 647bffa39..17b077ad2 100644 --- a/test/common_test/collisions.cc +++ b/test/common_test/collisions.cc @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -152,8 +153,11 @@ TYPED_TEST(CollisionTest, MeshAndPlane) using CollisionMeshFeaturesList = gz::physics::FeatureList< gz::physics::sdf::ConstructSdfModel, gz::physics::sdf::ConstructSdfWorld, + gz::physics::FindFreeGroupFeature, + gz::physics::SetFreeGroupWorldPose, gz::physics::LinkFrameSemantics, gz::physics::ForwardStep, + gz::physics::GetContactsFromLastStepFeature, gz::physics::GetEntities >; @@ -198,7 +202,8 @@ TEST_F(CollisionMeshTestFeaturesList, MeshOptimization) { // currently only bullet-featherstone supports mesh decomposition if (this->PhysicsEngineName(name) != "bullet-featherstone") - continue; + GTEST_SKIP(); + std::cout << "Testing plugin: " << name << std::endl; gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); @@ -390,6 +395,138 @@ TEST_F(CollisionMeshTestFeaturesList, MeshDecomposition) } } +TEST_F(CollisionMeshTestFeaturesList, MeshContacts) +{ + auto getMeshModelStr = [](const std::string &_optimization, + const std::string &_name, + const gz::math::Pose3d &_pose) + { + std::stringstream modelStr; + modelStr << R"( + + + )"; + modelStr << _pose; + modelStr << R"( + + + + + )"; + modelStr << gz::physics::test::resources::kChassisDae; + modelStr << R"( + + + + + + )"; + return modelStr.str(); + }; + + std::string boxModelStr = R"( + + + true + 0 0 0 0 0 0 + + + + + 10 10 10 + + + + + + )"; + + for (const std::string &name : this->pluginNames) + { + // currently only bullet-featherstone supports mesh decomposition + if (this->PhysicsEngineName(name) != "bullet-featherstone") + GTEST_SKIP(); + + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + sdf::Root rootWorld; + const sdf::Errors errorsWorld = + rootWorld.Load(common_test::worlds::kEmptySdf); + ASSERT_TRUE(errorsWorld.empty()) << errorsWorld; + + auto engine = + gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + auto world = engine->ConstructWorld(*rootWorld.WorldByIndex(0)); + ASSERT_NE(nullptr, world); + + sdf::Root root; + gz::math::Pose3d initialModelPose(0, 0, 0, 0, 0, 0); + sdf::Errors errors = root.LoadSdfString(getMeshModelStr( + "", "mesh", initialModelPose)); + ASSERT_TRUE(errors.empty()) << errors; + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + + errors = root.LoadSdfString(boxModelStr); + ASSERT_TRUE(errors.empty()) << errors; + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + + auto mesh = world->GetModel("mesh"); + ASSERT_NE(nullptr, mesh); + auto meshFreeGroup = mesh->FindFreeGroup(); + EXPECT_NE(nullptr, meshFreeGroup); + + // step and get contacts + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + world->Step(output, state, input); + + auto contacts = world->GetContactsFromLastStep(); + + // large box should be intersecting mesh + EXPECT_FALSE(contacts.empty()); + + // step and get contacts + world->Step(output, state, input); + contacts = world->GetContactsFromLastStep(); + + std::size_t contactSize = contacts.size(); + EXPECT_NE(0u, contactSize); + + // try with a decomposed mesh + errors = root.LoadSdfString(getMeshModelStr( + "convex_decomposition", "mesh_decomposed", + gz::math::Pose3d(0, 0, 3, 0, 0, 0))); + ASSERT_TRUE(errors.empty()) << errors; + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + + // step and get contacts + world->Step(output, state, input); + contacts = world->GetContactsFromLastStep(); + + if (this->PhysicsEngineName(name) != "bullet-featherstone") + { + // convex decomposed meshes in bullet-featherstone should generate only + // 1 contact manifold and (up to) 4 contact points + EXPECT_EQ(contactSize + 4, contacts.size()); + } + else + { + EXPECT_LT(contactSize, contacts.size()); + } + } +} + using CollisionStaticFeaturesList = gz::physics::FeatureList< gz::physics::sdf::ConstructSdfModel, gz::physics::sdf::ConstructSdfWorld, @@ -493,7 +630,7 @@ TEST_F(CollisionStaticTestFeaturesList, StaticCollisions) // currently only bullet-featherstone skips collision checking between // static bodies and bodies with world fixed joint if (this->PhysicsEngineName(name) != "bullet-featherstone") - continue; + GTEST_SKIP(); errors = root.LoadSdfString(getBoxFixedJointStr( "box_fixed_world_joint", gz::math::Pose3d::Zero)); From 9117975c947c54fa59acf94e537a35727b8e0ab6 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 3 Jul 2024 02:27:36 +0000 Subject: [PATCH 02/26] fix Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index 98fda516c..17c14380a 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -166,14 +166,12 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( continue; } - int numContacts = contactManifold->getNumContacts(); - totalContacts += numContacts; - - const btCompoundShape *compoundShape0 = - dynamic_cast(ob0->getCollisionShape()); - const btCompoundShape *compoundShape1 = - dynamic_cast(ob1->getCollisionShape()); + const btCompoundShape *compoundShape0 = + dynamic_cast(ob0->getCollisionShape()); + const btCompoundShape *compoundShape1 = + dynamic_cast(ob1->getCollisionShape()); + int numContacts = contactManifold->getNumContacts(); for (int j = 0; j < numContacts; ++j) { btManifoldPoint& pt = contactManifold->getContactPoint(j); From 6a31cd3cb1b988328ea9930c1bcf64bedfb80a73 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 3 Jul 2024 02:36:35 +0000 Subject: [PATCH 03/26] style Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index 17c14380a..b1c36c0ca 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -26,7 +26,7 @@ namespace bullet_featherstone { ///////////////////////////////////////////////// GzCollisionDispatcher::GzCollisionDispatcher( btCollisionConfiguration *_collisionConfiguration) - : btCollisionDispatcher(_collisionConfiguration) {}; + : btCollisionDispatcher(_collisionConfiguration) {} ///////////////////////////////////////////////// GzCollisionDispatcher::~GzCollisionDispatcher() From 927bf03e0c32967ab97a0da74763f0001df395fc Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 3 Jul 2024 02:42:25 +0000 Subject: [PATCH 04/26] remove unused features in test Signed-off-by: Ian Chen --- test/common_test/collisions.cc | 5 ----- 1 file changed, 5 deletions(-) diff --git a/test/common_test/collisions.cc b/test/common_test/collisions.cc index 17b077ad2..0ab7f2bbe 100644 --- a/test/common_test/collisions.cc +++ b/test/common_test/collisions.cc @@ -34,7 +34,6 @@ #include #include #include -#include #include #include #include @@ -153,8 +152,6 @@ TYPED_TEST(CollisionTest, MeshAndPlane) using CollisionMeshFeaturesList = gz::physics::FeatureList< gz::physics::sdf::ConstructSdfModel, gz::physics::sdf::ConstructSdfWorld, - gz::physics::FindFreeGroupFeature, - gz::physics::SetFreeGroupWorldPose, gz::physics::LinkFrameSemantics, gz::physics::ForwardStep, gz::physics::GetContactsFromLastStepFeature, @@ -480,8 +477,6 @@ TEST_F(CollisionMeshTestFeaturesList, MeshContacts) auto mesh = world->GetModel("mesh"); ASSERT_NE(nullptr, mesh); - auto meshFreeGroup = mesh->FindFreeGroup(); - EXPECT_NE(nullptr, meshFreeGroup); // step and get contacts gz::physics::ForwardStep::Output output; From a0ffccc9049f818e2581b7179bd07db488aaf753 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 3 Jul 2024 17:08:30 +0000 Subject: [PATCH 05/26] use 2 separate sets to keep track of manifolds, update logic to find collision in get contacts from last step Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 42 +++++++--------- bullet-featherstone/src/Base.hh | 22 +++++---- bullet-featherstone/src/SDFFeatures.cc | 8 ++- bullet-featherstone/src/SimulationFeatures.cc | 49 ++++++++++--------- 4 files changed, 64 insertions(+), 57 deletions(-) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index b1c36c0ca..18985c73f 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -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(); } @@ -48,24 +45,19 @@ void GzCollisionDispatcher::RemoveManifoldByCollisionObject( btCollisionObject *_colObj) { std::unordered_set 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); } } @@ -158,8 +150,8 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( dynamic_cast( contactManifold->getBody1()); - if (this->manifoldsToKeep.find(contactManifold) != - this->manifoldsToKeep.end()) + if (this->customManifolds.find(contactManifold) != + this->customManifolds.end()) { contactManifold->refreshContactPoints(ob0->getWorldTransform(), ob1->getWorldTransform()); @@ -184,7 +176,7 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( (!this->HasConvexHullChildShapes(colShape0) && !this->HasConvexHullChildShapes(colShape1))) { - this->manifoldsToKeep[contactManifold] = false; + this->manifoldsToClear.insert(contactManifold); continue; } @@ -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(), @@ -205,8 +197,8 @@ 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(); } } @@ -214,9 +206,9 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( ///////////////////////////////////////////////// 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); } diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 8f186288a..89f39f3f5 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -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> colPairManifolds; - /// \brief A map of contact manifolds and whether we own this manifold - private: std::unordered_map 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 manifoldsToClear; + + /// \brief A set of custom contact manifolds created and owned by gz-physics. + private: std::unordered_set customManifolds; }; /// Link information is embedded inside the model, so all we need to store here diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 8e6c4a742..2cffa906e 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -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; diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index a32d93d2c..24f6e50bf 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -60,40 +60,45 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const { return outContacts; } + GzCollisionDispatcher *dispatcher = + dynamic_cast(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(contactManifold->getBody0()); - const btMultiBodyLinkCollider* obB = + const btMultiBodyLinkCollider* ob1 = dynamic_cast(contactManifold->getBody1()); - std::size_t collision1ID = std::numeric_limits::max(); - std::size_t collision2ID = std::numeric_limits::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(ob0->getCollisionShape()); + const btCompoundShape *compoundShape1 = + dynamic_cast(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::max(); + std::size_t collision1ID = std::numeric_limits::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. @@ -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}); } } From 1b93f3f204dcbffed38d585af53f982371724ae1 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 3 Jul 2024 17:11:18 +0000 Subject: [PATCH 06/26] undo some changes Signed-off-by: Ian Chen --- test/common_test/collisions.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/common_test/collisions.cc b/test/common_test/collisions.cc index 0ab7f2bbe..3122294d8 100644 --- a/test/common_test/collisions.cc +++ b/test/common_test/collisions.cc @@ -199,7 +199,7 @@ TEST_F(CollisionMeshTestFeaturesList, MeshOptimization) { // currently only bullet-featherstone supports mesh decomposition if (this->PhysicsEngineName(name) != "bullet-featherstone") - GTEST_SKIP(); + continue; std::cout << "Testing plugin: " << name << std::endl; gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); @@ -445,7 +445,7 @@ TEST_F(CollisionMeshTestFeaturesList, MeshContacts) { // currently only bullet-featherstone supports mesh decomposition if (this->PhysicsEngineName(name) != "bullet-featherstone") - GTEST_SKIP(); + continue; std::cout << "Testing plugin: " << name << std::endl; gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); @@ -625,7 +625,7 @@ TEST_F(CollisionStaticTestFeaturesList, StaticCollisions) // currently only bullet-featherstone skips collision checking between // static bodies and bodies with world fixed joint if (this->PhysicsEngineName(name) != "bullet-featherstone") - GTEST_SKIP(); + continue; errors = root.LoadSdfString(getBoxFixedJointStr( "box_fixed_world_joint", gz::math::Pose3d::Zero)); From d1a0a879019fc25935e7e1de0e318e54aca12a73 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 3 Jul 2024 17:35:47 +0000 Subject: [PATCH 07/26] style Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index 18985c73f..b19daef59 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -45,7 +45,7 @@ void GzCollisionDispatcher::RemoveManifoldByCollisionObject( btCollisionObject *_colObj) { std::unordered_set manifoldsToRemove; - for (const auto& manifold: this->customManifolds) + for (const auto& manifold : this->customManifolds) { if (manifold->getBody0() == _colObj || manifold->getBody1() == _colObj) From 5fe67d0c9b83304ef90397eff776c474e704be60 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 10 Jul 2024 16:54:24 +0000 Subject: [PATCH 08/26] ifdef for windows Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index b19daef59..910be897b 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -219,8 +219,18 @@ WorldInfo::WorldInfo(std::string name_) { this->collisionConfiguration = std::make_unique(); +#ifdef _WIN32 + // Use original btCollisionDispatcher on window as GzCollisionDispatcher + // causes tests to crash. + // \todo(iche033) Investigate cause of crash + this->dispatcher = + std::make_unique(collisionConfiguration.get()); +#else + // Use custom GzCollisionDispatcher that reduces number of contact points + // for convex decomposed mesh collisions. this->dispatcher = std::make_unique(collisionConfiguration.get()); +#endif this->broadphase = std::make_unique(); this->solver = std::make_unique(); this->world = std::make_unique( From ba6f4d02bdd8a62473f6881b0fe9581dba448024 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 10 Jul 2024 20:36:06 +0000 Subject: [PATCH 09/26] undo changes in simulation features Signed-off-by: Ian Chen --- bullet-featherstone/src/SimulationFeatures.cc | 49 +++++++++---------- 1 file changed, 22 insertions(+), 27 deletions(-) diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index b6c6f1e03..c3509721d 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -65,45 +65,40 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const { return outContacts; } - GzCollisionDispatcher *dispatcher = - dynamic_cast(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* ob0 = + const btMultiBodyLinkCollider* obA = dynamic_cast(contactManifold->getBody0()); - const btMultiBodyLinkCollider* ob1 = + const btMultiBodyLinkCollider* obB = dynamic_cast(contactManifold->getBody1()); + std::size_t collision1ID = std::numeric_limits::max(); + std::size_t collision2ID = std::numeric_limits::max(); - const btCompoundShape *compoundShape0 = - dynamic_cast(ob0->getCollisionShape()); - const btCompoundShape *compoundShape1 = - dynamic_cast(ob1->getCollisionShape()); - + 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; + } + } + } 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::max(); - std::size_t collision1ID = std::numeric_limits::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. @@ -117,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}); } } From 4fb4b575809f4e0d966164bb65036b65b1b03fe5 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 11 Jul 2024 03:23:25 +0000 Subject: [PATCH 10/26] test with GzCollisionDispatcher Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index 910be897b..b19daef59 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -219,18 +219,8 @@ WorldInfo::WorldInfo(std::string name_) { this->collisionConfiguration = std::make_unique(); -#ifdef _WIN32 - // Use original btCollisionDispatcher on window as GzCollisionDispatcher - // causes tests to crash. - // \todo(iche033) Investigate cause of crash - this->dispatcher = - std::make_unique(collisionConfiguration.get()); -#else - // Use custom GzCollisionDispatcher that reduces number of contact points - // for convex decomposed mesh collisions. this->dispatcher = std::make_unique(collisionConfiguration.get()); -#endif this->broadphase = std::make_unique(); this->solver = std::make_unique(); this->world = std::make_unique( From 51c3b80d5a99e49c96091f0933a390901b85fe13 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 11 Jul 2024 05:25:49 +0000 Subject: [PATCH 11/26] testing windows Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index b19daef59..c79b9a84e 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -65,6 +65,7 @@ void GzCollisionDispatcher::RemoveManifoldByCollisionObject( bool GzCollisionDispatcher::HasConvexHullChildShapes( const btCollisionShape *_shape) { + return false; if (!_shape || !_shape->isCompound()) return false; @@ -166,11 +167,13 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( int numContacts = contactManifold->getNumContacts(); for (int j = 0; j < numContacts; ++j) { + std::cerr << " get contact manifold point " << std::endl; btManifoldPoint& pt = contactManifold->getContactPoint(j); const btCollisionShape *colShape0 = this->FindCollisionShape( compoundShape0, pt.m_index0); const btCollisionShape *colShape1 = this->FindCollisionShape( compoundShape1, pt.m_index1); + std::cerr << " found col shapes" << std::endl; if (!colShape0 || !colShape1 || (!this->HasConvexHullChildShapes(colShape0) && @@ -180,17 +183,21 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( continue; } + std::cerr << " retrieving contact manifold " << std::endl; btPersistentManifold* colManifold = this->colPairManifolds[colShape0][colShape1]; if (!colManifold) { + std::cerr << " creating contact manifold " << std::endl; // 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); } + std::cerr << " adding contact manifold " << std::endl; colManifold->addManifoldPoint(pt); + std::cerr << " adding contact manifold done " << std::endl; colManifold->refreshContactPoints(ob0->getWorldTransform(), ob1->getWorldTransform()); } From 9411c0eeb3f050ae5a8702f23a8c7dbd2223fe24 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 11 Jul 2024 16:45:57 +0000 Subject: [PATCH 12/26] more windows testing Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 16 ++--- bullet-featherstone/src/Base.hh | 113 ++++++++++++++++---------------- 2 files changed, 64 insertions(+), 65 deletions(-) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index c79b9a84e..f4cbcb227 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -26,7 +26,10 @@ namespace bullet_featherstone { ///////////////////////////////////////////////// GzCollisionDispatcher::GzCollisionDispatcher( btCollisionConfiguration *_collisionConfiguration) - : btCollisionDispatcher(_collisionConfiguration) {} + : btCollisionDispatcher(_collisionConfiguration) +{ + std::cerr << " ========== GzCollisionDispatcher constructor " << std::endl; +} ///////////////////////////////////////////////// GzCollisionDispatcher::~GzCollisionDispatcher() @@ -65,7 +68,6 @@ void GzCollisionDispatcher::RemoveManifoldByCollisionObject( bool GzCollisionDispatcher::HasConvexHullChildShapes( const btCollisionShape *_shape) { - return false; if (!_shape || !_shape->isCompound()) return false; @@ -129,6 +131,7 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher) { + std::cerr << " ========== GzCollisionDispatcher dispatch all collision pairs" << std::endl; btCollisionDispatcher::dispatchAllCollisionPairs( pairCache, dispatchInfo, dispatcher); @@ -167,13 +170,11 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( int numContacts = contactManifold->getNumContacts(); for (int j = 0; j < numContacts; ++j) { - std::cerr << " get contact manifold point " << std::endl; btManifoldPoint& pt = contactManifold->getContactPoint(j); const btCollisionShape *colShape0 = this->FindCollisionShape( compoundShape0, pt.m_index0); const btCollisionShape *colShape1 = this->FindCollisionShape( compoundShape1, pt.m_index1); - std::cerr << " found col shapes" << std::endl; if (!colShape0 || !colShape1 || (!this->HasConvexHullChildShapes(colShape0) && @@ -183,21 +184,17 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( continue; } - std::cerr << " retrieving contact manifold " << std::endl; btPersistentManifold* colManifold = this->colPairManifolds[colShape0][colShape1]; if (!colManifold) { - std::cerr << " creating contact manifold " << std::endl; // 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); } - std::cerr << " adding contact manifold " << std::endl; colManifold->addManifoldPoint(pt); - std::cerr << " adding contact manifold done " << std::endl; colManifold->refreshContactPoints(ob0->getWorldTransform(), ob1->getWorldTransform()); } @@ -213,6 +210,7 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( ///////////////////////////////////////////////// void GzCollisionDispatcher::releaseManifold(btPersistentManifold *_manifold) { + std::cerr << " ========== GzCollisionDispatcher release manifold" << std::endl; auto manifoldIt = this->manifoldsToClear.find(_manifold); if (manifoldIt != this->manifoldsToClear.end()) this->manifoldsToClear.erase(manifoldIt); @@ -226,8 +224,10 @@ WorldInfo::WorldInfo(std::string name_) { this->collisionConfiguration = std::make_unique(); + std::cerr << " ========================= creating dispatcher " << std::endl; this->dispatcher = std::make_unique(collisionConfiguration.get()); + std::cerr << " ========================= creating dispatcher done " << std::endl; this->broadphase = std::make_unique(); this->solver = std::make_unique(); this->world = std::make_unique( diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index b1610c0b4..fdce9524c 100644 --- a/bullet-featherstone/src/Base.hh +++ b/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> 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 manifoldsToClear; + + /// \brief A set of custom contact manifolds created and owned by gz-physics. + private: std::unordered_set 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 collisionConfiguration; - std::unique_ptr dispatcher; + std::unique_ptr dispatcher; std::unique_ptr broadphase; std::unique_ptr solver; std::unique_ptr world; @@ -132,62 +187,6 @@ class GzMultiBodyLinkCollider: public btMultiBodyLinkCollider { } }; -/// \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: virtual void dispatchAllCollisionPairs( - btOverlappingPairCache* pairCache, - const btDispatcherInfo& dispatchInfo, - btDispatcher* dispatcher) override; - - // Documentation Inherited. - public: virtual 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> 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 manifoldsToClear; - - /// \brief A set of custom contact manifolds created and owned by gz-physics. - private: std::unordered_set customManifolds; -}; - /// Link information is embedded inside the model, so all we need to store here /// is a reference to the model and the index of this link inside of it. struct LinkInfo From 93c87eb59a9643501029b2c650c7c7868f74445a Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 11 Jul 2024 20:23:07 +0000 Subject: [PATCH 13/26] more testing on win Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index f4cbcb227..8af894bdf 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -34,6 +34,7 @@ GzCollisionDispatcher::GzCollisionDispatcher( ///////////////////////////////////////////////// GzCollisionDispatcher::~GzCollisionDispatcher() { + std::cerr << " ========== GzCollisionDispatcher destructor " << std::endl; for (auto& manifold : this->customManifolds) { btCollisionDispatcher::releaseManifold(manifold); @@ -41,12 +42,14 @@ GzCollisionDispatcher::~GzCollisionDispatcher() this->customManifolds.clear(); this->colPairManifolds.clear(); + std::cerr << " ========== GzCollisionDispatcher destructor done " << std::endl; } ///////////////////////////////////////////////// void GzCollisionDispatcher::RemoveManifoldByCollisionObject( btCollisionObject *_colObj) { + std::cerr << " ========== GzCollisionDispatcher remove manifold by col " << std::endl; std::unordered_set manifoldsToRemove; for (const auto& manifold : this->customManifolds) { @@ -62,6 +65,7 @@ void GzCollisionDispatcher::RemoveManifoldByCollisionObject( btCollisionDispatcher::releaseManifold(manifold); this->customManifolds.erase(manifold); } + std::cerr << " ========== GzCollisionDispatcher remove manifold by col done " << std::endl; } ///////////////////////////////////////////////// @@ -215,7 +219,9 @@ void GzCollisionDispatcher::releaseManifold(btPersistentManifold *_manifold) if (manifoldIt != this->manifoldsToClear.end()) this->manifoldsToClear.erase(manifoldIt); + std::cerr << " ========== GzCollisionDispatcher release manifold 0 " << std::endl; btCollisionDispatcher::releaseManifold(_manifold); + std::cerr << " ========== GzCollisionDispatcher release manifold done" << std::endl; } ///////////////////////////////////////////////// From 517efd606b4496d317fa763ef0a02ec4f0f1d8bd Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 11 Jul 2024 22:34:15 +0000 Subject: [PATCH 14/26] rename var Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 23 +++++++++++++++-------- bullet-featherstone/src/Base.hh | 2 +- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index 8af894bdf..95cadd935 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -158,6 +158,8 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( dynamic_cast( 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()) { @@ -166,6 +168,12 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( continue; } + if (this->manifoldsToKeep.find(contactManifold) != + this->manifoldsToKeep.end()) + { + continue; + } + const btCompoundShape *compoundShape0 = dynamic_cast(ob0->getCollisionShape()); const btCompoundShape *compoundShape1 = @@ -184,7 +192,7 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( (!this->HasConvexHullChildShapes(colShape0) && !this->HasConvexHullChildShapes(colShape1))) { - this->manifoldsToClear.insert(contactManifold); + this->manifoldsToKeep.insert(contactManifold); continue; } @@ -203,10 +211,9 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( ob1->getWorldTransform()); } - // clear original manifolds so that bullet will only use the - // new ones - if (this->manifoldsToClear.find(contactManifold) == - this->manifoldsToClear.end()) + // clear manifolds that are replaced by custom ones + if (this->manifoldsToKeep.find(contactManifold) == + this->manifoldsToKeep.end()) contactManifold->clearManifold(); } } @@ -215,9 +222,9 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( void GzCollisionDispatcher::releaseManifold(btPersistentManifold *_manifold) { std::cerr << " ========== GzCollisionDispatcher release manifold" << std::endl; - auto manifoldIt = this->manifoldsToClear.find(_manifold); - if (manifoldIt != this->manifoldsToClear.end()) - this->manifoldsToClear.erase(manifoldIt); + auto manifoldIt = this->manifoldsToKeep.find(_manifold); + if (manifoldIt != this->manifoldsToKeep.end()) + this->manifoldsToKeep.erase(manifoldIt); std::cerr << " ========== GzCollisionDispatcher release manifold 0 " << std::endl; btCollisionDispatcher::releaseManifold(_manifold); diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index fdce9524c..911003aea 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -102,7 +102,7 @@ class GzCollisionDispatcher : public btCollisionDispatcher /// \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 manifoldsToClear; + private: std::unordered_set manifoldsToKeep; /// \brief A set of custom contact manifolds created and owned by gz-physics. private: std::unordered_set customManifolds; From a752843d5035d1bbe62dac3c06cad402eab2237e Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 12 Jul 2024 16:55:30 +0000 Subject: [PATCH 15/26] revert ba6f4d02bdd8a62473f6881b0fe9581dba448024 Signed-off-by: Ian Chen --- bullet-featherstone/src/SimulationFeatures.cc | 49 ++++++++++--------- 1 file changed, 27 insertions(+), 22 deletions(-) diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index c3509721d..b6c6f1e03 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -65,40 +65,45 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const { return outContacts; } + GzCollisionDispatcher *dispatcher = + dynamic_cast(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(contactManifold->getBody0()); - const btMultiBodyLinkCollider* obB = + const btMultiBodyLinkCollider* ob1 = dynamic_cast(contactManifold->getBody1()); - std::size_t collision1ID = std::numeric_limits::max(); - std::size_t collision2ID = std::numeric_limits::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(ob0->getCollisionShape()); + const btCompoundShape *compoundShape1 = + dynamic_cast(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::max(); + std::size_t collision1ID = std::numeric_limits::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. @@ -112,8 +117,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}); } } From a573ac6a854e7cbe101d4748a84240b7b7ab203c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 12 Jul 2024 17:00:44 +0000 Subject: [PATCH 16/26] return early Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index 95cadd935..a8cebf3bc 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -28,13 +28,11 @@ GzCollisionDispatcher::GzCollisionDispatcher( btCollisionConfiguration *_collisionConfiguration) : btCollisionDispatcher(_collisionConfiguration) { - std::cerr << " ========== GzCollisionDispatcher constructor " << std::endl; } ///////////////////////////////////////////////// GzCollisionDispatcher::~GzCollisionDispatcher() { - std::cerr << " ========== GzCollisionDispatcher destructor " << std::endl; for (auto& manifold : this->customManifolds) { btCollisionDispatcher::releaseManifold(manifold); @@ -42,14 +40,12 @@ GzCollisionDispatcher::~GzCollisionDispatcher() this->customManifolds.clear(); this->colPairManifolds.clear(); - std::cerr << " ========== GzCollisionDispatcher destructor done " << std::endl; } ///////////////////////////////////////////////// void GzCollisionDispatcher::RemoveManifoldByCollisionObject( btCollisionObject *_colObj) { - std::cerr << " ========== GzCollisionDispatcher remove manifold by col " << std::endl; std::unordered_set manifoldsToRemove; for (const auto& manifold : this->customManifolds) { @@ -65,7 +61,6 @@ void GzCollisionDispatcher::RemoveManifoldByCollisionObject( btCollisionDispatcher::releaseManifold(manifold); this->customManifolds.erase(manifold); } - std::cerr << " ========== GzCollisionDispatcher remove manifold by col done " << std::endl; } ///////////////////////////////////////////////// @@ -135,9 +130,9 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher) { - std::cerr << " ========== GzCollisionDispatcher dispatch all collision pairs" << std::endl; btCollisionDispatcher::dispatchAllCollisionPairs( pairCache, dispatchInfo, dispatcher); + return; // Loop through all the contact manifolds. // Find convex decomposed mesh collision shapes. @@ -221,14 +216,11 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( ///////////////////////////////////////////////// void GzCollisionDispatcher::releaseManifold(btPersistentManifold *_manifold) { - std::cerr << " ========== GzCollisionDispatcher release manifold" << std::endl; auto manifoldIt = this->manifoldsToKeep.find(_manifold); if (manifoldIt != this->manifoldsToKeep.end()) this->manifoldsToKeep.erase(manifoldIt); - std::cerr << " ========== GzCollisionDispatcher release manifold 0 " << std::endl; btCollisionDispatcher::releaseManifold(_manifold); - std::cerr << " ========== GzCollisionDispatcher release manifold done" << std::endl; } ///////////////////////////////////////////////// @@ -237,10 +229,8 @@ WorldInfo::WorldInfo(std::string name_) { this->collisionConfiguration = std::make_unique(); - std::cerr << " ========================= creating dispatcher " << std::endl; this->dispatcher = std::make_unique(collisionConfiguration.get()); - std::cerr << " ========================= creating dispatcher done " << std::endl; this->broadphase = std::make_unique(); this->solver = std::make_unique(); this->world = std::make_unique( From a33acf11cac412c2cc91a55ab7a78dbc66c07dac Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 12 Jul 2024 18:25:48 +0000 Subject: [PATCH 17/26] debugging Signed-off-by: Ian Chen --- bullet-featherstone/src/SimulationFeatures.cc | 5 +++++ test/common_test/collisions.cc | 1 + 2 files changed, 6 insertions(+) diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index b6c6f1e03..4f99cf60f 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -59,6 +59,7 @@ void SimulationFeatures::WorldForwardStep( std::vector SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const { + std::cerr << " get contacts from last step " << std::endl; std::vector outContacts; auto *const world = this->ReferenceInterface(_worldID); if (!world) @@ -67,6 +68,8 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const } GzCollisionDispatcher *dispatcher = dynamic_cast(world->world->getDispatcher()); + if (!dispatcher) + return; int numManifolds = world->world->getDispatcher()->getNumManifolds(); for (int i = 0; i < numManifolds; i++) @@ -122,6 +125,8 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const convert(pt.getPositionWorldOnA()), extraData}); } } + + std::cerr << " get contacts from last step done " << std::endl; return outContacts; } diff --git a/test/common_test/collisions.cc b/test/common_test/collisions.cc index 3122294d8..4bd45382b 100644 --- a/test/common_test/collisions.cc +++ b/test/common_test/collisions.cc @@ -520,6 +520,7 @@ TEST_F(CollisionMeshTestFeaturesList, MeshContacts) EXPECT_LT(contactSize, contacts.size()); } } + std::cerr << " ======== done with mesh contacts test " << std::endl; } using CollisionStaticFeaturesList = gz::physics::FeatureList< From bc5991602b921a5eaa95012e29adc0a30712da39 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 12 Jul 2024 18:26:23 +0000 Subject: [PATCH 18/26] debugging Signed-off-by: Ian Chen --- bullet-featherstone/src/SimulationFeatures.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index 4f99cf60f..3be5ebd08 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -69,7 +69,7 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const GzCollisionDispatcher *dispatcher = dynamic_cast(world->world->getDispatcher()); if (!dispatcher) - return; + return outContacts; int numManifolds = world->world->getDispatcher()->getNumManifolds(); for (int i = 0; i < numManifolds; i++) From 88ecdad6af810035c43618c165e3f92697816ea3 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 15 Jul 2024 23:25:54 +0000 Subject: [PATCH 19/26] add more debugging Signed-off-by: Ian Chen --- bullet-featherstone/src/SimulationFeatures.cc | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index 3be5ebd08..a727bd33a 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -66,21 +66,33 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const { return outContacts; } + + std::cerr << " get contacts from last step getting dispatcher " << std::endl; GzCollisionDispatcher *dispatcher = dynamic_cast(world->world->getDispatcher()); + + std::cerr << " get contacts from last step getting dispatcher 1 " << std::endl; if (!dispatcher) return outContacts; + std::cerr << " get contacts from last step getting dispatcher done " << std::endl; + int numManifolds = world->world->getDispatcher()->getNumManifolds(); for (int i = 0; i < numManifolds; i++) { btPersistentManifold* contactManifold = world->world->getDispatcher()->getManifoldByIndexInternal(i); + + std::cerr << " get contacts from last step get multi body link collider " << i << std::endl; + const btMultiBodyLinkCollider* ob0 = dynamic_cast(contactManifold->getBody0()); const btMultiBodyLinkCollider* ob1 = dynamic_cast(contactManifold->getBody1()); + std::cerr << " get contacts from last step cast compound shape " << i << ": " + << contactManifold->getNumContacts() << std::endl; + const btCompoundShape *compoundShape0 = dynamic_cast(ob0->getCollisionShape()); const btCompoundShape *compoundShape1 = @@ -91,8 +103,10 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const { btManifoldPoint& pt = contactManifold->getContactPoint(j); + std::cerr << " get contacts from last step find col shape 0 " << j << std::endl; const btCollisionShape *colShape0 = dispatcher->FindCollisionShape( compoundShape0, pt.m_index0); + std::cerr << " get contacts from last step find col shape 1" << j << std::endl; const btCollisionShape *colShape1 = dispatcher->FindCollisionShape( compoundShape1, pt.m_index1); From dcca698d99889aa2640782c9a605a560e5a9b8c4 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 16 Jul 2024 01:57:28 +0000 Subject: [PATCH 20/26] check nulls Signed-off-by: Ian Chen --- bullet-featherstone/src/SimulationFeatures.cc | 28 +++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index a727bd33a..a7ba166bf 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -93,14 +93,38 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const std::cerr << " get contacts from last step cast compound shape " << i << ": " << contactManifold->getNumContacts() << std::endl; + ///////////// + std::cerr << " get contacts from last step cast ob0 ob1 " + << ob0 << " " << ob1 << std::endl; + + if (!ob0 || !ob1) + continue; + + const btCollisionShape *linkShape0 = ob0->getCollisionShape(); + const btCollisionShape *linkShape1 = ob1->getCollisionShape(); + + std::cerr << " get contacts from last step cast to link col shape " + << std::endl; + + if (!linkShape0 || !linkShape1 || + !linkShape0->isCompound() || !linkShape1->isCompound()) + continue; + const btCompoundShape *compoundShape0 = - dynamic_cast(ob0->getCollisionShape()); + dynamic_cast(linkShape0); const btCompoundShape *compoundShape1 = - dynamic_cast(ob1->getCollisionShape()); + dynamic_cast(linkShape1); + + std::cerr << " get contacts from last step cast to link col shape done " + << std::endl; + int numContacts = contactManifold->getNumContacts(); for (int j = 0; j < numContacts; j++) { + std::cerr << " get contacts from last step get contact point " + << std::endl; + btManifoldPoint& pt = contactManifold->getContactPoint(j); std::cerr << " get contacts from last step find col shape 0 " << j << std::endl; From 51fa0d3f09007bdb7b22faf914289c605786aecf Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 16 Jul 2024 04:11:55 +0000 Subject: [PATCH 21/26] more debugging Signed-off-by: Ian Chen --- bullet-featherstone/src/SimulationFeatures.cc | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index a7ba166bf..58e46d04f 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -106,12 +106,30 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const std::cerr << " get contacts from last step cast to link col shape " << std::endl; + std::cerr << " get contacts from last step is compound 0 " + << std::endl; + std::cerr << linkShape0->isCompound() + << std::endl; + std::cerr << " get contacts from last step is compound 1 " + << std::endl; + std::cerr << linkShape1->isCompound() + << std::endl; + std::cerr << " get contacts from last step is compound done " + << std::endl; + if (!linkShape0 || !linkShape1 || !linkShape0->isCompound() || !linkShape1->isCompound()) continue; + std::cerr << " get contacts from last step cast to link col before cast 0" + << std::endl; + const btCompoundShape *compoundShape0 = dynamic_cast(linkShape0); + + std::cerr << " get contacts from last step cast to link col before cast 1" + << std::endl; + const btCompoundShape *compoundShape1 = dynamic_cast(linkShape1); From e34bffedee950b4e5c49f71d1c9d9475849c07d3 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 16 Jul 2024 16:47:12 +0000 Subject: [PATCH 22/26] test static cast Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 4 ++-- bullet-featherstone/src/SimulationFeatures.cc | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index a8cebf3bc..b5d5287ab 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -170,9 +170,9 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( } const btCompoundShape *compoundShape0 = - dynamic_cast(ob0->getCollisionShape()); + static_cast(ob0->getCollisionShape()); const btCompoundShape *compoundShape1 = - dynamic_cast(ob1->getCollisionShape()); + static_cast(ob1->getCollisionShape()); int numContacts = contactManifold->getNumContacts(); for (int j = 0; j < numContacts; ++j) diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index 58e46d04f..b37ad06a0 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -125,13 +125,13 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const << std::endl; const btCompoundShape *compoundShape0 = - dynamic_cast(linkShape0); + static_cast(linkShape0); std::cerr << " get contacts from last step cast to link col before cast 1" << std::endl; const btCompoundShape *compoundShape1 = - dynamic_cast(linkShape1); + static_cast(linkShape1); std::cerr << " get contacts from last step cast to link col shape done " << std::endl; From cc9bc62bda36cfcd19e360a86901705ebd2dc372 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 16 Jul 2024 16:51:15 +0000 Subject: [PATCH 23/26] test static cast Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index b5d5287ab..fb89efc5e 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -71,7 +71,7 @@ bool GzCollisionDispatcher::HasConvexHullChildShapes( return false; const btCompoundShape *compoundShape = - dynamic_cast(_shape); + static_cast(_shape); return (compoundShape->getNumChildShapes() > 0 && compoundShape->getChildShape(0)->getShapeType() == CONVEX_HULL_SHAPE_PROXYTYPE); From c561ee517062e535ce980b32243f73af58fe6f24 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 16 Jul 2024 18:47:21 +0000 Subject: [PATCH 24/26] enable gz collision dispatcher Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index fb89efc5e..388a4ba46 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -132,7 +132,6 @@ void GzCollisionDispatcher::dispatchAllCollisionPairs( { btCollisionDispatcher::dispatchAllCollisionPairs( pairCache, dispatchInfo, dispatcher); - return; // Loop through all the contact manifolds. // Find convex decomposed mesh collision shapes. From c992e4efb87f2e7e098dd2052e74e422de141e39 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 17 Jul 2024 00:23:59 +0000 Subject: [PATCH 25/26] cleanup Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.hh | 1 + bullet-featherstone/src/SimulationFeatures.cc | 45 ------------------- 2 files changed, 1 insertion(+), 45 deletions(-) diff --git a/bullet-featherstone/src/SDFFeatures.hh b/bullet-featherstone/src/SDFFeatures.hh index 2e8d3c9df..19a3c463a 100644 --- a/bullet-featherstone/src/SDFFeatures.hh +++ b/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()); diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index b37ad06a0..ec0745a12 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -59,7 +59,6 @@ void SimulationFeatures::WorldForwardStep( std::vector SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const { - std::cerr << " get contacts from last step " << std::endl; std::vector outContacts; auto *const world = this->ReferenceInterface(_worldID); if (!world) @@ -67,88 +66,45 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const return outContacts; } - std::cerr << " get contacts from last step getting dispatcher " << std::endl; GzCollisionDispatcher *dispatcher = dynamic_cast(world->world->getDispatcher()); - std::cerr << " get contacts from last step getting dispatcher 1 " << std::endl; if (!dispatcher) return outContacts; - std::cerr << " get contacts from last step getting dispatcher done " << std::endl; - int numManifolds = world->world->getDispatcher()->getNumManifolds(); for (int i = 0; i < numManifolds; i++) { btPersistentManifold* contactManifold = world->world->getDispatcher()->getManifoldByIndexInternal(i); - std::cerr << " get contacts from last step get multi body link collider " << i << std::endl; - const btMultiBodyLinkCollider* ob0 = dynamic_cast(contactManifold->getBody0()); const btMultiBodyLinkCollider* ob1 = dynamic_cast(contactManifold->getBody1()); - std::cerr << " get contacts from last step cast compound shape " << i << ": " - << contactManifold->getNumContacts() << std::endl; - - ///////////// - std::cerr << " get contacts from last step cast ob0 ob1 " - << ob0 << " " << ob1 << std::endl; - if (!ob0 || !ob1) continue; const btCollisionShape *linkShape0 = ob0->getCollisionShape(); const btCollisionShape *linkShape1 = ob1->getCollisionShape(); - std::cerr << " get contacts from last step cast to link col shape " - << std::endl; - - std::cerr << " get contacts from last step is compound 0 " - << std::endl; - std::cerr << linkShape0->isCompound() - << std::endl; - std::cerr << " get contacts from last step is compound 1 " - << std::endl; - std::cerr << linkShape1->isCompound() - << std::endl; - std::cerr << " get contacts from last step is compound done " - << std::endl; - if (!linkShape0 || !linkShape1 || !linkShape0->isCompound() || !linkShape1->isCompound()) continue; - std::cerr << " get contacts from last step cast to link col before cast 0" - << std::endl; - const btCompoundShape *compoundShape0 = static_cast(linkShape0); - - std::cerr << " get contacts from last step cast to link col before cast 1" - << std::endl; - const btCompoundShape *compoundShape1 = static_cast(linkShape1); - std::cerr << " get contacts from last step cast to link col shape done " - << std::endl; - - int numContacts = contactManifold->getNumContacts(); for (int j = 0; j < numContacts; j++) { - std::cerr << " get contacts from last step get contact point " - << std::endl; - btManifoldPoint& pt = contactManifold->getContactPoint(j); - std::cerr << " get contacts from last step find col shape 0 " << j << std::endl; const btCollisionShape *colShape0 = dispatcher->FindCollisionShape( compoundShape0, pt.m_index0); - std::cerr << " get contacts from last step find col shape 1" << j << std::endl; const btCollisionShape *colShape1 = dispatcher->FindCollisionShape( compoundShape1, pt.m_index1); @@ -182,7 +138,6 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const } } - std::cerr << " get contacts from last step done " << std::endl; return outContacts; } From e5b8120c1316d133cfa984cdd367cb98e160c5d2 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 17 Jul 2024 01:26:36 +0000 Subject: [PATCH 26/26] more cleanup Signed-off-by: Ian Chen --- test/common_test/collisions.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/test/common_test/collisions.cc b/test/common_test/collisions.cc index 4bd45382b..3122294d8 100644 --- a/test/common_test/collisions.cc +++ b/test/common_test/collisions.cc @@ -520,7 +520,6 @@ TEST_F(CollisionMeshTestFeaturesList, MeshContacts) EXPECT_LT(contactSize, contacts.size()); } } - std::cerr << " ======== done with mesh contacts test " << std::endl; } using CollisionStaticFeaturesList = gz::physics::FeatureList<