chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Encapsulate BVH class members.

Browse Source 
PiperOrigin-RevId: 662141171
Change-Id: Ica202d31dacc9cc500d0aeecded941b79002b9a1
This commit is contained in:
Kyle Bayes
2024-08-12 10:25:52 -07:00
committed by Copybara-Service
parent 648fd56ed1
commit 20cae556ee
4 changed files with 64 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_mesh.cc
+1 -1
View File
@@ -567,7 +567,7 @@ void mjCMesh::Compile(const mjVFS* vfs) {
} }
// make bounding volume hierarchy // make bounding volume hierarchy
if (tree_.bvh.empty()) { if (tree_.Bvh().empty()) {
face_aabb_.assign(6*nface(), 0); face_aabb_.assign(6*nface(), 0);
tree_.AllocateBoundingVolumes(nface()); tree_.AllocateBoundingVolumes(nface());
for (int i=0; i < nface(); i++) { for (int i=0; i < nface(); i++) {
src/user/user_model.cc
+32 -32
View File
@@ -1412,13 +1412,13 @@ void mjCModel::SetSizes() {
// nbvh, nbvhstatic, nbvhdynamic // nbvh, nbvhstatic, nbvhdynamic
for (int i=0; i<nbody; i++) { for (int i=0; i<nbody; i++) {
nbvhstatic += bodies_[i]->tree.nbvh; nbvhstatic += bodies_[i]->tree.Nbvh();
} }
for (int i=0; i<nmesh; i++) { for (int i=0; i<nmesh; i++) {
nbvhstatic += meshes_[i]->tree().nbvh; nbvhstatic += meshes_[i]->tree().Nbvh();
} }
for (int i=0; i<nflex; i++) { for (int i=0; i<nflex; i++) {
nbvhdynamic += flexes_[i]->tree.nbvh; nbvhdynamic += flexes_[i]->tree.Nbvh();
} }
nbvh = nbvhstatic + nbvhdynamic; nbvh = nbvhstatic + nbvhdynamic;
@@ -1884,17 +1884,17 @@ void mjCModel::CopyTree(mjModel* m) {
m->body_margin[i] = (mjtNum)pb->margin; m->body_margin[i] = (mjtNum)pb->margin;
// bounding volume hierarchy // bounding volume hierarchy
m->body_bvhadr[i] = pb->tree.nbvh ? bvh_adr : -1; m->body_bvhadr[i] = pb->tree.Nbvh() ? bvh_adr : -1;
m->body_bvhnum[i] = pb->tree.nbvh; m->body_bvhnum[i] = pb->tree.Nbvh();
if (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_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_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)); memcpy(m->bvh_depth + bvh_adr, pb->tree.Level().data(), pb->tree.Nbvh()*sizeof(int));
for (int i=0; i<pb->tree.nbvh; i++) { for (int i=0; i<pb->tree.Nbvh(); i++) {
m->bvh_nodeid[i + bvh_adr] = pb->tree.nodeid[i] ? *(pb->tree.nodeid[i]) : -1; 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 // count free joints
int cntfree = 0; int cntfree = 0;
@@ -2307,8 +2307,8 @@ void mjCModel::CopyObjects(mjModel* m) {
m->mesh_faceadr[i] = face_adr; m->mesh_faceadr[i] = face_adr;
m->mesh_facenum[i] = pme->nface(); m->mesh_facenum[i] = pme->nface();
m->mesh_graphadr[i] = (pme->szgraph() ? graph_adr : -1); m->mesh_graphadr[i] = (pme->szgraph() ? graph_adr : -1);
m->mesh_bvhnum[i] = pme->tree().nbvh; m->mesh_bvhnum[i] = pme->tree().Nbvh();
m->mesh_bvhadr[i] = pme->tree().nbvh ? bvh_adr : -1; 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_scale[3 * i], pme->get_scale(), 3);
mjuu_copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3); mjuu_copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3);
mjuu_copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4); mjuu_copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4);
@@ -2329,12 +2329,12 @@ void mjCModel::CopyObjects(mjModel* m) {
} }
// copy bvh data // copy bvh data
if (pme->tree().nbvh) { if (pme->tree().Nbvh()) {
memcpy(m->bvh_aabb + 6*bvh_adr, pme->tree().bvh.data(), 6*pme->tree().nbvh*sizeof(mjtNum)); 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_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_depth + bvh_adr, pme->tree().Level().data(), pme->tree().Nbvh()*sizeof(int));
for (int j=0; j<pme->tree().nbvh; j++) { for (int j=0; j<pme->tree().Nbvh(); j++) {
m->bvh_nodeid[j + bvh_adr] = pme->tree().nodeid[j] ? *(pme->tree().nodeid[j]) : -1; 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); texcoord_adr += (pme->HasTexcoord() ? pme->ntexcoord() : 0);
face_adr += pme->nface(); face_adr += pme->nface();
graph_adr += pme->szgraph(); graph_adr += pme->szgraph();
bvh_adr += pme->tree().nbvh; bvh_adr += pme->tree().Nbvh();
} }
// flexes // flexes
@@ -2414,8 +2414,8 @@ void mjCModel::CopyObjects(mjModel* m) {
m->flex_flatskin[i] = pfl->flatskin; m->flex_flatskin[i] = pfl->flatskin;
m->flex_selfcollide[i] = pfl->selfcollide; m->flex_selfcollide[i] = pfl->selfcollide;
m->flex_activelayers[i] = pfl->activelayers; m->flex_activelayers[i] = pfl->activelayers;
m->flex_bvhnum[i] = pfl->tree.nbvh; m->flex_bvhnum[i] = pfl->tree.Nbvh();
m->flex_bvhadr[i] = pfl->tree.nbvh ? bvh_adr : -1; m->flex_bvhadr[i] = pfl->tree.Nbvh() ? bvh_adr : -1;
// find equality constraint referencing this flex // find equality constraint referencing this flex
m->flex_edgeequality[i] = 0; m->flex_edgeequality[i] = 0;
@@ -2427,11 +2427,11 @@ void mjCModel::CopyObjects(mjModel* m) {
} }
// copy bvh data (flex aabb computed dynamically in mjData) // copy bvh data (flex aabb computed dynamically in mjData)
if (pfl->tree.nbvh) { if (pfl->tree.Nbvh()) {
memcpy(m->bvh_child + 2*bvh_adr, pfl->tree.child.data(), 2*pfl->tree.nbvh*sizeof(int)); 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_depth + bvh_adr, pfl->tree.Level().data(), pfl->tree.Nbvh()*sizeof(int));
for (int i=0; i<pfl->tree.nbvh; i++) { for (int i=0; i<pfl->tree.Nbvh(); i++) {
m->bvh_nodeid[i+ bvh_adr] = pfl->tree.nodeid[i] ? *(pfl->tree.nodeid[i]) : -1; 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(); shelldata_adr += (int)pfl->shell.size();
evpair_adr += (int)pfl->evpair.size()/2; evpair_adr += (int)pfl->evpair.size()/2;
texcoord_adr += (int)pfl->texcoord_.size()/2; texcoord_adr += (int)pfl->texcoord_.size()/2;
bvh_adr += pfl->tree.nbvh; bvh_adr += pfl->tree.Nbvh();
} }
// skins // skins
@@ -3117,10 +3117,10 @@ void mjCModel::FuseStatic(void) {
} }
// recompute BVH // recompute BVH
int nbvhfuse = body->tree.nbvh + par->tree.nbvh; int nbvhfuse = body->tree.Nbvh() + par->tree.Nbvh();
par->ComputeBVH(); par->ComputeBVH();
nbvhstatic += par->tree.nbvh - nbvhfuse; nbvhstatic += par->tree.Nbvh() - nbvhfuse;
nbvh += par->tree.nbvh - nbvhfuse; nbvh += par->tree.Nbvh() - nbvhfuse;
//------------- delete body (without deleting children) //------------- delete body (without deleting children)
src/user/user_objects.cc
+17 -18
View File
@@ -340,7 +340,6 @@ const char* ResolveOrientation(double* quat, bool degree, const char* sequence,
// constructor // constructor
mjCBoundingVolumeHierarchy::mjCBoundingVolumeHierarchy() { mjCBoundingVolumeHierarchy::mjCBoundingVolumeHierarchy() {
nbvh = 0;
mjuu_setvec(ipos_, 0, 0, 0); mjuu_setvec(ipos_, 0, 0, 0);
mjuu_setvec(iquat_, 1, 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) { void mjCBoundingVolumeHierarchy::AllocateBoundingVolumes(int nleaf) {
nbvh = 0; nbvh_ = 0;
bvh.clear(); bvh_.clear();
child.clear(); child_.clear();
nodeid.clear(); nodeid_.clear();
level.clear(); level_.clear();
bvleaf_.clear(); bvleaf_.clear();
bvleaf_.resize(nleaf); bvleaf_.resize(nleaf);
} }
@@ -453,26 +452,26 @@ int mjCBoundingVolumeHierarchy::MakeBVH(
} }
// store current index // store current index
int index = nbvh++; int index = nbvh_++;
child.push_back(-1); child_.push_back(-1);
child.push_back(-1); child_.push_back(-1);
nodeid.push_back(nullptr); nodeid_.push_back(nullptr);
level.push_back(lev); level_.push_back(lev);
// store bounding box of the current node // store bounding box of the current node
for (int i=0; i<3; i++) { 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++) { 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 // leaf node, return
if (nelements==1) { if (nelements==1) {
for (int i=0; i<2; i++) { 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; return index;
} }
@@ -491,15 +490,15 @@ int mjCBoundingVolumeHierarchy::MakeBVH(
// recursive calls // recursive calls
if (m > 0) { 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) { 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 // 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", mju_error("this should have been a leaf, body=%s nelements=%d",
name_.c_str(), nelements); name_.c_str(), nelements);
} }
src/user/user_objects.h
+14 -8
View File
@@ -112,14 +112,12 @@ class mjCBoundingVolume {
// bounding volume hierarchy // bounding volume hierarchy
struct mjCBoundingVolumeHierarchy_ { struct mjCBoundingVolumeHierarchy_ {
public:
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> level; // levels of each node (nbvh x 1)
protected: protected:
int nbvh_ = 0;
std::vector<mjtNum> bvh_; // bounding boxes (nbvh x 6)
std::vector<int> child_; // children of each node (nbvh x 2)
std::vector<int*> nodeid_; // id of elem contained by the node (nbvh x 1)
std::vector<int> level_; // levels of each node (nbvh x 1)
std::vector<mjCBoundingVolume> bvleaf_; std::vector<mjCBoundingVolume> bvleaf_;
std::string name_; std::string name_;
double ipos_[3]; double ipos_[3];
@@ -131,12 +129,20 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ {
mjCBoundingVolumeHierarchy(); mjCBoundingVolumeHierarchy();
// make bounding volume hierarchy // make bounding volume hierarchy
void CreateBVH(void); void CreateBVH();
void Set(double ipos_element[3], double iquat_element[4]); void Set(double ipos_element[3], double iquat_element[4]);
void AllocateBoundingVolumes(int nleaf); void AllocateBoundingVolumes(int nleaf);
void RemoveInactiveVolumes(int nmax); void RemoveInactiveVolumes(int nmax);
mjCBoundingVolume* GetBoundingVolume(int id); mjCBoundingVolume* GetBoundingVolume(int id);
// public accessors
int Nbvh() const { return nbvh_; }
const std::vector<mjtNum>& Bvh() const { return bvh_; }
const std::vector<int>& Child() const { return child_; }
const std::vector<int*>& Nodeid() const { return nodeid_; }
const int* Nodeid(int id) const { return nodeid_[id]; }
const std::vector<int>& Level() const { return level_; }
private: private:
// internal class used during BVH construction, for partial sorting of bounding volumes // internal class used during BVH construction, for partial sorting of bounding volumes
struct BVElement { struct BVElement {