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

Use mesh BVH for speeding up ray mesh intersection.

Browse Source 
PiperOrigin-RevId: 532516008
Change-Id: Ia9cac060279d030221c1a6b4fad457a857701370
This commit is contained in:
Alessio Quaglino
2023-05-16 11:09:53 -07:00
committed by Copybara-Service
parent 9db444cd46
commit dab49d487c
14 changed files with 51 additions and 267 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
-2
View File
@@ -32,9 +32,7 @@ General
trajectory optimization. See :ref:`mju_cholFactorBand` documentation for details.
- Added :ref:`mj_multiRay` function for intersecting multiple rays emanating from a single point.
This is significantly faster than calling :ref:`mj_ray` multiple times.
- Ray-mesh collisions are now up to 10x faster, using a bounding volume hierarchy of mesh faces.
- Increased ``mjMAXUIITEM`` (maximum number of UI elements per section in Simulate) to 100.
- Removed ``const`` qualifier from :ref:`mjData` in :ref:`mj_ray`, :ref:`mj_rayMesh`, and :ref:`mjv_select`.
- Added :ref:`documentation<exProvider>` for resource providers.
- Changed the formula for :ref:`mju_sigmoid`, a finite-support sigmoid :math:`s \colon \mathbf R \rightarrow [0, 1]`.
Previously, the smooth part consisted of two stitched quadratics, once continuously differentiable.
doc/includes/references.h
+3 -3
View File
@@ -2217,12 +2217,12 @@ const char* mj_versionString();
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);
mjtNum mj_ray(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum vec[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 mj_rayHfield(const mjModel* m, const mjData* d, int geomid,
const mjtNum pnt[3], const mjtNum vec[3]);
mjtNum mj_rayMesh(const mjModel* m, mjData* d, int geomid,
mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int geomid,
const mjtNum pnt[3], const mjtNum vec[3]);
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);
@@ -2258,7 +2258,7 @@ void mjv_applyPerturbPose(const mjModel* m, mjData* d, const mjvPerturb* pert,
int flg_paused);
void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert);
mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2);
int mjv_select(const mjModel* m, mjData* d, const mjvOption* vopt,
int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
mjtNum aspectratio, mjtNum relx, mjtNum rely,
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]);
void mjv_defaultOption(mjvOption* opt);
include/mujoco/mujoco.h
+3 -3
View File
@@ -493,7 +493,7 @@ MJAPI void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const m
// Intersect ray (pnt+x*vec, x>=0) with visible geoms, except geoms in bodyexclude.
// Return distance (x) to nearest surface, or -1 if no intersection and output geomid.
// geomgroup, flg_static are as in mjvOption; geomgroup==NULL skips group exclusion.
MJAPI mjtNum mj_ray(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum vec[3],
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]);
@@ -502,7 +502,7 @@ MJAPI mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int geomid,
const mjtNum pnt[3], const mjtNum vec[3]);
// Intersect ray with mesh, return nearest distance or -1 if no intersection.
MJAPI mjtNum mj_rayMesh(const mjModel* m, mjData* d, int geomid,
MJAPI mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int geomid,
const mjtNum pnt[3], const mjtNum vec[3]);
// Intersect ray with pure geom, return nearest distance or -1 if no intersection.
@@ -585,7 +585,7 @@ MJAPI void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb*
MJAPI mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2);
// Select geom or skin with mouse, return bodyid; -1: none selected.
MJAPI int mjv_select(const mjModel* m, mjData* d, const mjvOption* vopt,
MJAPI int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
mjtNum aspectratio, mjtNum relx, mjtNum rely,
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]);
introspect/functions.py
+3 -3
View File
@@ -2847,7 +2847,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData'),
inner_type=ValueType(name='mjData', is_const=True),
),
),
FunctionParameterDecl(
@@ -2940,7 +2940,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData'),
inner_type=ValueType(name='mjData', is_const=True),
),
),
FunctionParameterDecl(
@@ -3624,7 +3624,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData'),
inner_type=ValueType(name='mjData', is_const=True),
),
),
FunctionParameterDecl(
python/mujoco/functions.cc
+1 -1
View File
@@ -549,7 +549,7 @@ PYBIND11_MODULE(_functions, pymodule) {
Def<traits::mj_multiRay>(pymodule);
Def<traits::mj_ray>(
pymodule,
[](const raw::MjModel* m, raw::MjData* d, const mjtNum(*pnt)[3],
[](const raw::MjModel* m, const raw::MjData* d, const mjtNum(*pnt)[3],
const mjtNum(*vec)[3],
std::optional<Eigen::Ref<const Eigen::Vector<mjtByte, mjNGROUP>>>
geomgroup,
src/engine/engine_ray.c
+35 -112
View File
@@ -130,8 +130,8 @@ static mjtNum ray_quad(mjtNum a, mjtNum b, mjtNum c, mjtNum* x) {
// intersect ray with triangle
mjtNum ray_triangle(mjtNum v[][3], const mjtNum* lpnt, const mjtNum* lvec,
const mjtNum* b0, const mjtNum* b1) {
static mjtNum ray_triangle(mjtNum v[][3], const mjtNum* lpnt, const mjtNum* lvec,
const mjtNum* b0, const mjtNum* b1) {
// dif = v[i] - lpnt
mjtNum dif[3][3];
for (int i=0; i<3; i++) {
@@ -186,6 +186,8 @@ mjtNum ray_triangle(mjtNum v[][3], const mjtNum* lpnt, const mjtNum* lvec,
return (-mju_dot3(dif[2], nrm) / denom);
}
//---------------------------- geom-specific intersection functions --------------------------------
// plane
@@ -597,55 +599,19 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
// ray vs axis-aligned bounding box using slab method
// see Ericson, Real-time Collision Detection
int mju_raySlab(const mjtNum aabb[6], const mjtNum xpos[3],
const mjtNum xmat[9], const mjtNum* pnt, const mjtNum* vec) {
mjtNum tmin = 0.0, tmax = INFINITY;
// compute min and max
mjtNum min[3] = {aabb[0]-aabb[3], aabb[1]-aabb[4], aabb[2]-aabb[5]};
mjtNum max[3] = {aabb[0]+aabb[3], aabb[1]+aabb[4], aabb[2]+aabb[5]};
// compute ray in local coordinates
mjtNum src[3], dir[3];
ray_map(xpos, xmat, pnt, vec, src, dir);
// check intersections
for (int d = 0; d < 3; ++d) {
mjtNum t1 = (min[d] - src[d]) / dir[d];
mjtNum t2 = (max[d] - src[d]) / dir[d];
mjtNum minval = t1 < t2 ? t1 : t2;
mjtNum maxval = t1 < t2 ? t2 : t1;
tmin = tmin > minval ? tmin : minval;
tmax = tmax < maxval ? tmax : maxval;
// intersect ray with mesh
mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id,
const mjtNum* pnt, const mjtNum* vec) {
// check geom type
if (m->geom_type[id]!=mjGEOM_MESH) {
mju_error("mj_rayMesh: geom with mesh type expected");
}
return tmin < tmax;
}
// ray vs tree intersection
mjtNum mju_rayTree(const mjModel* m, mjData* d, int id, const mjtNum* pnt,
const mjtNum* vec) {
const int meshid = m->geom_dataid[id];
const int bvhadr = m->mesh_bvhadr[meshid];
const int* faceid = m->bvh_geomid + bvhadr;
const mjtNum* bvh = m->bvh_aabb + 6*bvhadr;
const int* child = m->bvh_child + 2*bvhadr;
if (meshid==-1) {
mju_error("mju_rayTree: this is not a mesh.");
// bounding box test
if (ray_box(d->geom_xpos+3*id, d->geom_xmat+9*id, m->geom_size+3*id, pnt, vec, NULL)<0) {
return -1;
}
mjMARKSTACK;
// TODO(quaglino): Store bvh max depths to make this bound tighter.
int max_stack = m->mesh_bvhnum[meshid];
int* stack = mj_stackAllocInt(d, max_stack);
int nstack = 0;
stack[nstack] = 0;
nstack++;
// 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);
@@ -667,80 +633,37 @@ mjtNum mju_rayTree(const mjModel* m, mjData* d, int id, const mjtNum* pnt,
// init solution
mjtNum x = -1, sol;
while (nstack) {
// pop from stack
nstack--;
int node = stack[nstack];
// process all triangles
int face, meshid = m->geom_dataid[id];
for (face = m->mesh_faceadr[meshid];
face < m->mesh_faceadr[meshid] + m->mesh_facenum[meshid];
face++) {
// get float vertices
float* vf[3];
vf[0] = m->mesh_vert + 3*(m->mesh_face[3*face] + m->mesh_vertadr[meshid]);
vf[1] = m->mesh_vert + 3*(m->mesh_face[3*face+1] + m->mesh_vertadr[meshid]);
vf[2] = m->mesh_vert + 3*(m->mesh_face[3*face+2] + m->mesh_vertadr[meshid]);
// intersection test
int intersect = mju_raySlab(bvh+6*node, d->geom_xpos+3*id, d->geom_xmat+9*id, pnt, vec);
// if no intersection, skip
if (!intersect) {
continue;
// convert to mjtNum
mjtNum v[3][3];
for (int i=0; i<3; i++) {
for (int j=0; j<3; j++) {
v[i][j] = (mjtNum)vf[i][j];
}
}
// node1 is a leaf
if (faceid[node] != -1) {
int face = faceid[node] + m->mesh_faceadr[meshid];
// solve
sol = ray_triangle(v, lpnt, lvec, b0, b1);
// get float vertices
float* vf[3];
vf[0] = m->mesh_vert + 3*(m->mesh_face[3*face+0] + m->mesh_vertadr[meshid]);
vf[1] = m->mesh_vert + 3*(m->mesh_face[3*face+1] + m->mesh_vertadr[meshid]);
vf[2] = m->mesh_vert + 3*(m->mesh_face[3*face+2] + m->mesh_vertadr[meshid]);
// convert to mjtNum
mjtNum v[3][3];
for (int i=0; i<3; i++) {
for (int j=0; j<3; j++) {
v[i][j] = (mjtNum)vf[i][j];
}
}
// solve
sol = ray_triangle(v, lpnt, lvec, b0, b1);
// update
if (sol>=0 && (x<0 || sol<x)) {
x = sol;
}
continue;
}
// used for rendering
d->bvh_active[node + bvhadr] = 1;
// recursive call
for (int i=0; i<2; i++) {
if (child[2*node+i] != -1) {
if (nstack >= max_stack) mju_error("BVH stack depth exceeded.");
stack[nstack] = child[2*node+i];
nstack++;
}
// update
if (sol>=0 && (x<0 || sol<x)) {
x = sol;
}
}
mjFREESTACK;
return x;
}
// intersect ray with mesh
mjtNum mj_rayMesh(const mjModel* m, mjData* d, int id,
const mjtNum* pnt, const mjtNum* vec) {
// check geom type
if (m->geom_type[id]!=mjGEOM_MESH) {
mju_error("mj_rayMesh: geom with mesh type expected");
}
// bounding box test
if (ray_box(d->geom_xpos+3*id, d->geom_xmat+9*id, m->geom_size+3*id, pnt, vec, NULL)<0) {
return -1;
}
return mju_rayTree(m, d, id, pnt, vec);
}
// intersect ray with pure geom, no meshes or hfields
@@ -893,7 +816,7 @@ 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_ray(const mjModel* m, mjData* d, const mjtNum* pnt, const mjtNum* vec,
mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum* vec,
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
int geomid[1]) {
mjtNum dist, newdist;
src/engine/engine_ray.h
+2 -6
View File
@@ -38,7 +38,7 @@ MJAPI void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const m
// 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
MJAPI mjtNum mj_ray(const mjModel* m, mjData* d, const mjtNum* pnt, const mjtNum* vec,
MJAPI mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum* vec,
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
int geomid[1]);
@@ -46,12 +46,8 @@ MJAPI mjtNum mj_ray(const mjModel* m, mjData* d, const mjtNum* pnt, const mjtNum
MJAPI mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int geomid,
const mjtNum* pnt, const mjtNum* vec);
// intersect ray with triangle
MJAPI mjtNum ray_triangle(mjtNum v[][3], const mjtNum* lpnt, const mjtNum* lvec,
const mjtNum* b0, const mjtNum* b1);
// intersect ray with mesh
MJAPI mjtNum mj_rayMesh(const mjModel* m, mjData* d, int geomid,
MJAPI mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int geomid,
const mjtNum* pnt, const mjtNum* vec);
// intersect ray with pure geom, no meshes or hfields
src/engine/engine_vis_interact.c
+1 -1
View File
@@ -731,7 +731,7 @@ mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2)
// Select geom or skin with mouse, return bodyid; -1: none selected.
int mjv_select(const mjModel* m, mjData* d, const mjvOption* vopt,
int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
mjtNum aspectratio, mjtNum relx, mjtNum rely,
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]) {
// get average camera
src/engine/engine_vis_interact.h
+1 -1
View File
@@ -73,7 +73,7 @@ MJAPI void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb*
MJAPI mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2);
// Select geom or skin with mouse, return bodyid; -1: none selected.
MJAPI int mjv_select(const mjModel* m, mjData* d, const mjvOption* vopt,
MJAPI int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
mjtNum aspectratio, mjtNum relx, mjtNum rely,
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]);
test/engine/engine_ray_test.cc
-125
View File
@@ -140,8 +140,6 @@ TEST_F(RayTest, ExcludeStatic) {
mj_deleteModel(model);
}
// ------------------------------- mj_multiRay --------------------------------
TEST_F(RayTest, MultiRayEqualsSingleRay) {
mjModel* m = LoadModelFromString(kRayCastingModel);
ASSERT_THAT(m, NotNull());
@@ -260,128 +258,5 @@ TEST_F(RayTest, EdgeCases) {
mj_deleteModel(m);
}
// ------------------------------- mj_rayMesh ---------------------------------
// map ray to local geom frame
static void ray_map(const mjtNum* pos, const mjtNum* mat, const mjtNum* pnt,
const mjtNum* vec, mjtNum* lpnt, mjtNum* lvec) {
const mjtNum dif[3] = {pnt[0]-pos[0], pnt[1]-pos[1], pnt[2]-pos[2]};
// lpnt = mat' * dif
lpnt[0] = mat[0]*dif[0] + mat[3]*dif[1] + mat[6]*dif[2];
lpnt[1] = mat[1]*dif[0] + mat[4]*dif[1] + mat[7]*dif[2];
lpnt[2] = mat[2]*dif[0] + mat[5]*dif[1] + mat[8]*dif[2];
// lvec = mat' * vec
lvec[0] = mat[0]*vec[0] + mat[3]*vec[1] + mat[6]*vec[2];
lvec[1] = mat[1]*vec[0] + mat[4]*vec[1] + mat[7]*vec[2];
lvec[2] = mat[2]*vec[0] + mat[5]*vec[1] + mat[8]*vec[2];
}
// old ray mesh intersection
mjtNum _rayMesh(const mjModel* m, const mjData* d, int geomid,
const mjtNum* pnt, const mjtNum* vec) {
// check geom type
if (m->geom_type[geomid] != mjGEOM_MESH) {
mju_error("mj_rayMesh: geom with mesh type expected");
}
// map to local frame
mjtNum lpnt[3], lvec[3];
ray_map(d->geom_xpos+3*geomid, d->geom_xmat+9*geomid, pnt, vec, lpnt, lvec);
// construct basis vectors of normal plane
mjtNum b0[3] = {1, 1, 1}, b1[3];
if (mju_abs(lvec[0]) >= mju_abs(lvec[1]) &&
mju_abs(lvec[0]) >= mju_abs(lvec[2])) {
b0[0] = 0;
} else if (mju_abs(lvec[1]) >= mju_abs(lvec[2])) {
b0[1] = 0;
} else {
b0[2] = 0;
}
mju_addScl3(b1, b0, lvec, -mju_dot3(lvec, b0)/mju_dot3(lvec, lvec));
mju_normalize3(b1);
mju_cross(b0, b1, lvec);
mju_normalize3(b0);
// init solution
mjtNum x = -1, sol;
// process all triangles
int face, meshid = m->geom_dataid[geomid];
for (face = m->mesh_faceadr[meshid];
face < m->mesh_faceadr[meshid] + m->mesh_facenum[meshid];
face++) {
// get float vertices
float* vf[3];
vf[0] = m->mesh_vert + 3*(m->mesh_face[3*face] + m->mesh_vertadr[meshid]);
vf[1] = m->mesh_vert + 3*(m->mesh_face[3*face+1] + m->mesh_vertadr[meshid]);
vf[2] = m->mesh_vert + 3*(m->mesh_face[3*face+2] + m->mesh_vertadr[meshid]);
// convert to mjtNum
mjtNum v[3][3];
for (int i=0; i < 3; i++) {
for (int j=0; j < 3; j++) {
v[i][j] = (mjtNum)vf[i][j];
}
}
// solve
sol = ray_triangle(v, lpnt, lvec, b0, b1);
// update
if (sol >= 0 && (x < 0 || sol < x)) {
x = sol;
}
}
return x;
}
TEST_F(RayTest, RayMeshPruning) {
char error[1024] = {0};
const std::string xml_path =
GetTestDataFilePath("testdata/stanford_bunny.xml");
mjModel* m = mj_loadXML(xml_path.c_str(), NULL, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
ASSERT_THAT(d, NotNull());
mj_forward(m, d);
// create ray array
constexpr int N = 80;
constexpr int M = 60;
mjtNum vec[3*N*M];
mjtNum pnt[3] = {1, .2, 0};
mjtNum cone[4][3] = {{-1, -1, -1}, {-1, -1, 1}, {1, -1, 1}, {1, -1, -1}};
memset(vec, 0, 3*N*M*sizeof(mjtNum));
for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) {
for (int k = 0; k < 3; ++k) {
vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) +
j * cone[1][1] / (M - 1) +
(N - i - 1) * cone[2][k] / (N - 1) +
(M - j - 1) * cone[3][k] / (M - 1);
}
}
}
// compare results with single ray function
mjtNum dist_new, dist_old;
for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) {
int idx = i * M + j;
dist_old = _rayMesh(m, d, /*geomid=*/0, pnt, vec + 3 * idx);
dist_new = mj_rayMesh(m, d, /*geomid=*/0, pnt, vec + 3 * idx);
EXPECT_FLOAT_EQ(dist_new, dist_old);
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco
test/testdata/stanford_bunny.xml
Vendored
-8
View File
@@ -1,8 +0,0 @@
<mujoco>
<asset>
<mesh file="stanford_bunny.obj"/>
</asset>
<worldbody>
<geom type="mesh" mesh="stanford_bunny" pos="1 0 0" euler="0 90 0"/>
</worldbody>
</mujoco>
test/user/testdata/fitmesh_aabb.xml
Vendored
+1 -1
View File
@@ -2,7 +2,7 @@
<compiler fitaabb="true"/>
<asset>
<mesh file="../../testdata/stanford_bunny.obj"/>
<mesh file="stanford_bunny.obj"/>
</asset>
<worldbody>
test/user/testdata/fitmesh_inertiabox.xml
Vendored
+1 -1
View File
@@ -2,7 +2,7 @@
<compiler fitaabb="false" exactmeshinertia="true"/>
<asset>
<mesh file="../../testdata/stanford_bunny.obj"/>
<mesh file="stanford_bunny.obj"/>
</asset>
<worldbody>
test/testdata/stanford_bunny.obj → test/user/testdata/stanford_bunny.obj
Vendored
View File