Only correct box-box collisions with the mjc_BoxBox function.

PiperOrigin-RevId: 716209565
Change-Id: I3419ce82f9934606bed80acd8372284fc7ac7ac8

src/engine/engine_collision_driver.c

@@ -1461,8 +1461,10 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) {
   type1 = m->geom_type[g1];
   type2 = m->geom_type[g2];
 
+  mjfCollision collisionFunc = mjCOLLISIONFUNC[type1][type2];
+
   // return if no collision function
-  if (!mjCOLLISIONFUNC[type1][type2]) {
+  if (!collisionFunc) {
     return;
   }
 
@@ -1503,7 +1505,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) {
   }
 
   // call collision detector to generate contacts
-  num = mjCOLLISIONFUNC[type1][type2](m, d, con, g1, g2, margin);
+  num = collisionFunc(m, d, con, g1, g2, margin);
 
   // check contacts
   if (!num) {
@@ -1517,7 +1519,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) {
   }
 
   // remove bad and repeated contacts in box-box
-  if (type1 == mjGEOM_BOX && type2 == mjGEOM_BOX) {
+  if (collisionFunc == mjc_BoxBox) {
     // use dim field to mark: -1: bad, 0: good
     for (int i=0; i < num; i++) {
       con[i].dim = 0;