From 20cae556eecfb5499f895363414cec68e0ab9f84 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Mon, 12 Aug 2024 10:25:52 -0700 Subject: [PATCH] Encapsulate BVH class members. PiperOrigin-RevId: 662141171 Change-Id: Ica202d31dacc9cc500d0aeecded941b79002b9a1 --- src/user/user_mesh.cc | 2 +- src/user/user_model.cc | 64 ++++++++++++++++++++-------------------- src/user/user_objects.cc | 35 +++++++++++----------- src/user/user_objects.h | 22 +++++++++----- 4 files changed, 64 insertions(+), 59 deletions(-) diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 5d8d82f9..b5e8e6f9 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -567,7 +567,7 @@ void mjCMesh::Compile(const mjVFS* vfs) { } // make bounding volume hierarchy - if (tree_.bvh.empty()) { + if (tree_.Bvh().empty()) { face_aabb_.assign(6*nface(), 0); tree_.AllocateBoundingVolumes(nface()); for (int i=0; i < nface(); i++) { diff --git a/src/user/user_model.cc b/src/user/user_model.cc index cf94b2d6..f08f8fc1 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1412,13 +1412,13 @@ void mjCModel::SetSizes() { // nbvh, nbvhstatic, nbvhdynamic for (int i=0; itree.nbvh; + nbvhstatic += bodies_[i]->tree.Nbvh(); } for (int i=0; itree().nbvh; + nbvhstatic += meshes_[i]->tree().Nbvh(); } for (int i=0; itree.nbvh; + nbvhdynamic += flexes_[i]->tree.Nbvh(); } nbvh = nbvhstatic + nbvhdynamic; @@ -1884,17 +1884,17 @@ void mjCModel::CopyTree(mjModel* m) { m->body_margin[i] = (mjtNum)pb->margin; // bounding volume hierarchy - m->body_bvhadr[i] = pb->tree.nbvh ? bvh_adr : -1; - m->body_bvhnum[i] = pb->tree.nbvh; - 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_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; + m->body_bvhadr[i] = pb->tree.Nbvh() ? bvh_adr : -1; + m->body_bvhnum[i] = pb->tree.Nbvh(); + 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_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; + bvh_adr += pb->tree.Nbvh(); // count free joints int cntfree = 0; @@ -2307,8 +2307,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_bvhnum[i] = pme->tree().nbvh; - m->mesh_bvhadr[i] = pme->tree().nbvh ? bvh_adr : -1; + m->mesh_bvhnum[i] = pme->tree().Nbvh(); + m->mesh_bvhadr[i] = pme->tree().Nbvh() ? bvh_adr : -1; mjuu_copyvec(&m->mesh_scale[3 * i], pme->get_scale(), 3); mjuu_copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3); mjuu_copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4); @@ -2329,12 +2329,12 @@ void mjCModel::CopyObjects(mjModel* m) { } // copy bvh data - if (pme->tree().nbvh) { - 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)); - for (int j=0; jtree().nbvh; j++) { - m->bvh_nodeid[j + bvh_adr] = pme->tree().nodeid[j] ? *(pme->tree().nodeid[j]) : -1; + if (pme->tree().Nbvh()) { + 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)); + for (int j=0; jtree().Nbvh(); j++) { + m->bvh_nodeid[j + bvh_adr] = pme->tree().Nodeid(j) ? *(pme->tree().Nodeid(j)) : -1; } } @@ -2344,7 +2344,7 @@ void mjCModel::CopyObjects(mjModel* m) { texcoord_adr += (pme->HasTexcoord() ? pme->ntexcoord() : 0); face_adr += pme->nface(); graph_adr += pme->szgraph(); - bvh_adr += pme->tree().nbvh; + bvh_adr += pme->tree().Nbvh(); } // flexes @@ -2414,8 +2414,8 @@ void mjCModel::CopyObjects(mjModel* m) { m->flex_flatskin[i] = pfl->flatskin; m->flex_selfcollide[i] = pfl->selfcollide; m->flex_activelayers[i] = pfl->activelayers; - m->flex_bvhnum[i] = pfl->tree.nbvh; - m->flex_bvhadr[i] = pfl->tree.nbvh ? bvh_adr : -1; + m->flex_bvhnum[i] = pfl->tree.Nbvh(); + m->flex_bvhadr[i] = pfl->tree.Nbvh() ? bvh_adr : -1; // find equality constraint referencing this flex m->flex_edgeequality[i] = 0; @@ -2427,11 +2427,11 @@ void mjCModel::CopyObjects(mjModel* m) { } // copy bvh data (flex aabb computed dynamically in mjData) - 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)); - for (int i=0; itree.nbvh; i++) { - m->bvh_nodeid[i+ bvh_adr] = pfl->tree.nodeid[i] ? *(pfl->tree.nodeid[i]) : -1; + 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)); + for (int i=0; itree.Nbvh(); i++) { + m->bvh_nodeid[i+ bvh_adr] = pfl->tree.Nodeid(i) ? *(pfl->tree.Nodeid(i)) : -1; } } @@ -2476,7 +2476,7 @@ void mjCModel::CopyObjects(mjModel* m) { shelldata_adr += (int)pfl->shell.size(); evpair_adr += (int)pfl->evpair.size()/2; texcoord_adr += (int)pfl->texcoord_.size()/2; - bvh_adr += pfl->tree.nbvh; + bvh_adr += pfl->tree.Nbvh(); } // skins @@ -3117,10 +3117,10 @@ void mjCModel::FuseStatic(void) { } // recompute BVH - int nbvhfuse = body->tree.nbvh + par->tree.nbvh; + int nbvhfuse = body->tree.Nbvh() + par->tree.Nbvh(); par->ComputeBVH(); - nbvhstatic += par->tree.nbvh - nbvhfuse; - nbvh += par->tree.nbvh - nbvhfuse; + nbvhstatic += par->tree.Nbvh() - nbvhfuse; + nbvh += par->tree.Nbvh() - nbvhfuse; //------------- delete body (without deleting children) diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 092837df..beed497f 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -340,7 +340,6 @@ const char* ResolveOrientation(double* quat, bool degree, const char* sequence, // constructor mjCBoundingVolumeHierarchy::mjCBoundingVolumeHierarchy() { - nbvh = 0; mjuu_setvec(ipos_, 0, 0, 0); mjuu_setvec(iquat_, 1, 0, 0, 0); } @@ -355,11 +354,11 @@ void mjCBoundingVolumeHierarchy::Set(double ipos_element[3], double iquat_elemen void mjCBoundingVolumeHierarchy::AllocateBoundingVolumes(int nleaf) { - nbvh = 0; - bvh.clear(); - child.clear(); - nodeid.clear(); - level.clear(); + nbvh_ = 0; + bvh_.clear(); + child_.clear(); + nodeid_.clear(); + level_.clear(); bvleaf_.clear(); bvleaf_.resize(nleaf); } @@ -453,26 +452,26 @@ int mjCBoundingVolumeHierarchy::MakeBVH( } // store current index - int index = nbvh++; - child.push_back(-1); - child.push_back(-1); - nodeid.push_back(nullptr); - level.push_back(lev); + int index = nbvh_++; + child_.push_back(-1); + child_.push_back(-1); + nodeid_.push_back(nullptr); + level_.push_back(lev); // store bounding box of the current node for (int i=0; i<3; i++) { - bvh.push_back((AAMM[3+i] + AAMM[i]) / 2); + bvh_.push_back((AAMM[3+i] + AAMM[i]) / 2); } for (int i=0; i<3; i++) { - bvh.push_back((AAMM[3+i] - AAMM[i]) / 2); + bvh_.push_back((AAMM[3+i] - AAMM[i]) / 2); } // leaf node, return if (nelements==1) { for (int i=0; i<2; i++) { - child[2*index+i] = -1; + child_[2*index+i] = -1; } - nodeid[index] = (int*)elements_begin->e->GetId(); + nodeid_[index] = (int*)elements_begin->e->GetId(); return index; } @@ -491,15 +490,15 @@ int mjCBoundingVolumeHierarchy::MakeBVH( // recursive calls if (m > 0) { - child[2*index+0] = MakeBVH(elements_begin, elements_begin + m, lev+1); + child_[2*index+0] = MakeBVH(elements_begin, elements_begin + m, lev+1); } if (m != nelements) { - child[2*index+1] = MakeBVH(elements_begin + m, elements_end, lev+1); + child_[2*index+1] = MakeBVH(elements_begin + m, elements_end, lev+1); } // SHOULD NOT OCCUR - if (child[2*index+0]==-1 && child[2*index+1]==-1) { + if (child_[2*index+0]==-1 && child_[2*index+1]==-1) { mju_error("this should have been a leaf, body=%s nelements=%d", name_.c_str(), nelements); } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index a6540a81..91b4842d 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -112,14 +112,12 @@ class mjCBoundingVolume { // bounding volume hierarchy struct mjCBoundingVolumeHierarchy_ { - public: - 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 level; // levels of each node (nbvh x 1) - protected: + int nbvh_ = 0; + std::vector bvh_; // bounding boxes (nbvh x 6) + std::vector child_; // children of each node (nbvh x 2) + std::vector nodeid_; // id of elem contained by the node (nbvh x 1) + std::vector level_; // levels of each node (nbvh x 1) std::vector bvleaf_; std::string name_; double ipos_[3]; @@ -131,12 +129,20 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ { mjCBoundingVolumeHierarchy(); // make bounding volume hierarchy - void CreateBVH(void); + void CreateBVH(); void Set(double ipos_element[3], double iquat_element[4]); void AllocateBoundingVolumes(int nleaf); void RemoveInactiveVolumes(int nmax); mjCBoundingVolume* GetBoundingVolume(int id); + // public accessors + int Nbvh() const { return nbvh_; } + const std::vector& Bvh() const { return bvh_; } + const std::vector& Child() const { return child_; } + const std::vector& Nodeid() const { return nodeid_; } + const int* Nodeid(int id) const { return nodeid_[id]; } + const std::vector& Level() const { return level_; } + private: // internal class used during BVH construction, for partial sorting of bounding volumes struct BVElement {