Remove contact pruning with box-box collisions in nativeccd and add benchmarks.

PiperOrigin-RevId: 744732382
Change-Id: I2e20b646c541ee99888f1e964302d1b4363b2dc4

src/engine/engine_collision_convex.c

@@ -908,24 +908,31 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9],
 
 
 
-// return true if multiccd can run in a single pass
+// return number of contacts supported by a single pass of narrowphase
-static int singlePass(const mjCCDObj* obj1, const mjCCDObj* obj2) {
+static int maxContacts(const mjCCDObj* obj1, const mjCCDObj* obj2) {
   const mjModel* m = obj1->model;
 
   // single pass not supported for margins
   if (obj1->margin > 0 || obj2->margin > 0) {
-    return 0;
+    return 1;
   }
 
-  // supported geoms for single pass
+  // can return 8 contacts for box-box collision in one pass
   int type1 = m->geom_type[obj1->geom];
   int type2 = m->geom_type[obj2->geom];
+  if (type1 == mjGEOM_BOX && type2 == mjGEOM_BOX) {
+    return 8;
+  }
+
+  // reduce mesh collisions to 4 contacts max
   if (type1 == mjGEOM_BOX || type1 == mjGEOM_MESH) {
     if (type2 == mjGEOM_BOX || type2 == mjGEOM_MESH) {
-      return 1;
+      return mjENABLED(mjENBL_MULTICCD) ? 4 : 1;
     }
   }
-  return 0;
+
+  // not supported for other geom types
+  return 1;
 }
 
 
@@ -937,17 +944,14 @@ int mjc_Convex(const mjModel* m, const mjData* d,
   mjCCDObj obj1, obj2;
   mjc_initCCDObj(&obj1, m, d, g1, margin);
   mjc_initCCDObj(&obj2, m, d, g2, margin);
-  int max_contacts = 1;
+  int max_contacts = maxContacts(&obj1, &obj2);
-
-  if (mjENABLED(mjENBL_MULTICCD) && singlePass(&obj1, &obj2)) {
-    max_contacts = 4;
-  }
 
   // find initial contact
   int ncon = mjc_CCDIteration(m, d, &obj1, &obj2, con, max_contacts, margin);
 
-  // nativeccd supports multi Box-Box collision directly
+
-  if (!mjDISABLED(mjDSBL_NATIVECCD) && singlePass(&obj1, &obj2)) {
+  // no additional contacts needed
+  if (!mjDISABLED(mjDSBL_NATIVECCD) && max_contacts > 1) {
     return ncon;
   }
 

src/engine/engine_collision_convex.h

@@ -81,12 +81,12 @@ void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]);
 void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]);
 
 // pairwise geom collision functions using ccd
