Contact point selection

Signed-off-by: Ian Chen <[email protected]>

dartsim/src/GzOdeCollisionDetector.cc

@@ -91,21 +91,41 @@ void GzOdeCollisionDetector::LimitMaxContacts(
   auto allContacts = _result->getContacts();
   _result->clear();
 
+  // A map of collision pairs and their their contact info
+  // Contact info is stored in std::pair. The elements are:
+  // <contact count, index of last contact point (in _result)>
   std::unordered_map<dart::collision::CollisionObject *,
-      std::unordered_map<dart::collision::CollisionObject *, std::size_t>>
+      std::unordered_map<dart::collision::CollisionObject *,
+      std::pair<std::size_t, std::size_t>>>
       contactMap;
 
   for (auto &contact : allContacts)
   {
-    auto &count =
-      contactMap[contact.collisionObject1][contact.collisionObject2];
+    auto &[count, lastContactIdx] =
+        contactMap[contact.collisionObject1][contact.collisionObject2];
     count++;
-    auto &otherCount =
+    auto &[otherCount, otherLastContactIdx] =
       contactMap[contact.collisionObject2][contact.collisionObject1];
+
     std::size_t total =  count + otherCount;
     if (total <= this->maxCollisionPairContacts)
     {
+      if (total == this->maxCollisionPairContacts)
+      {
+        lastContactIdx = _result->getNumContacts();
+        otherLastContactIdx = lastContactIdx;
+      }
       _result->addContact(contact);
     }
+    else
+    {
+      // If too many contacts were generated, replace the last contact point
+      // of the collision pair with one that has a larger penetration depth
+      auto &c = _result->getContact(lastContactIdx);
+      if (contact.penetrationDepth > c.penetrationDepth)
+      {
+        c = contact;
+      }
+    }
   }
 }