From a02e891679c09a4850284012506abc7c06da1116 Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@openrobotics.org>
Date: Wed, 26 Jun 2024 23:26:07 +0000
Subject: [PATCH] Set max contacts for collision pairs (#2270)

Signed-off-by: Ian Chen <ichen@openrobotics.org>
Co-authored-by: Addisu Z. Taddese <addisu@openrobotics.org>
---
 src/systems/physics/Physics.cc | 38 +++++++++++++++++++++++++++++++++-
 1 file changed, 37 insertions(+), 1 deletion(-)

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index b3581ae3e8..0a6564231f 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -617,6 +617,14 @@ class gz::sim::systems::PhysicsPrivate
   public: struct SolverFeatureList : gz::physics::FeatureList<
             gz::physics::Solver>{};
 
+  //////////////////////////////////////////////////
+  // CollisionPairMaxContacts
+  /// \brief Feature list for setting and getting the max total contacts for
+  /// collision pairs
+  public: struct CollisionPairMaxContactsFeatureList :
+            gz::physics::FeatureList<
+            gz::physics::CollisionPairMaxContacts>{};
+
   //////////////////////////////////////////////////
   // Nested Models
 
@@ -642,7 +650,8 @@ class gz::sim::systems::PhysicsPrivate
           NestedModelFeatureList,
           CollisionDetectorFeatureList,
           SolverFeatureList,
-          WorldModelFeatureList
+          WorldModelFeatureList,
+          CollisionPairMaxContactsFeatureList
           >;
 
   /// \brief A map between world entity ids in the ECM to World Entities in
@@ -1045,6 +1054,33 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm,
           }
         }
 
+        auto physicsComp =
+            _ecm.Component<components::Physics>(_entity);
+        if (physicsComp)
+        {
+          auto maxContactsFeature =
+              this->entityWorldMap.EntityCast<
+              CollisionPairMaxContactsFeatureList>(_entity);
+          if (!maxContactsFeature)
+          {
+            static bool informed{false};
+            if (!informed)
+            {
+              gzdbg << "Attempting to set physics options, but the "
+                     << "phyiscs engine doesn't support feature "
+                     << "[CollisionPairMaxContacts]. "
+                     << "Options will be ignored."
+                     << std::endl;
+              informed = true;
+            }
+          }
+          else
+          {
+            maxContactsFeature->SetCollisionPairMaxContacts(
+              physicsComp->Data().MaxContacts());
+          }
+        }
+
         // World Model proxy (used for joints directly under <world> in SDF)
         auto worldModelFeature =
             this->entityWorldMap.EntityCast<WorldModelFeatureList>(_entity);