-int mjc_PlaneConvex   (const mjModel* m, const mjData* d,
+int mjc_PlaneConvex(const mjModel* m, const mjData* d,
-                       mjContact* con, int g1, int g2, mjtNum margin);
+                    mjContact* con, int g1, int g2, mjtNum margin);
-int mjc_ConvexHField  (const mjModel* m, const mjData* d,
+int mjc_ConvexHField(const mjModel* m, const mjData* d,
-                       mjContact* con, int g1, int g2, mjtNum margin);
+                     mjContact* con, int g1, int g2, mjtNum margin);
-int mjc_Convex        (const mjModel* m, const mjData* d,
+MJAPI int mjc_Convex(const mjModel* m, const mjData* d,
-                       mjContact* con, int g1, int g2, mjtNum margin);
+                     mjContact* con, int g1, int g2, mjtNum margin);
 
 // geom-elem or elem-elem or vert-elem collision function using ccd
 int mjc_ConvexElem    (const mjModel* m, const mjData* d, mjContact* con,

src/engine/engine_collision_gjk.c

@@ -1863,36 +1863,38 @@ static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj,
   const mjtNum* mat = obj->data->geom_xmat + 3*g;
 
   if (dim == 3) {
+    int c = 0;
     int x = ((v1 & 1) && (v2 & 1) && (v3 & 1)) - (!(v1 & 1) && !(v2 & 1) && !(v3 & 1));
     int y = ((v1 & 2) && (v2 & 2) && (v3 & 2)) - (!(v1 & 2) && !(v2 & 2) && !(v3 & 2));
     int z = ((v1 & 4) && (v2 & 4) && (v3 & 4)) - (!(v1 & 4) && !(v2 & 4) && !(v3 & 4));
     globalcoord(res, mat, NULL, x, y, z);
     int sgn = x + y + z;
-    if (x) resind[0] = 0;
+    if (x) resind[c++] = 0;
-    if (y) resind[0] = 2;
+    if (y) resind[c++] = 2;
-    if (z) resind[0] = 4;
+    if (z) resind[c++] = 4;
     if (sgn == -1) resind[0]++;
-    return 1;
+    return c == 1 ? 1 : 0;  // return 1 only if vertices make a valid face
   }
 
   if (dim == 2) {
+    int c = 0;
     int x = ((v1 & 1) && (v2 & 1)) - (!(v1 & 1) && !(v2 & 1));
     int y = ((v1 & 2) && (v2 & 2)) - (!(v1 & 2) && !(v2 & 2));
     int z = ((v1 & 4) && (v2 & 4)) - (!(v1 & 4) && !(v2 & 4));
     if (x) {
       globalcoord(res, mat, NULL, x, 0, 0);
-      resind[0] = (x > 0) ? 0 : 1;
+      resind[c++] = (x > 0) ? 0 : 1;
     }
     if (y) {
-      int i = (x ? 1 : 0);
+      globalcoord(res + 3*c, mat, NULL, 0, y, 0);
-      globalcoord(res + 3*i, mat, NULL, 0, y, 0);
+      resind[c++] = (y > 0) ? 2 : 3;
-      resind[i] = (y > 0) ? 2 : 3;
     }
     if (z) {
       globalcoord(res + 3, mat, NULL, 0, 0, z);
-      resind[1] = (z > 0) ? 4 : 5;
+      resind[c++] = (z > 0) ? 4 : 5;
     }
-    return 2;
+    // TODO(kylebayes): Should be able to recover multiple contacts here.
+    return c == 2 ? 2 : 0;
   }
 
   if (dim == 1) {

test/benchmark/CMakeLists.txt

@@ -16,7 +16,7 @@
 mujoco_test(
   ccd_benchmark_test
   MAIN_TARGET benchmark::benchmark_main
-  ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers
+  ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers ccd
 )
 
 mujoco_test(

test/benchmark/ccd_benchmark_test.cc

@@ -24,6 +24,9 @@
 #include <mujoco/mujoco.h>
 #include "test/fixture.h"
 
+#include "src/engine/engine_collision_convex.h"
+#include "src/engine/engine_collision_primitive.h"
+
 namespace mujoco {
 namespace {
 
@@ -116,11 +119,19 @@ void ABSL_ATTRIBUTE_NO_TAIL_CALL
 BENCHMARK(BM_BoxMesh_LibCCD);
 
 void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_BoxBox(benchmark::State& state) {
-  static TestHarness harness(kBoxBoxPath, "box.xml (BoxBox)");
+  static TestHarness harness(kBoxBoxPath, "box.xml (BoxBox)", mjDSBL_NATIVECCD);
   harness.RunBenchmark(state);
 }
 BENCHMARK(BM_BoxBox);
 
+void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_BoxBox_NativeCCD(benchmark::State& state) {
+  mjCOLLISIONFUNC[mjGEOM_BOX][mjGEOM_BOX] = mjc_Convex;
+  static TestHarness harness(kBoxBoxPath, "box.xml (NativeCCD)");
+  harness.RunBenchmark(state);
+  mjCOLLISIONFUNC[mjGEOM_BOX][mjGEOM_BOX] = mjc_BoxBox;
+}
+BENCHMARK(BM_BoxBox_NativeCCD);
+
 void ABSL_ATTRIBUTE_NO_TAIL_CALL
     BM_Ellipsoid_NativeCCD(benchmark::State& state) {
   static TestHarness harness(kEllipsoidPath, "ellipsoid.xml (nativeccd)");