Clamp dist in mj_geomDistanceCCD. Closes #2710

PiperOrigin-RevId: 785393424 Change-Id: I508cd2ad6f47ca022cc54f228247ca498fda691f
2025-07-21 05:08:29 -07:00
parent c2f5d0f266
commit db72d0d53c
1 changed files with 3 additions and 1 deletions
@@ -1303,12 +1303,14 @@ static mjtNum mj_geomDistanceCCD(const mjModel* m, const mjData* d, int g1, int

  mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2);

+  // witness points are only computed if dist <= distmax
  if (fromto && status.nx > 0) {
    mju_copy3(fromto, status.x1);
    mju_copy3(fromto+3, status.x2);
  }

-  return dist;
+  // clamp dist to distmax as mjc_ccd returns DBL_MAX if dist > distmax
+  return dist < distmax ? dist : distmax;
 }