Skip to content

Commit

Permalink
Add mjGEOM_LINEBOX, used in visualization of collision trees.
Browse files Browse the repository at this point in the history 
PiperOrigin-RevId: 591224734
Change-Id: I09ad8c4b711021ff4dd5548ba5381d25d5a3d7f8
  • Loading branch information
@yuvaltassa @copybara-github
yuvaltassa authored and copybara-github committed Dec 15, 2023
1 parent f2a9673 commit 38fa756
Show file tree
Hide file tree
Showing 8 changed files with 84 additions and 75 deletions.
3 changes: 2 additions & 1 deletion doc/includes/references.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -459,10 +459,11 @@ typedef enum mjtGeom_ { // type of geometric shape
mjGEOM_ARROW1, // arrow without wedges
mjGEOM_ARROW2, // arrow in both directions
mjGEOM_LINE, // line
mjGEOM_LINEBOX, // box with line edges
mjGEOM_FLEX, // flex
mjGEOM_SKIN, // skin
mjGEOM_LABEL, // text label
mjGEOM_TRIANGLE, // triangle connecting a frame
mjGEOM_TRIANGLE, // triangle

mjGEOM_NONE = 1001 // missing geom type
} mjtGeom;
Expand Down
3 changes: 2 additions & 1 deletion include/mujoco/mjmodel.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,10 +109,11 @@ typedef enum mjtGeom_ { // type of geometric shape
mjGEOM_ARROW1, // arrow without wedges
mjGEOM_ARROW2, // arrow in both directions
mjGEOM_LINE, // line
mjGEOM_LINEBOX, // box with line edges
mjGEOM_FLEX, // flex
mjGEOM_SKIN, // skin
mjGEOM_LABEL, // text label
mjGEOM_TRIANGLE, // triangle connecting a frame
mjGEOM_TRIANGLE, // triangle

mjGEOM_NONE = 1001 // missing geom type
} mjtGeom;
Expand Down
9 changes: 5 additions & 4 deletions introspect/enums.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,10 +90,11 @@
('mjGEOM_ARROW1', 101),
('mjGEOM_ARROW2', 102),
('mjGEOM_LINE', 103),
('mjGEOM_FLEX', 104),
('mjGEOM_SKIN', 105),
('mjGEOM_LABEL', 106),
('mjGEOM_TRIANGLE', 107),
('mjGEOM_LINEBOX', 104),
('mjGEOM_FLEX', 105),
('mjGEOM_SKIN', 106),
('mjGEOM_LABEL', 107),
('mjGEOM_TRIANGLE', 108),
('mjGEOM_NONE', 1001),
]),
)),
Expand Down
2 changes: 1 addition & 1 deletion introspect/enums_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ def test_mjtGeom(self): # pylint: disable=invalid-name
self.assertEqual(enum_decl.values['mjGEOM_ARROW'], 100)
self.assertEqual(enum_decl.values['mjGEOM_ARROW1'], 101)
self.assertEqual(enum_decl.values['mjGEOM_ARROW2'], 102)
self.assertEqual(enum_decl.values['mjGEOM_TRIANGLE'], 107)
self.assertEqual(enum_decl.values['mjGEOM_TRIANGLE'], 108)
# Skip a few...

self.assertEqual(enum_decl.values['mjGEOM_NONE'], 1001)
Expand Down
2 changes: 1 addition & 1 deletion python/mujoco/bindings_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -840,7 +840,7 @@ def test_enum_values(self):
self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW, 100)
self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW1, 101)
self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW2, 102)
self.assertEqual(mujoco.mjtGeom.mjGEOM_TRIANGLE, 107)
self.assertEqual(mujoco.mjtGeom.mjGEOM_TRIANGLE, 108)
self.assertEqual(mujoco.mjtGeom.mjGEOM_NONE, 1001)

def test_enum_from_int(self):
Expand Down
96 changes: 33 additions & 63 deletions src/engine/engine_vis_visualize.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -509,56 +509,6 @@ static int bodycategory(const mjModel* m, int bodyid) {



// draw bounding box
static void drawBoundingBox(mjData* d, mjvScene* scn,
const mjtNum aabb[6], const mjtNum xpos[3],
const mjtNum xmat[9], const float rgba[4]) {
mjtNum x[3];
mjtNum dist[3][3];
mjvGeom* thisgeom;
int category = mjCAT_DECOR;
int objtype = mjOBJ_UNKNOWN;
int i = -1;

if (xmat != NULL) {
mju_rotVecMat(x, aabb, xmat);
mju_addTo3(x, xpos);
for (int j=0; j < 3; j++) {
for (int k=0; k < 3; k++) {
dist[k][j] = aabb[k+3] * xmat[3*j+k];
}
}
} else {
mju_copy3(x, aabb);
mju_addTo3(x, xpos);
for (int j=0; j < 3; j++) {
mju_zero3(dist[j]);
dist[j][j] = aabb[j+3];
}
}

int split[3] = {1, 2, 4};
for (int v=0; v < 8; v++) {
mjtNum from[3] = {x[0], x[1], x[2]};
for (int k=0; k < 3; k++) {
mju_addToScl3(from, dist[k], v&split[k] ? 1 : -1);
}

mjtNum to[3];
for (int k=0; k < 3; k++) {
mju_addScl3(to, from, dist[k], 2);
if (!(v&split[k])) {
START
mjv_connector(thisgeom, mjGEOM_LINE, 2, from, to);
f2f(thisgeom->rgba, rgba, 4);
FINISH
}
}
}
}



// computes the camera frustum
static void getFrustum(float zver[2], float zhor[2], float znear,
const float K[4], const float sensorsize[2]) {
Expand Down Expand Up @@ -683,6 +633,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}

// body BVH
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++) {
Expand All @@ -708,20 +660,30 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
break;
}

