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

Add mj_rayHfieldNormal (not exposed in public header)

Browse Source 
PiperOrigin-RevId: 842259581
Change-Id: I27d6c46024d55addbc22eed276331e2ef5acfa7d
This commit is contained in:
Yuval Tassa
2025-12-09 08:52:39 -08:00
committed by Copybara-Service
parent 7553541b4b
commit 5b1fa666fb
3 changed files with 67 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_ray.c
+55 -16
View File
@@ -557,16 +557,19 @@ static mjtNum ray_box(const mjtNum pos[3], const mjtNum mat[9], const mjtNum siz
}
// intersect ray with hfield
mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
const mjtNum pnt[3], const mjtNum vec[3]) {
// intersect ray with hfield, compute normal if given
mjtNum mj_rayHfieldNormal(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);
// check geom type
if (m->geom_type[id] != mjGEOM_HFIELD) {
if (m->geom_type[geomid] != mjGEOM_HFIELD) {
mjERROR("geom with hfield type expected");
}
// hfield id and dimensions
int hid = m->geom_dataid[id];
int hid = m->geom_dataid[geomid];
int nrow = m->hfield_nrow[hid];
int ncol = m->hfield_ncol[hid];
const mjtNum* size = m->hfield_size + 4*hid;
@@ -574,33 +577,38 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
// compute size and pos of base box
mjtNum base_size[3] = {size[0], size[1], size[3]*0.5};
const mjtNum* xmat = d->geom_xmat + 9*geomid;
const mjtNum* xpos = d->geom_xpos + 3*geomid;
mjtNum base_pos[3] = {
d->geom_xpos[3*id] - d->geom_xmat[9*id+2]*size[3]*0.5,
d->geom_xpos[3*id+1] - d->geom_xmat[9*id+5]*size[3]*0.5,
d->geom_xpos[3*id+2] - d->geom_xmat[9*id+8]*size[3]*0.5
xpos[0] - xmat[2]*size[3]*0.5,
xpos[1] - xmat[5]*size[3]*0.5,
xpos[2] - xmat[8]*size[3]*0.5
};
// compute size and pos of top box
mjtNum top_size[3] = {size[0], size[1], size[2]*0.5};
mjtNum top_pos[3] = {
d->geom_xpos[3*id] + d->geom_xmat[9*id+2]*size[2]*0.5,
d->geom_xpos[3*id+1] + d->geom_xmat[9*id+5]*size[2]*0.5,
d->geom_xpos[3*id+2] + d->geom_xmat[9*id+8]*size[2]*0.5
xpos[0] + xmat[2]*size[2]*0.5,
xpos[1] + xmat[5]*size[2]*0.5,
xpos[2] + xmat[8]*size[2]*0.5
};
// init: intersection with base box
mjtNum x = ray_box(base_pos, d->geom_xmat+9*id, base_size, pnt, vec, NULL, NULL);
mjtNum normal_base[3];
mjtNum x = ray_box(base_pos, xmat, base_size, pnt, vec,
NULL, normal ? normal_base : NULL);
// check top box: done if no intersection
mjtNum all[6];
mjtNum top_intersect = ray_box(top_pos, d->geom_xmat+9*id, top_size, pnt, vec, all, NULL);
mjtNum top_intersect = ray_box(top_pos, xmat, top_size, pnt, vec, all, NULL);
if (top_intersect < 0) {
if (normal && x >= 0) mju_copy3(normal, normal_base);
return x;
}
// map to local frame
mjtNum lpnt[3], lvec[3];
ray_map(d->geom_xpos+3*id, d->geom_xmat+9*id, pnt, vec, lpnt, lvec);
ray_map(xpos, xmat, pnt, vec, lpnt, lvec);
// construct basis vectors of normal plane
mjtNum b0[3] = {1, 1, 1}, b1[3];
@@ -641,18 +649,28 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
int rmin = mjMAX(0, (int)mju_floor(mjMIN(SY[0], SY[1]))-1);
int rmax = mjMIN(nrow-1, (int)mju_ceil(mjMAX(SY[0], SY[1]))+1);
// local normal, initialize with base box normal (if any), in local frame
mjtNum normal_local[3] = {0, 0, 0};
if (normal && x >= 0) {
mju_mulMatTVec3(normal_local, xmat, normal_base);
}
// check triangles within bounds
for (int r=rmin; r < rmax; r++) {
for (int c=cmin; c < cmax; c++) {
// triangle normal
mjtNum normal_tri[3];
// first triangle
mjtNum va[3][3] = {
{dx*c-size[0], dy*r-size[1], data[r*ncol+c]*size[2]},
{dx*(c+1)-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+(c+1)]*size[2]},
{dx*(c+1)-size[0], dy*(r+0)-size[1], data[(r+0)*ncol+(c+1)]*size[2]}
};
mjtNum sol = ray_triangle(va, lpnt, lvec, b0, b1, NULL);
mjtNum sol = ray_triangle(va, lpnt, lvec, b0, b1, normal ? normal_tri : NULL);
if (sol >= 0 && (x < 0 || sol < x)) {
x = sol;
if (normal) mju_copy3(normal_local, normal_tri);
}
// second triangle
@@ -661,9 +679,10 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
{dx*(c+1)-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+(c+1)]*size[2]},
{dx*(c+0)-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+(c+0)]*size[2]}
};
sol = ray_triangle(vb, lpnt, lvec, b0, b1, NULL);
sol = ray_triangle(vb, lpnt, lvec, b0, b1, normal ? normal_tri : NULL);
if (sol >= 0 && (x < 0 || sol < x)) {
x = sol;
if (normal) mju_copy3(normal_local, normal_tri);
}
}
}
@@ -696,13 +715,33 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
// check if point is below line segment
if (z < z0*(y0+1-y) + z1*(y-y0)) {
x = all[i];
// compute normal
if (normal) {
mju_zero3(normal_local);
if (i == 0) normal_local[0] = -1;
else if (i == 1) normal_local[0] = 1;
else if (i == 2) normal_local[1] = -1;
else if (i == 3) normal_local[1] = 1;
}
}
}
}
// rotate normal to global frame
if (normal && x >= 0) {
mju_mulMatVec3(normal, xmat, normal_local);
}
return x;
}
// 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.
src/engine/engine_ray.h
+4
View File
@@ -46,6 +46,10 @@ MJAPI mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum pnt[3], cons
MJAPI mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int geomid,
const mjtNum pnt[3], const mjtNum vec[3]);
// intersect ray with hfield, compute normal if given
MJAPI mjtNum mj_rayHfieldNormal(const mjModel* m, const mjData* d, int geomid,
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]);
test/engine/engine_ray_test.cc
+8 -1
View File
@@ -476,11 +476,12 @@ static const char* const kCylinderModel = "engine/testdata/ray/cylinder.xml";
static const char* const kBoxModel = "engine/testdata/ray/box.xml";
static const char* const kMeshModel = "engine/testdata/ray/mesh.xml";
static const char* const kSdfModel = "engine/testdata/ray/sdf.xml";
static const char* const kHfieldModel = "engine/testdata/ray/hfield.xml";
TEST_F(RayTest, GeomNormal) {
for (const char* path :
{kPlaneModel, kSphereModel, kCapsuleModel, kEllipsoidModel,
kCylinderModel, kBoxModel, kMeshModel, kSdfModel}) {
kCylinderModel, kBoxModel, kMeshModel, kSdfModel, kHfieldModel}) {
const std::string xml_path = GetTestDataFilePath(path);
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
@@ -528,6 +529,9 @@ TEST_F(RayTest, GeomNormal) {
case mjGEOM_SDF:
r = mj_raySdfNormal(m, d, 0, pnt, vec, normal);
break;
case mjGEOM_HFIELD:
r = mj_rayHfieldNormal(m, d, 0, pnt, vec, normal);
break;
default:
r = mju_rayGeomNormal(pos, mat, size, pnt, vec, type, normal);
}
@@ -562,6 +566,9 @@ TEST_F(RayTest, GeomNormal) {
case mjGEOM_SDF:
dr = mj_raySdfNormal(m, d, 0, dpnt, vec, nullptr);
break;
case mjGEOM_HFIELD:
dr = mj_rayHfieldNormal(m, d, 0, dpnt, vec, nullptr);
break;
default:
dr = mju_rayGeomNormal(pos, mat, size, dpnt, vec, type, nullptr);
}