undo changes in simulation features

Signed-off-by: Ian Chen <[email protected]>
gazebosim · Jul 10, 2024 · ba6f4d0 · ba6f4d0
1 parent 5fe67d0
commit ba6f4d0
Showing 1 changed file with 22 additions and 27 deletions.
diff --git 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<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* ob0 =
+    const btMultiBodyLinkCollider* obA =
       dynamic_cast<const btMultiBodyLinkCollider*>(contactManifold->getBody0());
-    const btMultiBodyLinkCollider* ob1 =
+    const btMultiBodyLinkCollider* obB =
       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();
 
-    const btCompoundShape *compoundShape0 =
-        dynamic_cast<const btCompoundShape *>(ob0->getCollisionShape());
-    const btCompoundShape *compoundShape1 =
-        dynamic_cast<const btCompoundShape *>(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<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.
@@ -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});
       }
   }