// compute transformation
mjtNum *aabb = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i;

// get xpos, xmat, size
const mjtNum* xpos = isleaf ? d->geom_xpos + 3 * geomid : d->xipos + 3 * bodyid;
const mjtNum* xmat = isleaf ? d->geom_xmat + 9 * geomid : d->ximat + 9 * bodyid;
const mjtNum *size = isleaf ? m->geom_aabb + 6*geomid + 3 : m->bvh_aabb + 6*i + 3;

// offset xpos with aabb center (not always at frame origin)
const mjtNum *center = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i;
mjtNum pos[3];
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;

drawBoundingBox(d, scn, aabb, xpos, xmat, rgba);
START
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba);
FINISH

}
}

// flex BVH
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++) {
Expand All @@ -741,17 +703,17 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
rgba[0] = d->bvh_active[i] ? 1 : 0;
rgba[1] = d->bvh_active[i] ? 0 : 1;

// b/304453879 : add LINEBOX geom for bounding box visualization

START
mjv_initGeom(thisgeom, mjGEOM_BOX, aabb+3, aabb, NULL, rgba);
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, aabb+3, aabb, NULL, rgba);
FINISH
}
}
}
}

// mesh BVH
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++) {
Expand All @@ -770,19 +732,27 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
}

// compute transformation
const mjtNum *aabb = m->bvh_aabb + 6*i;
const mjtNum* xpos = d->geom_xpos + 3 * geomid;
const mjtNum* xmat = d->geom_xmat + 9 * geomid;

if (!d->bvh_active[i]) {
continue;
}

rgba[0] = d->bvh_active[i] ? 1 : 0;
rgba[1] = d->bvh_active[i] ? 0 : 1;

drawBoundingBox(d, scn, aabb, xpos, xmat, rgba);
// get xpos, xmat, size
const mjtNum* xpos = d->geom_xpos + 3 * geomid;
const mjtNum* xmat = d->geom_xmat + 9 * geomid;
const mjtNum *size = m->bvh_aabb + 6*i + 3;

// offset xpos with aabb center (not always at geom origin)
const mjtNum *center = m->bvh_aabb + 6*i;
mjtNum pos[3];
mju_rotVecMat(pos, center, xmat);
mju_addTo3(pos, xpos);

START
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba);
FINISH
}
}
}
Expand Down
35 changes: 35 additions & 0 deletions src/render/render_gl3.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -404,6 +404,41 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos,
}
break;

case mjGEOM_LINEBOX: // box with line edges
glLineWidth(1.5*con->lineWidth);
lighting = glIsEnabled(GL_LIGHTING);
glDisable(GL_LIGHTING);
// bottom face
glBegin(GL_LINE_LOOP);
glVertex3f(-size[0], -size[1], -size[2]);
glVertex3f( size[0], -size[1], -size[2]);
glVertex3f( size[0], size[1], -size[2]);
glVertex3f(-size[0], size[1], -size[2]);
glEnd();
// top face
glBegin(GL_LINE_LOOP);
glVertex3f(-size[0], -size[1], size[2]);
glVertex3f( size[0], -size[1], size[2]);
glVertex3f( size[0], size[1], size[2]);
glVertex3f(-size[0], size[1], size[2]);
glEnd();
// vertical edges
glBegin(GL_LINES);
glVertex3f(-size[0], -size[1], -size[2]);
glVertex3f(-size[0], -size[1], size[2]);
glVertex3f( size[0], -size[1], -size[2]);
glVertex3f( size[0], -size[1], size[2]);
glVertex3f( size[0], size[1], -size[2]);
glVertex3f( size[0], size[1], size[2]);
glVertex3f(-size[0], size[1], -size[2]);
glVertex3f(-size[0], size[1], size[2]);
glEnd();
glLineWidth(con->lineWidth);
if (lighting) {
glEnable(GL_LIGHTING);
}
break;

case mjGEOM_TRIANGLE: // triangle
glBegin(GL_TRIANGLES);
glVertex3f(0, 0, 0);
Expand Down
9 changes: 5 additions & 4 deletions unity/Runtime/Bindings/MjBindings.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -191,10 +191,11 @@ public enum mjtGeom : int{
mjGEOM_ARROW1 = 101,
mjGEOM_ARROW2 = 102,
mjGEOM_LINE = 103,
mjGEOM_FLEX = 104,
mjGEOM_SKIN = 105,
mjGEOM_LABEL = 106,
mjGEOM_TRIANGLE = 107,
mjGEOM_LINEBOX = 104,
mjGEOM_FLEX = 105,
mjGEOM_SKIN = 106,
mjGEOM_LABEL = 107,
mjGEOM_TRIANGLE = 108,
mjGEOM_NONE = 1001,
}
public enum mjtCamLight : int{
Expand Down

0 comments on commit 38fa756

Please sign in to comment.