From db72d0d53c14b55d2dff349cbe31f7d91c9f279e Mon Sep 17 00:00:00 2001
From: Kyle Bayes <kylebayes@google.com>
Date: Mon, 21 Jul 2025 05:08:29 -0700
Subject: [PATCH] Clamp dist in mj_geomDistanceCCD. Closes #2710

PiperOrigin-RevId: 785393424
Change-Id: I508cd2ad6f47ca022cc54f228247ca498fda691f
---
 src/engine/engine_support.c | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c
index 204aa28a..907dcc8d 100644
--- a/src/engine/engine_support.c
+++ b/src/engine/engine_support.c
@@ -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;
 }