From 767996ab17999d9f8a3c76dea362d0694118a1d4 Mon Sep 17 00:00:00 2001
From: Alessio Quaglino <quaglino@google.com>
Date: Wed, 3 Sep 2025 06:11:41 -0700
Subject: [PATCH] Transform inputs to global before calling rayMesh in SDF
 collisions.

PiperOrigin-RevId: 802523399
Change-Id: I889b020e4e721fbf471992f5c87446df601ba766
---
 src/engine/engine_collision_sdf.c | 18 +++++++++++-------
 1 file changed, 11 insertions(+), 7 deletions(-)

diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c
index aa5b264b..ee30adbc 100644
--- a/src/engine/engine_collision_sdf.c
+++ b/src/engine/engine_collision_sdf.c
@@ -274,12 +274,14 @@ static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p
     if (m->mesh_octnum[i]) {
       return oct_distance(m, x, i);
     } else {
-      mjtNum dir[3] = {-x[0], -x[1], -x[2]};
-      mjtNum r = mju_normalize3(dir);
-      mjtNum dist = mj_rayMesh(m, d, i, x, dir);
+      mju_mulMatVec3(a, d->geom_xmat + 9 * i, x);
+      mju_addTo3(a, d->geom_xpos + 3 * i);
+      mjtNum dir[3] = {-a[0], -a[1], -a[2]};
+      mjtNum r = mju_norm3(dir);
+      mjtNum dist = mj_rayMesh(m, d, i, a, dir);
       if (dist > r) {
         mju_scl3(dir, dir, -1);
-        return -mj_rayMesh(m, d, i, x, dir);
+        return -mj_rayMesh(m, d, i, a, dir);
       }
       return dist;
     }
@@ -391,9 +393,11 @@ static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d,
     if (m->mesh_octnum[i]) {
       oct_gradient(m, gradient, x, i);
     } else {
-      mjtNum dir[3] = {-x[0], -x[1], -x[2]};
-      mjtNum r = mju_normalize3(dir);
-      mjtNum dist = mj_rayMesh(m, d, i, x, dir);
+      mju_mulMatVec3(a, d->geom_xmat+9*i, x);
+      mju_addTo3(a, d->geom_xpos+3*i);
+      mjtNum dir[3] = {-a[0], -a[1], -a[2]};
+      mjtNum r = mju_norm3(dir);
+      mjtNum dist = mj_rayMesh(m, d, i, a, dir);
       gradient[0] = dist > r ? 1 : -1;
       gradient[1] = dist > r ? 1 : -1;
       gradient[2] = dist > r ? 1 : -1;