Add mesh BVH.

PiperOrigin-RevId: 530559807
Change-Id: Ibdafae22c7c74d15d707f205be73e4a571a6ae10

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;

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)

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;

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                    ) \

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',

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)',
              ),

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"}
 };
 
 

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) {
-        if (bodyid >= m->nbody) {
+          break;
-          mju_error("nbvh outside body range.");
         }
       }
 
+      // 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];
+      drawBoundingBox(thisgeom, d, scn, aabb, xpos, xmat, rgba, i, objtype, category);
-      for (int j=0; j<3; j++) {
+    }
-        for (int k=0; k<3; k++) {
+  }
-          dist[k][j] = aabb[k+3] * xmat[3*j+k];
+
-        }
+  // 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 b = 0; b < m->mesh_bvhnum[meshid]; b++) {
-      for (int v=0; v<8; v++) {
+        int i = b + m->mesh_bvhadr[meshid];
-        mjtNum from[3] = {x[0], x[1], x[2]};
+        int isleaf = m->bvh_child[2*i]==-1 && m->bvh_child[2*i+1]==-1;
-        for (int k=0; k<3; k++) {
+        if (scn->ngeom >= scn->maxgeom) break;
-          mju_addToScl3(from, dist[k], v&split[k] ? 1 : -1);
+        if (m->bvh_depth[i] != vopt->bvh_depth) {
-        }
+          if (!isleaf || m->bvh_depth[i] > vopt->bvh_depth) {
-
+            continue;
-        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
           }
         }
+
+        // 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);
       }
     }
   }

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; i<nface; i++) {
+      tree.AddBundingVolume(GetBoundingVolume(i));
+    }
+    tree.CreateBVH();
+  }
+}
+
+
+
+// get bounding volume
+mjCBoundingVolume mjCMesh::GetBoundingVolume(int faceid) {
+  mjCBoundingVolume node;
+  node.id = faceid;
+  node.conaffinity = 1;
+  node.contype = 1;
+  node.pos = center + 3*faceid;
+  node.quat = NULL;
+  mjtNum AABB[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10};
+  for (int j=0; j<3; j++) {
+    int vertid = face[3*faceid+j];
+    AABB[0] = mjMIN(AABB[0], vert[3*vertid+0]);
+    AABB[1] = mjMIN(AABB[1], vert[3*vertid+1]);
+    AABB[2] = mjMIN(AABB[2], vert[3*vertid+2]);
+    AABB[3] = mjMAX(AABB[3], vert[3*vertid+0]);
+    AABB[4] = mjMAX(AABB[4], vert[3*vertid+1]);
+    AABB[5] = mjMAX(AABB[5], vert[3*vertid+2]);
+  }
+  face_aabb[6*faceid+0] = .5 * (AABB[0] + AABB[3]);
+  face_aabb[6*faceid+1] = .5 * (AABB[1] + AABB[4]);
+  face_aabb[6*faceid+2] = .5 * (AABB[2] + AABB[5]);
+  face_aabb[6*faceid+3] = .5 * (AABB[3] - AABB[0]);
+  face_aabb[6*faceid+4] = .5 * (AABB[4] - AABB[1]);
+  face_aabb[6*faceid+5] = .5 * (AABB[5] - AABB[2]);
+  node.aabb = face_aabb.data() + 6*faceid;
+  return node;
 }
 
 
@@ -1488,6 +1534,52 @@ void mjCMesh::MakeNormal(void) {
 
 
 
+// compute face circumradii
+void mjCMesh::MakeCenter(void) {
+  if (center) {
+    return;
+  }
+
+  // allocate and clear
+  center = (double*) mju_malloc(3*nface*sizeof(double));
+  memset(center, 0, 3*nface*sizeof(double));
+
+  for (int i=0; i<nface; i++) {
+    // get vertex ids
+    int* vertid = face + 3*i;
+
+    // get triangle edges
+    mjtNum a[3], b[3], c[3];
+    for (int j=0; j<3; j++) {
+      a[j] = vert[3*vertid[0]+j] - vert[3*vertid[2]+j];
+      b[j] = vert[3*vertid[1]+j] - vert[3*vertid[2]+j];
+      c[j] = vert[3*vertid[0]+j] - vert[3*vertid[1]+j];
+    }
+
+    // compute face normal
+    mjtNum nrm[3];
+    mju_cross(nrm, a, b);
+
+    // compute circumradius
+    mjtNum norm_a_2 = mju_dot3(a, a);
+    mjtNum norm_b_2 = mju_dot3(b, b);
+    mjtNum area = mju_norm3(nrm);
+
+    // compute circumcenter
+    mjtNum res[3], vec[3] = {
+      norm_a_2 * b[0] - norm_b_2 * a[0],
+      norm_a_2 * b[1] - norm_b_2 * a[1],
+      norm_a_2 * b[2] - norm_b_2 * a[2]
+    };
+    mju_cross(res, vec, nrm);
+    center[3*i+0] = res[0]/(2*area*area) + vert[3*vertid[2]+0];
+    center[3*i+1] = res[1]/(2*area*area) + vert[3*vertid[2]+1];
+    center[3*i+2] = res[2]/(2*area*area) + vert[3*vertid[2]+2];
+  }
+}
+
+
+
 //------------------ class mjCSkin implementation --------------------------------------------------
 
 // constructor

src/user/user_model.cc

@@ -939,6 +939,7 @@ void mjCModel::SetSizes(void) {
     nmeshface += meshes[i]->nface;
     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; i<nbody; i++) {
+    bvh_adr = mju_max(bvh_adr, m->body_bvhadr[i] + m->body_bvhnum[i]);
+  }
   for (int i=0; i<nmesh; i++) {
     // get pointer
     mjCMesh* pme = meshes[i];
@@ -1714,6 +1718,8 @@ void mjCModel::CopyObjects(mjModel* m) {
     m->mesh_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

src/user/user_objects.cc

@@ -321,11 +321,14 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
       vert[2] = (v&4 ? aabb[5] : aabb[2]);
 
       // rotate to the body inertial frame
-      mju_rotVecQuat(box, vert, elements[i].quat);
+      if (elements[i].quat) {
-      box[0] += elements[i].pos[0] - ipos_[0];
+        mju_rotVecQuat(box, vert, elements[i].quat);
-      box[1] += elements[i].pos[1] - ipos_[1];
+        box[0] += elements[i].pos[0] - ipos_[0];
-      box[2] += elements[i].pos[2] - ipos_[2];
+        box[1] += elements[i].pos[1] - ipos_[1];
-      mju_rotVecQuat(vert, box, qinv);
+        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<mjCBoundingVolume>& 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

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<double> face_aabb;      // bounding boxes of all faces
 };
 
 

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;
 }