Use mesh BVH for speeding up ray mesh intersection.
PiperOrigin-RevId: 532516008 Change-Id: Ia9cac060279d030221c1a6b4fad457a857701370
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
9db444cd46
commit
dab49d487c
+35
-112
@@ -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;
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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]);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user