diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index cd53b2df..1e588965 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -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. diff --git a/src/engine/engine_ray.h b/src/engine/engine_ray.h index 2a1d40e0..bf7d7045 100644 --- a/src/engine/engine_ray.h +++ b/src/engine/engine_ray.h @@ -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]); diff --git a/test/engine/engine_ray_test.cc b/test/engine/engine_ray_test.cc index a158d7b3..a4e3c606 100644 --- a/test/engine/engine_ray_test.cc +++ b/test/engine/engine_ray_test.cc @@ -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); }