From 0108b7b569fe21b1cda38ae6dcff18a6036a1641 Mon Sep 17 00:00:00 2001
From: Kyle Bayes <kylebayes@google.com>
Date: Tue, 21 May 2024 12:45:05 -0700
Subject: [PATCH] Group CCD common initialization options together.

PiperOrigin-RevId: 635896664
Change-Id: Ib3135b1137484df5606a0377052c7db001b746c9
---
 src/engine/engine_collision_convex.c | 28 +++++++++++++---------------
 1 file changed, 13 insertions(+), 15 deletions(-)

diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c
index 08db81d3..f873e612 100644
--- a/src/engine/engine_collision_convex.c
+++ b/src/engine/engine_collision_convex.c
@@ -269,6 +269,16 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
 
 
 
+// initialize CCD structure
+static void mjc_initCCD(ccd_t* ccd, const mjModel* m) {
+  CCD_INIT(ccd);
+  ccd->mpr_tolerance = m->opt.mpr_tolerance;
+  ccd->epa_tolerance = m->opt.mpr_tolerance;  // use MPR tolerance for EPA
+  ccd->max_iterations = m->opt.mpr_iterations;
+}
+
+
+
 // find single convex-convex collision, using libccd
 static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd,
                             const mjModel* m, const mjData* d,
@@ -345,17 +355,13 @@ 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);
+  mjc_initCCD(&ccd, m);
   ccd.first_dir = ccdFirstDirDefault;
   ccd.center1 = mjccd_center;
   ccd.center2 = mjccd_center;
   ccd.support1 = mjccd_support;
   ccd.support2 = mjccd_support;
 
-  // set ccd parameters
-  ccd.max_iterations = m->opt.mpr_iterations;
-  ccd.mpr_tolerance = m->opt.mpr_tolerance;
-
   // find initial contact
   int ncon = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con, margin);
 
@@ -750,17 +756,13 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
   //------------------------------------- collision testing
 
   // init ccd structure
-  CCD_INIT(&ccd);
+  mjc_initCCD(&ccd, m);
   ccd.first_dir = prism_firstdir;
   ccd.center1 = prism_center;
   ccd.center2 = mjccd_center;
   ccd.support1 = prism_support;
   ccd.support2 = mjccd_support;
 
-  // set ccd parameters
-  ccd.max_iterations = m->opt.mpr_iterations;
-  ccd.mpr_tolerance = m->opt.mpr_tolerance;
-
   // geom margin needed for actual collision test
   obj.margin = margin;
 
@@ -1096,17 +1098,13 @@ 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);
+  mjc_initCCD(&ccd, m);
   ccd.first_dir = ccdFirstDirDefault;
   ccd.center1 = mjccd_center;
   ccd.center2 = mjccd_center;
   ccd.support1 = mjccd_support;
   ccd.support2 = mjccd_support;
 
-  // set ccd parameters
-  ccd.max_iterations = m->opt.mpr_iterations;
-  ccd.mpr_tolerance = m->opt.mpr_tolerance;
-
   // find contacts
   int ncon = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con, margin);