chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Transform inputs to global before calling rayMesh in SDF collisions.

Browse Source 
PiperOrigin-RevId: 802523399
Change-Id: I889b020e4e721fbf471992f5c87446df601ba766
This commit is contained in:
Alessio Quaglino
2025-09-03 06:11:41 -07:00
committed by Copybara-Service
parent 7a93e22514
commit 767996ab17
1 changed files with 11 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_sdf.c
+11 -7
View File
@@ -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;