Compute witness points from EPA, and use witness points to determine pos and dir for contact.

PiperOrigin-RevId: 665791337 Change-Id: If33ca1f455804c4cda534508803f5c3e99c3c52a
2024-08-21 03:05:17 -07:00
parent cf3e60f9a4
commit f986a52e1e
2 changed files with 49 additions and 62 deletions
@@ -70,6 +70,7 @@ mjtNum run_gjkPenetration(mjModel* m, mjData* d, int g1, int g2,
  mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0},
                   mjc_center, mjc_support};
  ccd_t ccd;
+  // CCD_INIT(&ccd);  // uncomment to run ccdMPRPenetration
  ccd.mpr_tolerance = kTolerance;
  ccd.epa_tolerance = kTolerance;
  ccd.max_iterations = kMaxIterations;