diff --git a/doc/includes/references.h b/doc/includes/references.h index 0a8ae066..8e3a2ef1 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -989,6 +989,8 @@ struct mjModel_ { int* mesh_vertnum; // number of vertices (nmesh x 1) int* mesh_faceadr; // first face address (nmesh x 1) int* mesh_facenum; // number of faces (nmesh x 1) + int* mesh_bvhadr; // address of bvh root (nmesh x 1) + int* mesh_bvhnum; // number of bvh (nmesh x 1) int* mesh_normaladr; // first normal address (nmesh x 1) int* mesh_normalnum; // number of normals (nmesh x 1) int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1) @@ -1666,6 +1668,7 @@ typedef enum mjtVisFlag_ { // flags enabling model element visualization mjVIS_STATIC, // static bodies mjVIS_SKIN, // skin mjVIS_MIDPHASE, // mid-phase bounding volume hierarchy + mjVIS_MESHBVH, // mesh bounding volume hierarchy mjNVISFLAG // number of visualization flags } mjtVisFlag; diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 9fff6995..259ceee1 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -771,6 +771,8 @@ struct mjModel_ { int* mesh_vertnum; // number of vertices (nmesh x 1) int* mesh_faceadr; // first face address (nmesh x 1) int* mesh_facenum; // number of faces (nmesh x 1) + int* mesh_bvhadr; // address of bvh root (nmesh x 1) + int* mesh_bvhnum; // number of bvh (nmesh x 1) int* mesh_normaladr; // first normal address (nmesh x 1) int* mesh_normalnum; // number of normals (nmesh x 1) int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1) diff --git a/include/mujoco/mjvisualize.h b/include/mujoco/mjvisualize.h index 7a035d79..53c124c3 100644 --- a/include/mujoco/mjvisualize.h +++ b/include/mujoco/mjvisualize.h @@ -123,6 +123,7 @@ typedef enum mjtVisFlag_ { // flags enabling model element visualization mjVIS_STATIC, // static bodies mjVIS_SKIN, // skin mjVIS_MIDPHASE, // mid-phase bounding volume hierarchy + mjVIS_MESHBVH, // mesh bounding volume hierarchy mjNVISFLAG // number of visualization flags } mjtVisFlag; diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 43667ffc..ec5cf8f4 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -281,6 +281,8 @@ X ( int, mesh_texcoordnum, nmesh, 1 ) \ X ( int, mesh_faceadr, nmesh, 1 ) \ X ( int, mesh_facenum, nmesh, 1 ) \ + X ( int, mesh_bvhadr, nmesh, 1 ) \ + X ( int, mesh_bvhnum, nmesh, 1 ) \ XMJV( int, mesh_graphadr, nmesh, 1 ) \ X ( float, mesh_vert, nmeshvert, 3 ) \ X ( float, mesh_normal, nmeshnormal, 3 ) \ diff --git a/introspect/enums.py b/introspect/enums.py index 22f04147..2b82759b 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -523,7 +523,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjVIS_STATIC', 21), ('mjVIS_SKIN', 22), ('mjVIS_MIDPHASE', 23), - ('mjNVISFLAG', 24), + ('mjVIS_MESHBVH', 24), + ('mjNVISFLAG', 25), ]), )), ('mjtRndFlag', diff --git a/introspect/structs.py b/introspect/structs.py index 7188886c..ec09d856 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -1993,6 +1993,20 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='number of faces (nmesh x 1)', ), + StructFieldDecl( + name='mesh_bvhadr', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='address of bvh root (nmesh x 1)', + ), + StructFieldDecl( + name='mesh_bvhnum', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='number of bvh (nmesh x 1)', + ), StructFieldDecl( name='mesh_normaladr', type=PointerType( @@ -4758,7 +4772,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ name='flags', type=ArrayType( inner_type=ValueType(name='mjtByte'), - extents=(24,), + extents=(25,), ), doc='visualization flags (indexed by mjtVisFlag)', ), diff --git a/src/engine/engine_vis_init.c b/src/engine/engine_vis_init.c index 2b0db455..a2c97de3 100644 --- a/src/engine/engine_vis_init.c +++ b/src/engine/engine_vis_init.c @@ -89,7 +89,8 @@ const char* mjVISSTRING[mjNVISFLAG][3] = { {"S&elect Point", "0", "E"}, {"Static Bo&dy", "1", "D"}, {"Skin", "1", ";"}, - {"Body Tree", "0", "`"} + {"Body Tree", "0", "`"}, + {"Mesh Tree", "0", "1"} }; diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 97f402ee..a5439de0 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -458,6 +458,57 @@ static int bodycategory(const mjModel* m, int bodyid) { } } + + +// draw bounding box +static void drawBoundingBox(mjvGeom* thisgeom, mjData* d, mjvScene* scn, + const mjtNum aabb[6], const mjtNum xpos[3], + const mjtNum xmat[9], const float rgba[4], + int i, int objtype, int category) { + mjtNum x[3]; + mjtNum dist[3][3]; + + 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_makeConnector(thisgeom, mjGEOM_LINE, 2, + from[0], from[1], from[2], + to[0], to[1], to[2]); + f2f(thisgeom->rgba, rgba, 4); + FINISH + } + } + } +} + + + // add abstract geoms void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, const mjvPerturb* pert, int catmask, mjvScene* scn) { @@ -536,51 +587,57 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, // find geom number int geomid = m->bvh_geomid[i]; while (i >= m->body_bvhadr[bodyid] + m->body_bvhnum[bodyid]) { - bodyid++; - if (bodyid >= m->nbody) { - mju_error("nbvh outside body range."); + if (++bodyid >= m->nbody) { + break; } } + // stop after body bvh are finished + if (bodyid >= m->nbody) { + break; + } + // compute transformation mjtNum *aabb = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i; - mjtNum x[3]; 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; - mju_rotVecMat(x, aabb, xmat); - mju_addTo3(x, xpos); - rgba[0] = d->bvh_active[i] ? 1 : 0; rgba[1] = d->bvh_active[i] ? 0 : 1; - mjtNum dist[3][3]; - for (int j=0; j<3; j++) { - for (int k=0; k<3; k++) { - dist[k][j] = aabb[k+3] * xmat[3*j+k]; - } + drawBoundingBox(thisgeom, d, scn, aabb, xpos, xmat, rgba, i, objtype, category); + } + } + + // mesh bounding volume hierarchy + 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) { + continue; } - 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_makeConnector(thisgeom, mjGEOM_LINE, 2, - from[0], from[1], from[2], - to[0], to[1], to[2]); - f2f(thisgeom->rgba, rgba, 4); - FINISH + for (int b = 0; b < m->mesh_bvhnum[meshid]; b++) { + int i = b + m->mesh_bvhadr[meshid]; + int isleaf = m->bvh_child[2*i]==-1 && m->bvh_child[2*i+1]==-1; + if (scn->ngeom >= scn->maxgeom) break; + if (m->bvh_depth[i] != vopt->bvh_depth) { + if (!isleaf || m->bvh_depth[i] > vopt->bvh_depth) { + continue; } } + + // 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; + + rgba[0] = d->bvh_active[i] ? 1 : 0; + rgba[1] = d->bvh_active[i] ? 0 : 1; + + drawBoundingBox(thisgeom, d, scn, aabb, xpos, xmat, rgba, i, objtype, category); } } } diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 0912bb12..d82eae4b 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -113,6 +113,7 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) { szgraph = 0; vert = NULL; normal = NULL; + center = NULL; texcoord = NULL; face = NULL; facenormal = NULL; @@ -153,6 +154,7 @@ mjCMesh::~mjCMesh() { if (vert) mju_free(vert); if (normal) mju_free(normal); if (texcoord) mju_free(texcoord); + if (center) mju_free(center); if (face) mju_free(face); if (facenormal) mju_free(facenormal); if (facetexcoord) mju_free(facetexcoord); @@ -370,6 +372,50 @@ void mjCMesh::Compile(int vfs_provider) { // scale, center, orient, compute mass and inertia Process(); processed = true; + + // no radii: make + if (!center) { + MakeCenter(); + } + + // make bounding volume hierarchy + if (tree.bvh.empty()) { + face_aabb.assign(6*nface, 0); + for (int i=0; inface; nmeshtexcoord += (meshes[i]->texcoord ? meshes[i]->ntexcoord : 0); nmeshgraph += meshes[i]->szgraph; + nbvh += meshes[i]->tree.nbvh; } // nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert @@ -1685,7 +1686,7 @@ void mjCModel::CopyTree(mjModel* m) { // copy objects outside kinematic tree void mjCModel::CopyObjects(mjModel* m) { int adr, bone_adr, vert_adr, normal_adr, face_adr, texcoord_adr; - int bonevert_adr, graph_adr, data_adr; + int bonevert_adr, graph_adr, data_adr, bvh_adr=0; // sizes outside call to mj_makeModel m->nemax = nemax; @@ -1700,6 +1701,9 @@ void mjCModel::CopyObjects(mjModel* m) { texcoord_adr = 0; face_adr = 0; graph_adr = 0; + for (int i=0; ibody_bvhadr[i] + m->body_bvhnum[i]); + } for (int i=0; imesh_faceadr[i] = face_adr; m->mesh_facenum[i] = pme->nface; m->mesh_graphadr[i] = (pme->szgraph ? graph_adr : -1); + m->mesh_bvhadr[i] = bvh_adr; + m->mesh_bvhnum[i] = pme->tree.nbvh; // copy vertices, normals, faces, texcoords, aux data memcpy(m->mesh_vert + 3*vert_adr, pme->vert, 3*pme->nvert*sizeof(float)); @@ -1729,6 +1735,10 @@ void mjCModel::CopyObjects(mjModel* m) { if (pme->szgraph) { memcpy(m->mesh_graph + graph_adr, pme->graph, pme->szgraph*sizeof(int)); } + memcpy(m->bvh_aabb + 6*bvh_adr, pme->tree.bvh.data(), 6*pme->tree.nbvh*sizeof(mjtNum)); + memcpy(m->bvh_child + 2*bvh_adr, pme->tree.child.data(), 2*pme->tree.nbvh*sizeof(int)); + memcpy(m->bvh_depth + bvh_adr, pme->tree.level.data(), pme->tree.nbvh*sizeof(int)); + memcpy(m->bvh_geomid + bvh_adr, pme->tree.nodeid.data(), pme->tree.nbvh*sizeof(int)); // advance counters vert_adr += pme->nvert; @@ -1736,6 +1746,7 @@ void mjCModel::CopyObjects(mjModel* m) { texcoord_adr += (pme->texcoord ? pme->ntexcoord : 0); face_adr += pme->nface; graph_adr += pme->szgraph; + bvh_adr += pme->tree.nbvh; } // skins diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 71c99ad4..9ec76f22 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -321,11 +321,14 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector& elements vert[2] = (v&4 ? aabb[5] : aabb[2]); // rotate to the body inertial frame - mju_rotVecQuat(box, vert, elements[i].quat); - box[0] += elements[i].pos[0] - ipos_[0]; - box[1] += elements[i].pos[1] - ipos_[1]; - box[2] += elements[i].pos[2] - ipos_[2]; - mju_rotVecQuat(vert, box, qinv); + if (elements[i].quat) { + mju_rotVecQuat(box, vert, elements[i].quat); + box[0] += elements[i].pos[0] - ipos_[0]; + box[1] += elements[i].pos[1] - ipos_[1]; + box[2] += elements[i].pos[2] - ipos_[2]; + mju_rotVecQuat(vert, box, qinv); + } + AABB[0] = mjMIN(AABB[0], vert[0]); AABB[1] = mjMIN(AABB[1], vert[1]); AABB[2] = mjMIN(AABB[2], vert[2]); @@ -342,18 +345,12 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector& elements nodeid.push_back(-1); level.push_back(lev); - // transform representation - mjtNum center[] = {(AABB[3] + AABB[0]) / 2, (AABB[4] + AABB[1]) / 2, - (AABB[5] + AABB[2]) / 2}; - mjtNum size[] = {(AABB[3] - AABB[0]) / 2, (AABB[4] - AABB[1]) / 2, - (AABB[5] - AABB[2]) / 2}; - // store bounding box of the current node for (int i=0; i<3; i++) { - bvh.push_back(center[i]); + bvh.push_back((AABB[3+i] + AABB[i]) / 2); } for (int i=0; i<3; i++) { - bvh.push_back(size[i]); + bvh.push_back((AABB[3+i] - AABB[i]) / 2); } // leaf node, return diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 8b4188b8..0058df75 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -120,6 +120,7 @@ class mjCAlternative { //------------------------- class mjCBoundingVolumeHierarchy --------------------------------------- + // bounding volume class mjCBoundingVolume { public: @@ -334,7 +335,6 @@ class mjCGeom : public mjCBase { friend class mjCModel; friend class mjXWriter; friend class mjXURDF; - friend class mjCBoundingVolumeHierarchy; public: double GetVolume(void); // compute geom volume @@ -524,7 +524,10 @@ class mjCMesh: public mjCBase { double* GetQuatPtr(mjtMeshType type); // get orientation double* GetInertiaBoxPtr(mjtMeshType type); // get inertia box double& GetVolumeRef(mjtMeshType type); // get volume - void FitGeom(mjCGeom* geom, double* meshpos); // approximate mesh with simple geom + void FitGeom(mjCGeom* geom, double* meshpos); // approximate mesh with simple geom + + // returns a bounding volume given a face + mjCBoundingVolume GetBoundingVolume(int faceid); std::string file; // mesh file double refpos[3]; // reference position (translate) @@ -550,6 +553,7 @@ class mjCMesh: public mjCBase { void MakeGraph(void); // make graph of convex hull void CopyGraph(void); // copy graph into face data void MakeNormal(void); // compute vertex normals + void MakeCenter(void); // compute face circumcircle data void Process(); // apply transformations void RemoveRepeated(void); // remove repeated vertices void CheckMesh(void); // check if the mesh is valid @@ -585,6 +589,7 @@ class mjCMesh: public mjCBase { int szgraph; // size of graph data in ints float* vert; // vertex data (3*nvert), relative to (pos, quat) float* normal; // vertex normal data (3*nnormal) + double* center; // face circumcenter data (3*nface) float* texcoord; // vertex texcoord data (2*ntexcoord or NULL) int* face; // face vertex indices (3*nface) int* facenormal; // face normal indices (3*nface) @@ -592,6 +597,9 @@ class mjCMesh: public mjCBase { int* graph; // convex graph data bool needhull; // needs convex hull for collisions + + mjCBoundingVolumeHierarchy tree; // bounding volume hierarchy + std::vector face_aabb; // bounding boxes of all faces }; diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 8280be3f..1ad01525 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -487,7 +487,8 @@ public enum mjtVisFlag : int{ mjVIS_STATIC = 21, mjVIS_SKIN = 22, mjVIS_MIDPHASE = 23, - mjNVISFLAG = 24, + mjVIS_MESHBVH = 24, + mjNVISFLAG = 25, } public enum mjtRndFlag : int{ mjRND_SHADOW = 0, @@ -2067,6 +2068,8 @@ public unsafe struct mjModel_ { public int* mesh_vertnum; public int* mesh_faceadr; public int* mesh_facenum; + public int* mesh_bvhadr; + public int* mesh_bvhnum; public int* mesh_normaladr; public int* mesh_normalnum; public int* mesh_texcoordadr; @@ -2574,7 +2577,7 @@ public unsafe struct mjvOption_ { public fixed byte tendongroup[6]; public fixed byte actuatorgroup[6]; public fixed byte skingroup[6]; - public fixed byte flags[24]; + public fixed byte flags[25]; public int bvh_depth; }