diff --git a/doc/includes/references.h b/doc/includes/references.h index c89a4363..7224e3a3 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -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; diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 49312dd1..d3417683 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -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; diff --git a/introspect/enums.py b/introspect/enums.py index 4982cf36..8502dbdb 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -90,10 +90,11 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('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), ]), )), diff --git a/introspect/enums_test.py b/introspect/enums_test.py index b4144420..3580d4c1 100644 --- a/introspect/enums_test.py +++ b/introspect/enums_test.py @@ -61,7 +61,7 @@ class EnumsTest(absltest.TestCase): 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) diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 1222569c..3d2e0ac6 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -840,7 +840,7 @@ Euler integrator, semi-implicit in velocity. 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): diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 393bd509..d2e2de20 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -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]) { @@ -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++) { @@ -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++) { @@ -741,10 +703,8 @@ 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 } } @@ -752,6 +712,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } // 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++) { @@ -770,11 +732,6 @@ 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; } @@ -782,7 +739,20 @@ 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; - 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 } } } diff --git a/src/render/render_gl3.c b/src/render/render_gl3.c index 4e108c0d..6535874a 100644 --- a/src/render/render_gl3.c +++ b/src/render/render_gl3.c @@ -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); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index c4969e46..d773cf87 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -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{