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

Add bvhactive flag to visual/global. Fixes #1279.

Browse Source 
PiperOrigin-RevId: 599778272
Change-Id: If5530629035cc68b198447503388f59a04041b13
This commit is contained in:
Yuval Tassa
2024-01-19 02:59:06 -08:00
committed by Copybara-Service
parent 2feefbc5d2
commit fea7c10b95
18 changed files with 147 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_driver.c
+20 -9
View File
@@ -273,7 +273,9 @@ void mj_collision(const mjModel* m, mjData* d) {
mj_clearEfc(d);
// reset the visualization flags
memset(d->bvh_active, 0, m->nbvh);
if (m->vis.global.bvactive) {
memset(d->bvh_active, 0, m->nbvh);
}
// return if disabled
if (mjDISABLED(mjDSBL_CONSTRAINT) || mjDISABLED(mjDSBL_CONTACT)
@@ -634,6 +636,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
mjtByte isbody2 = (bf2 < nbody);
int f1 = isbody1 ? -1 : bf1 - nbody;
int f2 = isbody2 ? -1 : bf2 - nbody;
int mark_active = m->vis.global.bvactive;
const int bvhadr1 = isbody1 ? m->body_bvhadr[bf1] : m->flex_bvhadr[f1];
const int bvhadr2 = isbody2 ? m->body_bvhadr[bf2] : m->flex_bvhadr[f2];
const int* child1 = m->bvh_child + 2*bvhadr1;
@@ -705,8 +708,10 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
d->geom_xpos + 3*nodeid2, d->geom_xmat + 9*nodeid2,
margin, NULL, NULL, &initialize)) {
mj_collideGeomPair(m, d, nodeid1, nodeid2, merged, startadr, pairadr);
d->bvh_active[node1 + bvhadr1] = 1;
d->bvh_active[node2 + bvhadr2] = 1;
if (mark_active) {
d->bvh_active[node1 + bvhadr1] = 1;
d->bvh_active[node2 + bvhadr2] = 1;
}
}
}
continue;
@@ -742,8 +747,10 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
if (m->geom_type[nodeid1] != mjGEOM_PLANE) {
mj_collideGeomElem(m, d, nodeid1, f2, nodeid2);
}
d->bvh_active[node1 + bvhadr1] = 1;
d->bvh_active[node2 + bvhadr2] = 1;
if (mark_active) {
d->bvh_active[node1 + bvhadr1] = 1;
d->bvh_active[node2 + bvhadr2] = 1;
}
}
}
continue;
@@ -771,8 +778,10 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
// box filter applied in mj_collideElems, bitmask filter applied earlier
if (isleaf1 && isleaf2) {
mj_collideElems(m, d, f1, nodeid1, f2, nodeid2);
d->bvh_active[node1 + bvhadr1] = 1;
d->bvh_active[node2 + bvhadr2] = 1;
if (mark_active) {
d->bvh_active[node1 + bvhadr1] = 1;
d->bvh_active[node2 + bvhadr2] = 1;
}
continue;
}
@@ -784,8 +793,10 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
}
}
d->bvh_active[node1 + bvhadr1] = 1;
d->bvh_active[node2 + bvhadr2] = 1;
if (mark_active) {
d->bvh_active[node1 + bvhadr1] = 1;
d->bvh_active[node2 + bvhadr2] = 1;
}
// keep traversing the tree
if (!isleaf1 && isleaf2) {
src/engine/engine_collision_sdf.c
+3 -6
View File
@@ -496,7 +496,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g,
const int* faceid = m->bvh_nodeid + bvhadr;
const mjtNum* bvh = m->bvh_aabb + 6*bvhadr;
const int* child = m->bvh_child + 2*bvhadr;
mjtByte* visited = d->bvh_active + bvhadr;
mjtByte* bvh_active = m->vis.global.bvactive ? d->bvh_active + bvhadr : NULL;
mj_markStack(d);
// TODO(quaglino): Store bvh max depths to make this bound tighter.
@@ -521,9 +521,6 @@ static void collideBVH(const mjModel* m, mjData* d, int g,
// node1 is a leaf
if (faceid[node] != -1) {
if (visited[node]) {
continue;
}
if (boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) {
faces[*npoints] = faceid[node];
if (++(*npoints) == MAXSDFFACE) {
@@ -531,7 +528,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g,
mj_freeStack(d);
return;
}
visited[node] = 1;
if (bvh_active) bvh_active[node] = 1;
}
continue;
}
@@ -541,7 +538,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g,
continue;
}
visited[node] = 1;
if (bvh_active) bvh_active[node] = 1;
// recursive call
for (int i=0; i < 2; i++) {
src/engine/engine_io.c
+3
View File
@@ -194,6 +194,7 @@ void mj_defaultVisual(mjVisual* vis) {
vis->global.offheight = 480;
vis->global.realtime = 1.0;
vis->global.ellipsoidinertia = 0;
vis->global.bvactive = 1;
// rendering quality
vis->quality.shadowsize = 4096;
@@ -272,6 +273,8 @@ void mj_defaultVisual(mjVisual* vis) {
setf4(vis->rgba.slidercrank, .5, .3, .8, 1.);
setf4(vis->rgba.crankbroken, .9, .0, .0, 1.);
setf4(vis->rgba.frustum, 1., 1., .0, .2);
setf4(vis->rgba.bv, 0., 1., .0, .5);
setf4(vis->rgba.bvactive, 1., 0., .0, .5);
}
src/engine/engine_ray.c
+7 -1
View File
@@ -627,6 +627,7 @@ int mju_raySlab(const mjtNum aabb[6], const mjtNum xpos[3],
// ray vs tree intersection
mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt,
const mjtNum* vec) {
int mark_active = m->vis.global.bvactive;
const int meshid = m->geom_dataid[id];
const int bvhadr = m->mesh_bvhadr[meshid];
const int* faceid = m->bvh_nodeid + bvhadr;
@@ -701,12 +702,17 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt,
// update
if (sol >= 0 && (x < 0 || sol < x)) {
x = sol;
if (mark_active) {
d->bvh_active[node + bvhadr] = 1;
}
}
continue;
}
// used for rendering
d->bvh_active[node + bvhadr] = 1;
if (mark_active) {
d->bvh_active[node + bvhadr] = 1;
}
// add children to the stack
for (int i=0; i < 2; i++) {
src/engine/engine_vis_visualize.c
+23 -15
View File
@@ -532,6 +532,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mjvGeom* thisgeom;
mjvPerturb localpert;
float scl = m->stat.meansize;
int mark_active = m->vis.global.bvactive;
// make default pert if missing
if (!pert) {
@@ -636,7 +637,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
category = mjCAT_DECOR;
objtype = mjOBJ_UNKNOWN;
if (vopt->flags[mjVIS_BODYBVH]) {
float rgba[] = {1, 0, 0, 1};
for (int i = 0; i < m->nbvhstatic; i++) {
int isleaf = m->bvh_child[2*i] == -1 && m->bvh_child[2*i+1] == -1;
if (scn->ngeom >= scn->maxgeom) break;
@@ -671,8 +671,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mju_rotVecMat(pos, center, xmat);
mju_addTo3(pos, xpos);
rgba[0] = d->bvh_active[i] ? 1 : 0;
rgba[1] = d->bvh_active[i] ? 0 : 1;
// set box color
const float* rgba = m->vis.rgba.bv;
if (mark_active && d->bvh_active[i]) {
rgba = m->vis.rgba.bvactive;
}
START
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba);
@@ -685,10 +688,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
category = mjCAT_DECOR;
objtype = mjOBJ_UNKNOWN;
if (vopt->flags[mjVIS_FLEXBVH]) {
float rgba[] = {1, 0, 0, 0.1};
for (int f=0; f < m->nflex; f++) {
if (m->flex_bvhnum[f] &&
vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[f]))]) {
if (m->flex_bvhnum[f] && vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[f]))]) {
for (int i=m->flex_bvhadr[f]; i < m->flex_bvhadr[f]+m->flex_bvhnum[f]; i++) {
int isleaf = m->bvh_child[2*i] == -1 && m->bvh_child[2*i+1] == -1;
if (scn->ngeom >= scn->maxgeom) break;
@@ -698,10 +699,14 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
}
// set box data
// get box data
mjtNum *aabb = d->bvh_aabb_dyn + 6*(i - m->nbvhstatic);
rgba[0] = d->bvh_active[i] ? 1 : 0;
rgba[1] = d->bvh_active[i] ? 0 : 1;
// set box color
const float* rgba = m->vis.rgba.bv;
if (mark_active && d->bvh_active[i]) {
rgba = m->vis.rgba.bvactive;
}
START
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, aabb+3, aabb, NULL, rgba);
@@ -715,7 +720,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
category = mjCAT_DECOR;
objtype = mjOBJ_UNKNOWN;
if (vopt->flags[mjVIS_MESHBVH]) {
float rgba[] = {1, 0, 0, 1};
for (int geomid = 0; geomid < m->ngeom; geomid++) {
int meshid = m->geom_dataid[geomid];
if (meshid == -1) {
@@ -732,13 +736,17 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
}
if (!d->bvh_active[i]) {
continue;
// box color
const float* rgba = m->vis.rgba.bv;
if (mark_active) {
if (d->bvh_active[i]) {
rgba = m->vis.rgba.bvactive;
} else {
// when marking active bvs, skip inactive volumes
continue;
}
}
rgba[0] = d->bvh_active[i] ? 1 : 0;
rgba[1] = d->bvh_active[i] ? 0 : 1;
// get xpos, xmat, size
const mjtNum* xpos = d->geom_xpos + 3 * geomid;
const mjtNum* xmat = d->geom_xmat + 9 * geomid;