diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 44ac6dec..cc494576 100644 --- a/src/user/user_mesh.cc +++ b/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; } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index ca4790f4..7ceb163a 100644 --- a/src/user/user_model.cc +++ b/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; itree.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; jtree().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; itree.nbvh; i++) { + m->bvh_nodeid[i+ bvh_adr] = pfl->tree.nodeid[i] ? *(pfl->tree.nodeid[i]) : -1; + } } // copy or set vert diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 3a51d22d..bffee237 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -372,7 +372,7 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector& 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& 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; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 9fd59d25..4b6df1bd 100644 --- a/src/user/user_objects.h +++ b/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 bvh; // bounding boxes (nbvh x 6) std::vector child; // children of each node (nbvh x 2) - std::vector nodeid; // geom of elem id contained by the node (nbvh x 1) + std::vector nodeid; // geom of elem id contained by the node (nbvh x 1) std::vector level; // levels of each node (nbvh x 1) // make bounding volume hierarchy