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

Breaking change: Add surface normal output to MuJoCo raycast functions.

Browse Source 
PiperOrigin-RevId: 855781592
Change-Id: Id96b1ca7eaf722e260cc69d7706c28dc51f52d92
This commit is contained in:
Yuval Tassa
2026-01-13 10:21:31 -08:00
committed by Copybara-Service
parent 37762e3f70
commit 218226fc95
17 changed files with 457 additions and 326 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_sdf.c
+3 -3
View File
@@ -286,10 +286,10 @@ static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p
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);
mjtNum dist = mj_rayMesh(m, d, i, a, dir, NULL);
if (dist > r) {
mju_scl3(dir, dir, -1);
return -mj_rayMesh(m, d, i, a, dir);
return -mj_rayMesh(m, d, i, a, dir, NULL);
}
return dist;
}
@@ -414,7 +414,7 @@ static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d,
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);
mjtNum dist = mj_rayMesh(m, d, i, a, dir, NULL);
gradient[0] = dist > r ? 1 : -1;
gradient[1] = dist > r ? 1 : -1;
gradient[2] = dist > r ? 1 : -1;
src/engine/engine_ray.c
+37 -84
View File
@@ -559,8 +559,8 @@ static mjtNum ray_box(const mjtNum pos[3], const mjtNum mat[9], const mjtNum siz
// intersect ray with hfield, compute normal if given
static mjtNum mj_rayHfieldNormal(const mjModel* m, const mjData* d, int geomid,
const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) {
mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int geomid,
const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) {
// clear normal if given
if (normal) mju_zero3(normal);
@@ -738,13 +738,6 @@ static mjtNum mj_rayHfieldNormal(const mjModel* m, const mjData* d, int geomid,
}
// intersect ray with hfield
mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int geomid,
const mjtNum pnt[3], const mjtNum vec[3]) {
return mj_rayHfieldNormal(m, d, geomid, pnt, vec, NULL);
}
// ray vs axis-aligned bounding box using slab method
// see Ericson, Real-time Collision Detection section 5.3.3.
int mju_raySlab(const mjtNum aabb[6], const mjtNum xpos[3],
@@ -889,8 +882,8 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3
// intersect ray with signed distance field, compute normal if given
static mjtNum mj_raySdfNormal(const mjModel* m, const mjData* d, int g,
const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) {
static mjtNum mj_raySdf(const mjModel* m, const mjData* d, int g,
const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) {
if (normal) mju_zero3(normal);
mjtNum distance_total = 0;
@@ -956,8 +949,8 @@ static mjtNum mj_raySdfNormal(const mjModel* m, const mjData* d, int g,
}
// intersect ray with mesh, compute normal if given
static mjtNum mj_rayMeshNormal(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3],
const mjtNum vec[3], mjtNum normal[3]) {
mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3],
const mjtNum vec[3], mjtNum normal[3]) {
// clear normal if given
if (normal) mju_zero3(normal);
@@ -975,17 +968,10 @@ static mjtNum mj_rayMeshNormal(const mjModel* m, const mjData* d, int id, const
}
// intersect ray with mesh
mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3],
const mjtNum vec[3]) {
return mj_rayMeshNormal(m, d, id, pnt, vec, NULL);
}
// intersect ray and find normal with primitive geom, no meshes or hfields, compute normal if given
mjtNum mju_rayGeomNormal(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3],
const mjtNum pnt[3], const mjtNum vec[3], int geomtype,
mjtNum normal[3]) {
// intersect ray with primitive geom, no meshes or hfields, compute normal if given
mjtNum mju_rayGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3],
const mjtNum pnt[3], const mjtNum vec[3], int geomtype,
mjtNum normal[3]) {
switch ((mjtGeom) geomtype) {
case mjGEOM_PLANE:
return ray_plane(pos, mat, size, pnt, vec, normal);
@@ -1012,18 +998,11 @@ mjtNum mju_rayGeomNormal(const mjtNum pos[3], const mjtNum mat[9], const mjtNum
}
// intersect ray with primitive geom, no meshes or hfields
mjtNum mju_rayGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3],
const mjtNum pnt[3], const mjtNum vec[3], int geomtype) {
return mju_rayGeomNormal(pos, mat, size, pnt, vec, geomtype, NULL);
}
// intersect ray with flex, return nearest vertex id, compute normal if given
mjtNum mju_rayFlexNormal(const mjModel* m, const mjData* d, int flex_layer,
mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face,
mjtByte flg_skin, int flexid, const mjtNum pnt[3],
const mjtNum vec[3], int vertid[1], mjtNum normal[3]) {
mjtNum mj_rayFlex(const mjModel* m, const mjData* d, int flex_layer,
mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face,
mjtByte flg_skin, int flexid, const mjtNum pnt[3],
const mjtNum vec[3], int vertid[1], mjtNum normal[3]) {
int dim = m->flex_dim[flexid];
// clear normal if given
@@ -1102,8 +1081,8 @@ mjtNum mju_rayFlexNormal(const mjModel* m, const mjData* d, int flex_layer,
mju_quat2Mat(mat, quat);
// intersect ray with capsule
mjtNum sol = mju_rayGeomNormal(pos, mat, size, pnt, vec, mjGEOM_CAPSULE,
normal ? normal_local : NULL);
mjtNum sol = mju_rayGeom(pos, mat, size, pnt, vec, mjGEOM_CAPSULE,
normal ? normal_local : NULL);
// update
if (sol >= 0 && (x < 0 || sol < x)) {
@@ -1136,8 +1115,8 @@ mjtNum mju_rayFlexNormal(const mjModel* m, const mjData* d, int flex_layer,
size[0] = radius;
// intersect ray with sphere
mjtNum sol = mju_rayGeomNormal(vpos, NULL, size, pnt, vec, mjGEOM_SPHERE,
normal ? normal_local : NULL);
mjtNum sol = mju_rayGeom(vpos, NULL, size, pnt, vec, mjGEOM_SPHERE,
normal ? normal_local : NULL);
// update
if (sol >= 0 && (x < 0 || sol < x)) {
@@ -1208,17 +1187,6 @@ mjtNum mju_rayFlexNormal(const mjModel* m, const mjData* d, int flex_layer,
return x;
}
// intersect ray with flex, return nearest vertex id
mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer,
mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face,
mjtByte flg_skin, int flexid, const mjtNum pnt[3],
const mjtNum vec[3], int vertid[1]) {
return mju_rayFlexNormal(m, d, flex_layer, flg_vert, flg_edge, flg_face,
flg_skin, flexid, pnt, vec, vertid, NULL);
}
// intersect ray with skin, return nearest vertex id
mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
const mjtNum pnt[3], const mjtNum vec[3], int vertid[1]) {
@@ -1337,9 +1305,9 @@ static int point_in_box(const mjtNum aabb[6], const mjtNum xpos[3],
// intersect ray (pnt+x*vec, x>=0) with visible geoms, except geoms on bodyexclude
// return geomid and distance (x) to nearest surface, or -1 if no intersection
// geomgroup, flg_static are as in mjvOption; geomgroup==NULL skips group exclusion
mjtNum mj_rayNormal(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum vec[3],
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
int geomid[1], mjtNum normal[3]) {
mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum vec[3],
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
int geomid[1], mjtNum normal[3]) {
int ngeom = m->ngeom;
mjtNum dist, newdist;
mjtNum normal_local[3];
@@ -1360,13 +1328,13 @@ mjtNum mj_rayNormal(const mjModel* m, const mjData* d, const mjtNum pnt[3], cons
if (!ray_eliminate(m, d, i, geomgroup, flg_static, bodyexclude)) {
int type = m->geom_type[i];
if (type == mjGEOM_MESH) {
newdist = mj_rayMeshNormal(m, d, i, pnt, vec, p_normal);
newdist = mj_rayMesh(m, d, i, pnt, vec, p_normal);
} else if (type == mjGEOM_HFIELD) {
newdist = mj_rayHfieldNormal(m, d, i, pnt, vec, p_normal);
newdist = mj_rayHfield(m, d, i, pnt, vec, p_normal);
} else if (type == mjGEOM_SDF) {
newdist = mj_raySdfNormal(m, d, i, pnt, vec, p_normal);
newdist = mj_raySdf(m, d, i, pnt, vec, p_normal);
} else {
newdist = mju_rayGeomNormal(d->geom_xpos+3*i, d->geom_xmat+9*i,
newdist = mju_rayGeom(d->geom_xpos+3*i, d->geom_xmat+9*i,
m->geom_size+3*i, pnt, vec, type, p_normal);
}
@@ -1383,13 +1351,6 @@ mjtNum mj_rayNormal(const mjModel* m, const mjData* d, const mjtNum pnt[3], cons
}
// intersect ray
mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum vec[3],
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, int geomid[1]) {
return mj_rayNormal(m, d, pnt, vec, geomgroup, flg_static, bodyexclude, geomid, NULL);
}
// Initializes spherical bounding angles (geom_ba) and flag vector for a given source
void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3],
const mjtNum ray_xmat[9], const mjtByte* geomgroup, mjtByte flg_static,
@@ -1502,7 +1463,7 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co
// clear result
dist = -1;
*geomid = -1;
if (geomid) *geomid = -1;
if (normal) mju_zero3(normal);
// get ray spherical coordinates
@@ -1556,20 +1517,20 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co
// dispatch to type-specific ray function
int type = m->geom_type[i];
if (type == mjGEOM_MESH) {
newdist = mj_rayMeshNormal(m, d, i, pnt, vec, p_normal);
newdist = mj_rayMesh(m, d, i, pnt, vec, p_normal);
} else if (type == mjGEOM_HFIELD) {
newdist = mj_rayHfieldNormal(m, d, i, pnt, vec, p_normal);
newdist = mj_rayHfield(m, d, i, pnt, vec, p_normal);
} else if (type == mjGEOM_SDF) {
newdist = mj_raySdfNormal(m, d, i, pnt, vec, p_normal);
newdist = mj_raySdf(m, d, i, pnt, vec, p_normal);
} else {
newdist = mju_rayGeomNormal(d->geom_xpos+3*i, d->geom_xmat+9*i,
m->geom_size+3*i, pnt, vec, type, p_normal);
newdist = mju_rayGeom(d->geom_xpos+3*i, d->geom_xmat+9*i,
m->geom_size+3*i, pnt, vec, type, p_normal);
}
// update if closer intersection found
if (newdist >= 0 && (newdist < dist || dist < 0)) {
dist = newdist;
*geomid = i;
if (geomid) *geomid = i;
if (normal) mju_copy3(normal, normal_local);
}
}
@@ -1580,9 +1541,9 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co
// performs multiple ray intersections, compute normals if given
void mj_multiRayNormal(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec,
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
int* geomid, mjtNum* dist, mjtNum* normal, int nray, mjtNum cutoff) {
void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec,
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
int* geomid, mjtNum* dist, mjtNum* normal, int nray, mjtNum cutoff) {
mj_markStack(d);
// allocate source
@@ -1598,7 +1559,8 @@ void mj_multiRayNormal(const mjModel* m, mjData* d, const mjtNum pnt[3], const m
if (mju_dot3(vec+3*i, vec+3*i) < mjMINVAL) {
dist[i] = -1;
} else {
dist[i] = mju_singleRay(m, d, pnt, vec+3*i, geom_eliminate, geom_ba, geomid+i,
int* p_geomid = geomid ? geomid + i : NULL;
dist[i] = mju_singleRay(m, d, pnt, vec+3*i, geom_eliminate, geom_ba, p_geomid,
normal ? normal+3*i : NULL);
}
}
@@ -1606,12 +1568,3 @@ void mj_multiRayNormal(const mjModel* m, mjData* d, const mjtNum pnt[3], const m
mj_freeStack(d);
}
// performs multiple ray intersections with the precomputed bv and flags
void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec,
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
int* geomid, mjtNum* dist, int nray, mjtNum cutoff) {
mj_multiRayNormal(m, d, pnt, vec, geomgroup, flg_static, bodyexclude,
geomid, dist, NULL, nray, cutoff);
}
src/engine/engine_ray.h
+15 -31
View File
@@ -29,17 +29,11 @@ MJAPI void mju_multiRayPrepare(const mjModel* m, const mjData* d,
int bodyexclude, mjtNum cutoff, mjtNum* geom_ba,
int* geom_eliminate);
// intersect multiple rays emanating from a single source
// similar semantics to mj_ray, but vec is (nray x 3) and dist is (nray).
// intersect multiple rays emanating from a single source, compute normals if given
// similar semantics to mj_ray, but vec, normal and dist are arrays
MJAPI void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec,
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
int* geomid, mjtNum* dist, int nray, mjtNum cutoff);
// intersect multiple rays, compute normals if given
// similar semantics to mj_rayNormal, but vec, normal and dist are arrays.
MJAPI void mj_multiRayNormal(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec,
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
int* geomid, mjtNum* dist, mjtNum* normal, int nray, mjtNum cutoff);
int* geomid, mjtNum* dist, mjtNum* normal, int nray, mjtNum cutoff);
// intersect ray (pnt+x*vec, x>=0) with visible geoms, except geoms on bodyexclude
@@ -47,40 +41,30 @@ MJAPI void mj_multiRayNormal(const mjModel* m, mjData* d, const mjtNum pnt[3], c
// geomgroup, flg_static are as in mjvOption; geomgroup==NULL skips group exclusion
MJAPI mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum vec[3],
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
int geomid[1]);
int geomid[1], mjtNum normal[3]);
// intersect ray with hfield
// intersect ray with hfield, compute normal if given
MJAPI mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int geomid,
const mjtNum pnt[3], const mjtNum vec[3]);
const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]);
// intersect ray with triangle
MJAPI mjtNum ray_triangle(mjtNum v[][3], const mjtNum lpnt[3], const mjtNum lvec[3],
const mjtNum b0[3], const mjtNum b1[3], mjtNum normal[3]);
// intersect ray with mesh
// intersect ray with mesh, compute normal if given
MJAPI mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int geomid,
const mjtNum pnt[3], const mjtNum vec[3]);
const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]);
// intersect ray with primitive geom, no meshes or hfields
// intersect ray with primitive geom, no meshes or hfields, compute normal if given
MJAPI mjtNum mju_rayGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3],
const mjtNum pnt[3], const mjtNum vec[3], int geomtype);
// intersect ray with geom, compute normal if given
MJAPI mjtNum mj_rayNormal(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum vec[3],
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
int geomid[1], mjtNum normal[3]);
const mjtNum pnt[3], const mjtNum vec[3], int geomtype,
mjtNum normal[3]);
// intersect ray with flex, return nearest vertex id, compute normal if given
MJAPI mjtNum mju_rayFlexNormal(const mjModel* m, const mjData* d,
int flex_layer, mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face,
mjtByte flg_skin, int flexid,
const mjtNum pnt[3], const mjtNum vec[3],
int vertid[1], mjtNum normal[3]);
// intersect ray with flex, return nearest vertex id
MJAPI mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte flg_vert,
mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid,
const mjtNum pnt[3], const mjtNum vec[3], int vertid[1]);
MJAPI mjtNum mj_rayFlex(const mjModel* m, const mjData* d, int flex_layer,
mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face,
mjtByte flg_skin, int flexid, const mjtNum pnt[3],
const mjtNum vec[3], int vertid[1], mjtNum normal[3]);
// intersect ray with skin, return nearest vertex id
MJAPI mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
src/engine/engine_sensor.c
+7 -7
View File
@@ -505,8 +505,8 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
int geomid;
mjtNum normal[3];
mjtNum* p_normal = (dataspec & (1 << mjRAYDATA_NORMAL)) ? normal : NULL;
mjtNum dist = mj_rayNormal(m, d, origin, rvec, NULL, 1,
m->site_bodyid[objid], &geomid, p_normal);
mjtNum dist = mj_ray(m, d, origin, rvec, NULL, 1,
m->site_bodyid[objid], &geomid, p_normal);
// for site sensor: pass NULL for cam_z so depth = dist
fill_raydata(ptr, dataspec, dist, origin, rvec, normal, NULL, NULL);
@@ -550,8 +550,8 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
}
// cast all rays with normals if needed
mj_multiRayNormal(m, d, cam_xpos, vec, NULL, 1, bodyexclude,
geomid, dist, normals, npixel, mjMAXVAL);
mj_multiRay(m, d, cam_xpos, vec, NULL, 1, bodyexclude,
geomid, dist, normals, npixel, mjMAXVAL);
// fill in output for each pixel
ptr = d->sensordata + adr;
@@ -576,8 +576,8 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
int geomid;
mjtNum normal[3];
mjtNum dist = mj_rayNormal(m, d, origin, direction, NULL, 1,
bodyexclude, &geomid, normal);
mjtNum dist = mj_ray(m, d, origin, direction, NULL, 1,
bodyexclude, &geomid, normal);
ptr = fill_raydata(ptr, dataspec, dist, origin, direction,
normal, cam_xpos, cam_z);
@@ -1123,7 +1123,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
// add if ray-zone intersection (always true when con->pos inside zone)
if (mju_rayGeom(d->site_xpos+3*objid, d->site_xmat+9*objid,
m->site_size+3*objid, con->pos, conray,
m->site_type[objid]) >= 0) {
m->site_type[objid], NULL) >= 0) {
d->sensordata[adr] += conforce[0];
}
}
src/engine/engine_vis_interact.c
+6 -5
View File
@@ -840,7 +840,8 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
// find intersection with geoms
*geomid = -1;
mjtNum geomdist = mj_ray(m, d, pos, ray, vopt->geomgroup, vopt->flags[mjVIS_STATIC], -1, geomid);
mjtNum geomdist = mj_ray(m, d, pos, ray,
vopt->geomgroup, vopt->flags[mjVIS_STATIC], -1, geomid, NULL);
// find intersection with flexes
int flexbodyid = -1;
@@ -852,10 +853,10 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
for (int i=0; i < m->nflex; i++) {
// process one flex
int vertid;
mjtNum newdist = mju_rayFlex(m, d, vopt->flex_layer,
vopt->flags[mjVIS_FLEXVERT], vopt->flags[mjVIS_FLEXEDGE],
vopt->flags[mjVIS_FLEXFACE], vopt->flags[mjVIS_FLEXSKIN],
i, pos, ray, &vertid);
mjtNum newdist =
mj_rayFlex(m, d, vopt->flex_layer, vopt->flags[mjVIS_FLEXVERT],
vopt->flags[mjVIS_FLEXEDGE], vopt->flags[mjVIS_FLEXFACE],
vopt->flags[mjVIS_FLEXSKIN], i, pos, ray, &vertid, NULL);
// update if closer intersection found
if (newdist >= 0 && (newdist < flexdist || flexdist < 0)) {