Avoid duplicate contact points in compound bodies.

Sometimes broadphase returns the same body pair multiple times. In those cases, avoid creating every contact twice.

PiperOrigin-RevId: 499869235
Change-Id: I58606168b0723d40b4fdd43b1fafad79bf35d76d

src/engine/engine_collision_driver.c

@@ -51,7 +51,7 @@ mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
 //----------------------------- collision detection entry point ------------------------------------
 
 void mj_collision(const mjModel* m, mjData* d) {
-  int g1, g2, signature, merged, b1 = 0, b2 = 0, exadr = 0, pairadr = 0, startadr;
+  int g1, g2, merged, b1 = 0, b2 = 0, exadr = 0, pairadr = 0, startadr;
   int nexclude = m->nexclude, npair = m->npair, nbodypair = ((m->nbody-1)*m->nbody)/2;
   int *broadphasepair = 0;
   mjMARKSTACK;
@@ -77,6 +77,7 @@ void mj_collision(const mjModel* m, mjData* d) {
     // call broadphase collision detector
     broadphasepair = (int*)mj_stackAlloc(d, (m->nbody*(m->nbody-1))/2);
     nbodypair = mj_broadphase(m, d, broadphasepair, (m->nbody*(m->nbody-1))/2);
+    unsigned int last_signature = -1;
 
     // loop over body pairs (broadphase or all)
     for (int i=0; i<nbodypair; i++) {
@@ -85,7 +86,13 @@ void mj_collision(const mjModel* m, mjData* d) {
       b2 = broadphasepair[i] & 0xFFFF;
 
       // compute signature for this body pair
-      signature = ((b1+1)<<16) + (b2+1);
+      unsigned int signature = ((b1+1)<<16) + (b2+1);
+      // pairs come sorted by signature, but may not be unique
+      // if signature is repeated, skip it
+      if (signature == last_signature) {
+        continue;
+      }
+      last_signature = signature;
 
       // merge predefined pairs
       merged = 0;

test/engine/engine_collision_driver_test.cc

@@ -31,6 +31,7 @@ using MjCollisionTest = MujocoTest;
 using GeomPair = std::pair<std::string, std::string>;
 using ::testing::IsEmpty;
 using ::testing::ElementsAre;
+using ::testing::NotNull;
 
 // Returns a sorted list of pairs of colliding geom names, where each pair of
 // geom names is sorted.
@@ -108,5 +109,41 @@ TEST_F(MjCollisionTest, ZeroedHessian) {
   mj_deleteData(data);
   mj_deleteModel(model);
 }
+
+TEST_F(MjCollisionTest, ContactCount) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <geom type="plane" size="5 5 .01"/>
+      </body>
+      <body pos="0 0 0.9">
+        <freejoint/>
+        <geom type="sphere" size="1" pos="-1 -1 0"/>
+        <geom type="sphere" size="1" pos="-1  1 0"/>
+        <geom type="sphere" size="1" pos=" 1 -1 0"/>
+        <geom type="sphere" size="1" pos=" 1  1 0"/>
+        <geom type="sphere" size="1" pos="-2 -2 0"/>
+        <geom type="sphere" size="1" pos="-2  2 0"/>
+        <geom type="sphere" size="1" pos=" 2 -2 0"/>
+        <geom type="sphere" size="1" pos=" 2  2 0"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  mjModel* m = LoadModelFromString(xml);
+  ASSERT_THAT(m, NotNull());
+  mjData* d = mj_makeData(m);
+  ASSERT_THAT(d, NotNull());
+
+  mj_forward(m, d);
+
+  // there are 8 spheres, all touching the floor
+  EXPECT_EQ(d->ncon, 8);
+
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
 }  // namespace
 }  // namespace mujoco