From 4da2ddce29f48ecff32b6a75dc8cd6d90c3460c5 Mon Sep 17 00:00:00 2001
From: Kyle Bayes <kylebayes@google.com>
Date: Thu, 16 Jan 2025 06:21:34 -0800
Subject: [PATCH] Only correct box-box collisions with the mjc_BoxBox function.

PiperOrigin-RevId: 716209565
Change-Id: I3419ce82f9934606bed80acd8372284fc7ac7ac8
---
 src/engine/engine_collision_driver.c | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c
index f46d8157..36c81652 100644
--- a/src/engine/engine_collision_driver.c
+++ b/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;