Store pointer to index in mjCBoundingVolume if possible.

PiperOrigin-RevId: 598592242
Change-Id: I8dc20a8d8a9a0e967688839fa22d75263c226958

src/user/user_mesh.cc

@@ -600,7 +600,7 @@ void mjCMesh::Compile(const mjVFS* vfs) {
 // get bounding volume
 void mjCMesh::SetBoundingVolume(int faceid) {
   mjCBoundingVolume* node = tree_.GetBoundingVolume(faceid);
-  node->id = faceid;
+  node->SetId(faceid);
   node->conaffinity = 1;
   node->contype = 1;
   node->pos = center_ + 3*faceid;
@@ -2467,7 +2467,7 @@ void mjCFlex::CreateBVH(void) {
     bv->contype = contype;
     bv->conaffinity = conaffinity;
     bv->quat = NULL;
-    bv->id = e;
+    bv->SetId(e);
     bv->aabb = elemaabb.data() + 6*e;
     bv->pos = bv->aabb;
   }

src/user/user_model.cc

@@ -1358,8 +1358,10 @@ void mjCModel::CopyTree(mjModel* m) {
     if (pb->tree.nbvh) {
       memcpy(m->bvh_aabb + 6*bvh_adr, pb->tree.bvh.data(), 6*pb->tree.nbvh*sizeof(mjtNum));
       memcpy(m->bvh_child + 2*bvh_adr, pb->tree.child.data(), 2*pb->tree.nbvh*sizeof(int));
-      memcpy(m->bvh_nodeid + bvh_adr, pb->tree.nodeid.data(), pb->tree.nbvh*sizeof(int));
       memcpy(m->bvh_depth + bvh_adr, pb->tree.level.data(), pb->tree.nbvh*sizeof(int));
+      for (int i=0; i<pb->tree.nbvh; i++) {
+        m->bvh_nodeid[i + bvh_adr] = pb->tree.nodeid[i] ? *(pb->tree.nodeid[i]) : -1;
+      }
     }
     bvh_adr += pb->tree.nbvh;
 
@@ -1787,7 +1789,9 @@ void mjCModel::CopyObjects(mjModel* m) {
       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_nodeid + bvh_adr, pme->tree().nodeid.data(), pme->tree().nbvh*sizeof(int));
+      for (int j=0; j<pme->tree().nbvh; j++) {
+        m->bvh_nodeid[j + bvh_adr] = pme->tree().nodeid[j] ? *(pme->tree().nodeid[j]) : -1;
+      }
     }
 
     // advance counters
@@ -1882,7 +1886,9 @@ void mjCModel::CopyObjects(mjModel* m) {
     if (pfl->tree.nbvh) {
       memcpy(m->bvh_child + 2*bvh_adr, pfl->tree.child.data(), 2*pfl->tree.nbvh*sizeof(int));
       memcpy(m->bvh_depth + bvh_adr, pfl->tree.level.data(), pfl->tree.nbvh*sizeof(int));
-      memcpy(m->bvh_nodeid + bvh_adr, pfl->tree.nodeid.data(), pfl->tree.nbvh*sizeof(int));
+      for (int i=0; i<pfl->tree.nbvh; i++) {
+        m->bvh_nodeid[i+ bvh_adr] = pfl->tree.nodeid[i] ? *(pfl->tree.nodeid[i]) : -1;
+      }
     }
 
     // copy or set vert

src/user/user_objects.cc

@@ -372,7 +372,7 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<const mjCBoundingVolume*>& e
   int index = nbvh++;
   child.push_back(-1);
   child.push_back(-1);
-  nodeid.push_back(-1);
+  nodeid.push_back(nullptr);
   level.push_back(lev);
 
   // store bounding box of the current node
@@ -388,7 +388,7 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<const mjCBoundingVolume*>& e
     for (int i=0; i<2; i++) {
       child[2*index+i] = -1;
     }
-    nodeid[index] = elements[0]->id;
+    nodeid[index] = (int*)elements[0]->GetId();
     return index;
   }
 
@@ -1412,7 +1412,7 @@ double mjCGeom::GetVolume(void) {
 
 
 void mjCGeom::SetBoundingVolume(mjCBoundingVolume* bv) const {
-  bv->id = id;
+  bv->SetId(&id);
   bv->contype = contype;
   bv->conaffinity = conaffinity;
   bv->aabb = aabb;

src/user/user_objects.h

@@ -125,14 +125,21 @@ class mjCAlternative {
 // bounding volume
 class mjCBoundingVolume {
  public:
-  mjCBoundingVolume() = default;
+  mjCBoundingVolume() { id_ = nullptr; };
 
-  int id;                       // object id
   int contype;                  // contact type
   int conaffinity;              // contact affinity
   const mjtNum* aabb;           // axis-aligned bounding box (center, size)
   const mjtNum* pos;            // position (set by user or Compile1)
   const mjtNum* quat;           // orientation (set by user or Compile1)
+
+  const int* GetId() const { if (id_) return id_; else return &idval_; }
+  void SetId(const int* id) { id_ = id; }
+  void SetId(int val) { idval_ = val; }
+
+ private:
+  int idval_;                   // local id copy for nodes not storing their id's (e.g. faces)
+  const int* id_;               // pointer to object id
 };
 
 
@@ -144,7 +151,7 @@ class mjCBoundingVolumeHierarchy {
   int nbvh;
   std::vector<mjtNum> bvh;            // bounding boxes                                (nbvh x 6)
   std::vector<int> child;             // children of each node                         (nbvh x 2)
-  std::vector<int> nodeid;            // geom of elem id contained by the node         (nbvh x 1)
+  std::vector<int*> nodeid;           // geom of elem id contained by the node         (nbvh x 1)
   std::vector<int> level;             // levels of each node                           (nbvh x 1)
 
   // make bounding volume hierarchy