From f6f9ff537b6145f8f3c9b81a9f9305eccc193d8b Mon Sep 17 00:00:00 2001
From: Kyle Bayes <kylebayes@google.com>
Date: Thu, 16 May 2024 15:59:25 -0700
Subject: [PATCH] Wrap LibCCD MPR function in a macro and initialize ccd_t
 struct.

PiperOrigin-RevId: 634556417
Change-Id: I6a980e9b86c082523758fa401fbc75fcdc798c6b
---
 src/engine/engine_collision_convex.c | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c
index e6ea9b0d..08db81d3 100644
--- a/src/engine/engine_collision_convex.c
+++ b/src/engine/engine_collision_convex.c
@@ -29,6 +29,8 @@
 #include "engine/engine_util_misc.h"
 #include "engine/engine_util_spatial.h"
 
+// the LibCCD penetration function we use (ccdMPRPenetration or ccdGJKPenetration)
+#define _mjCCDPENETRATION ccdMPRPenetration
 
 // ccd center function
 void mjccd_center(const void *obj, ccd_vec3_t *center) {
@@ -273,7 +275,7 @@ static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd,
                             mjContact* con, mjtNum margin) {
   ccd_vec3_t dir, pos;
   ccd_real_t depth;
-  if (ccdMPRPenetration(obj1, obj2, ccd, &depth, &dir, &pos) == 0) {
+  if (_mjCCDPENETRATION(obj1, obj2, ccd, &depth, &dir, &pos) == 0) {
     // contact is found but normal is undefined
     if (ccdVec3Eq(&dir, ccd_vec3_origin)) {
       return 0;
@@ -343,6 +345,7 @@ int mjc_Convex(const mjModel* m, const mjData* d,
   mjtCCD obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}};
 
   // init ccd structure
+  CCD_INIT(&ccd);
   ccd.first_dir = ccdFirstDirDefault;
   ccd.center1 = mjccd_center;
   ccd.center2 = mjccd_center;
@@ -747,6 +750,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
   //------------------------------------- collision testing
 
   // init ccd structure
+  CCD_INIT(&ccd);
   ccd.first_dir = prism_firstdir;
   ccd.center1 = prism_center;
   ccd.center2 = mjccd_center;
@@ -787,7 +791,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
           }
 
           // run MPR, save contact
-          if (ccdMPRPenetration(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0 &&
+          if (_mjCCDPENETRATION(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0 &&
               !ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
             // fill in contact data, transform to global coordinates
             con[cnt].dist = -depth;
@@ -1092,6 +1096,7 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
   mjtCCD obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}};
 
   // init ccd structure
+  CCD_INIT(&ccd);
   ccd.first_dir = ccdFirstDirDefault;
   ccd.center1 = mjccd_center;
   ccd.center2 = mjccd_center;
@@ -1197,6 +1202,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
   //------------------------------------- collision testing
 
   // init ccd structure
+  CCD_INIT(&ccd);
   ccd.first_dir = prism_firstdir;
   ccd.center1 = prism_center;
   ccd.center2 = mjccd_center;
@@ -1234,7 +1240,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
           }
 
           // run MPR, save contact
-          if (ccdMPRPenetration(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0) {
+          if (_mjCCDPENETRATION(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0) {
             if (!ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
               // fill in contact data, transform to global coordinates
               con[cnt].dist = -depth;
@@ -1266,3 +1272,5 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
 
   return cnt;
 }
+
+#undef _mjCCDPENETRATION