From 1a67aaf1b74d945e405f9c5b9626a3fb52bb568a Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Fri, 14 Mar 2025 07:00:23 -0700 Subject: [PATCH] Remove pointers from BVH for caching in mjCMesh. PiperOrigin-RevId: 736849392 Change-Id: I684cca918c65718b29a1bcb41ecd6bad8d88f8f9 --- src/user/user_mesh.cc | 65 ++++++++++---------- src/user/user_model.cc | 6 +- src/user/user_objects.cc | 129 ++++++++++++++++++++------------------- src/user/user_objects.h | 101 +++++++++++++++++++----------- 4 files changed, 166 insertions(+), 135 deletions(-) diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 5c6b8838..6c764fa9 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -648,9 +649,10 @@ void mjCMesh::TryCompile(const mjVFS* vfs) { // make bounding volume hierarchy if (tree_.Bvh().empty()) { - face_aabb_.assign(6*nface(), 0); + face_aabb_.clear(); + face_aabb_.reserve(3*face_.size()); tree_.AllocateBoundingVolumes(nface()); - for (int i=0; i < nface(); i++) { + for (int i = 0; i < nface(); i++) { SetBoundingVolume(i); } tree_.CreateBVH(); @@ -667,29 +669,28 @@ void mjCMesh::TryCompile(const mjVFS* vfs) { // get bounding volume void mjCMesh::SetBoundingVolume(int faceid) { - mjCBoundingVolume* node = tree_.GetBoundingVolume(faceid); - node->SetId(faceid); - node->conaffinity = 1; - node->contype = 1; - node->pos = center_ + 3*faceid; - node->quat = NULL; - double face_aamm[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]; - face_aamm[0] = mjMIN(face_aamm[0], vert_[3*vertid+0]); - face_aamm[1] = mjMIN(face_aamm[1], vert_[3*vertid+1]); - face_aamm[2] = mjMIN(face_aamm[2], vert_[3*vertid+2]); - face_aamm[3] = mjMAX(face_aamm[3], vert_[3*vertid+0]); - face_aamm[4] = mjMAX(face_aamm[4], vert_[3*vertid+1]); - face_aamm[5] = mjMAX(face_aamm[5], vert_[3*vertid+2]); + constexpr double kMaxVal = std::numeric_limits::max(); + double face_aamm[6] = {kMaxVal, kMaxVal, kMaxVal, -kMaxVal, -kMaxVal, -kMaxVal}; + + for (int j = 0; j < 3; j++) { + int vertid = face_[3*faceid + j]; + face_aamm[0] = std::min(face_aamm[0], vert_[3*vertid + 0]); + face_aamm[1] = std::min(face_aamm[1], vert_[3*vertid + 1]); + face_aamm[2] = std::min(face_aamm[2], vert_[3*vertid + 2]); + face_aamm[3] = std::max(face_aamm[3], vert_[3*vertid + 0]); + face_aamm[4] = std::max(face_aamm[4], vert_[3*vertid + 1]); + face_aamm[5] = std::max(face_aamm[5], vert_[3*vertid + 2]); } - face_aabb_[6*faceid+0] = .5 * (face_aamm[0] + face_aamm[3]); - face_aabb_[6*faceid+1] = .5 * (face_aamm[1] + face_aamm[4]); - face_aabb_[6*faceid+2] = .5 * (face_aamm[2] + face_aamm[5]); - face_aabb_[6*faceid+3] = .5 * (face_aamm[3] - face_aamm[0]); - face_aabb_[6*faceid+4] = .5 * (face_aamm[4] - face_aamm[1]); - face_aabb_[6*faceid+5] = .5 * (face_aamm[5] - face_aamm[2]); - node->aabb = face_aabb_.data() + 6*faceid; + + face_aabb_.push_back(.5 * (face_aamm[0] + face_aamm[3])); + face_aabb_.push_back(.5 * (face_aamm[1] + face_aamm[4])); + face_aabb_.push_back(.5 * (face_aamm[2] + face_aamm[5])); + face_aabb_.push_back(.5 * (face_aamm[3] - face_aamm[0])); + face_aabb_.push_back(.5 * (face_aamm[4] - face_aamm[1])); + face_aabb_.push_back(.5 * (face_aamm[5] - face_aamm[2])); + + tree_.AddBoundingVolume(faceid, 1, 1, center_ + 3*faceid, nullptr, + &face_aabb_[6*faceid]); } @@ -3565,7 +3566,7 @@ void mjCFlex::Compile(const mjVFS* vfs) { // create flex BVH -void mjCFlex::CreateBVH(void) { +void mjCFlex::CreateBVH() { int nbvh = 0; // allocate element bounding boxes @@ -3587,8 +3588,8 @@ void mjCFlex::CreateBVH(void) { mjuu_copyvec(xmax, vertxpos.data() + 3*edata[0], 3); for (int i=1; i <= dim; i++) { for (int j=0; j<3; j++) { - xmin[j] = mjMIN(xmin[j], vertxpos[3*edata[i]+j]); - xmax[j] = mjMAX(xmax[j], vertxpos[3*edata[i]+j]); + xmin[j] = std::min(xmin[j], vertxpos[3*edata[i]+j]); + xmax[j] = std::max(xmax[j], vertxpos[3*edata[i]+j]); } } @@ -3601,13 +3602,9 @@ void mjCFlex::CreateBVH(void) { elemaabb_[6*e+5] = 0.5*(xmax[2]-xmin[2]) + radius; // add bounding volume for this element - mjCBoundingVolume* bv = tree.GetBoundingVolume(nbvh++); - bv->contype = contype; - bv->conaffinity = conaffinity; - bv->quat = NULL; - bv->SetId(e); - bv->aabb = elemaabb_.data() + 6*e; - bv->pos = bv->aabb; + const double* aabb = elemaabb_.data() + 6*e; + tree.AddBoundingVolume(e, contype, conaffinity, aabb, nullptr, aabb); + nbvh++; } // create hierarchy diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 193b77ef..80f77556 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2218,7 +2218,7 @@ void mjCModel::CopyTree(mjModel* m) { 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->bvh_nodeid[i + bvh_adr] = pb->tree.Nodeidptr(i) ? *(pb->tree.Nodeidptr(i)) : -1; } } bvh_adr += pb->tree.Nbvh(); @@ -2838,7 +2838,7 @@ void mjCModel::CopyObjects(mjModel* m) { 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; + m->bvh_nodeid[j + bvh_adr] = pme->tree().Nodeid(j) > -1 ? pme->tree().Nodeid(j) : -1; } } @@ -2955,7 +2955,7 @@ void mjCModel::CopyObjects(mjModel* m) { 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; + m->bvh_nodeid[i+ bvh_adr] = pfl->tree.Nodeidptr(i) ? *(pfl->tree.Nodeidptr(i)) : -1; } } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index d06a121f..e742c36b 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -38,6 +38,7 @@ #include "cc/array_safety.h" #include "engine/engine_passive.h" #include +#include #include "user/user_api.h" #include "user/user_cache.h" #include "user/user_model.h" @@ -347,12 +348,6 @@ const char* ResolveOrientation(double* quat, bool degree, const char* sequence, //------------------------- class mjCBoundingVolumeHierarchy implementation ------------------------ -// constructor -mjCBoundingVolumeHierarchy::mjCBoundingVolumeHierarchy() { - mjuu_setvec(ipos_, 0, 0, 0); - mjuu_setvec(iquat_, 1, 0, 0, 0); -} - // assign position and orientation void mjCBoundingVolumeHierarchy::Set(double ipos_element[3], double iquat_element[4]) { @@ -369,7 +364,7 @@ void mjCBoundingVolumeHierarchy::AllocateBoundingVolumes(int nleaf) { nodeid_.clear(); level_.clear(); bvleaf_.clear(); - bvleaf_.resize(nleaf); + bvleaf_.reserve(nleaf); } @@ -377,9 +372,21 @@ void mjCBoundingVolumeHierarchy::RemoveInactiveVolumes(int nmax) { bvleaf_.erase(bvleaf_.begin() + nmax, bvleaf_.end()); } +const mjCBoundingVolume* +mjCBoundingVolumeHierarchy::AddBoundingVolume(int id, int contype, int conaffinity, + const double* pos, const double* quat, + const double* aabb) { + bvleaf_.emplace_back(id, contype, conaffinity, pos, quat, aabb); + return &bvleaf_.back(); +} -mjCBoundingVolume* mjCBoundingVolumeHierarchy::GetBoundingVolume(int id) { - return bvleaf_.data() + id; + +const mjCBoundingVolume* +mjCBoundingVolumeHierarchy::AddBoundingVolume(const int* id, int contype, int conaffinity, + const double* pos, const double* quat, + const double* aabb) { + bvleaf_.emplace_back(id, contype, conaffinity, pos, quat, aabb); + return &bvleaf_.back(); } @@ -391,12 +398,12 @@ void mjCBoundingVolumeHierarchy::CreateBVH() { elements.reserve(bvleaf_.size()); double qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]}; for (int i = 0; i < bvleaf_.size(); i++) { - if (bvleaf_[i].conaffinity || bvleaf_[i].contype) { + if (bvleaf_[i].Conaffinity() || bvleaf_[i].Contype()) { BVElement element; element.e = &bvleaf_[i]; - double vert[3] = {element.e->pos[0] - ipos_[0], - element.e->pos[1] - ipos_[1], - element.e->pos[2] - ipos_[2]}; + double vert[3] = {element.e->Pos(0) - ipos_[0], + element.e->Pos(1) - ipos_[1], + element.e->Pos(2) - ipos_[2]}; mjuu_rotVecQuat(element.lpos, vert, qinv); elements.push_back(std::move(element)); } @@ -412,7 +419,8 @@ int mjCBoundingVolumeHierarchy::MakeBVH( if (nelements == 0) { return -1; } - double AAMM[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL}; + constexpr double kMaxVal = std::numeric_limits::max(); + double AAMM[6] = {kMaxVal, kMaxVal, kMaxVal, -kMaxVal, -kMaxVal, -kMaxVal}; // inverse transformation double qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]}; @@ -420,26 +428,26 @@ int mjCBoundingVolumeHierarchy::MakeBVH( // accumulate AAMM over elements for (auto element = elements_begin; element != elements_end; ++element) { // transform element aabb to aamm format - double aamm[6] = {element->e->aabb[0] - element->e->aabb[3], - element->e->aabb[1] - element->e->aabb[4], - element->e->aabb[2] - element->e->aabb[5], - element->e->aabb[0] + element->e->aabb[3], - element->e->aabb[1] + element->e->aabb[4], - element->e->aabb[2] + element->e->aabb[5]}; + double aamm[6] = {element->e->AABB(0) - element->e->AABB(3), + element->e->AABB(1) - element->e->AABB(4), + element->e->AABB(2) - element->e->AABB(5), + element->e->AABB(0) + element->e->AABB(3), + element->e->AABB(1) + element->e->AABB(4), + element->e->AABB(2) + element->e->AABB(5)}; // update node AAMM - for (int v=0; v<8; v++) { + for (int v=0; v < 8; v++) { double vert[3], box[3]; vert[0] = (v&1 ? aamm[3] : aamm[0]); vert[1] = (v&2 ? aamm[4] : aamm[1]); vert[2] = (v&4 ? aamm[5] : aamm[2]); // rotate to the body inertial frame if specified - if (element->e->quat) { - mjuu_rotVecQuat(box, vert, element->e->quat); - box[0] += element->e->pos[0] - ipos_[0]; - box[1] += element->e->pos[1] - ipos_[1]; - box[2] += element->e->pos[2] - ipos_[2]; + if (element->e->Quat()) { + mjuu_rotVecQuat(box, vert, element->e->Quat()); + box[0] += element->e->Pos(0) - ipos_[0]; + box[1] += element->e->Pos(1) - ipos_[1]; + box[2] += element->e->Pos(2) - ipos_[2]; mjuu_rotVecQuat(vert, box, qinv); } @@ -453,10 +461,10 @@ int mjCBoundingVolumeHierarchy::MakeBVH( } // inflate flat AABBs - for (int i=0; i<3; i++) { - if (std::abs(AAMM[i]-AAMM[i+3])e->GetId(); + if (nelements == 1) { + child_[2*index + 0] = -1; + child_[2*index + 1] = -1; + nodeid_[index] = *elements_begin->e->Id(); + nodeidptr_[index] = (int*)elements_begin->e->Id(); return index; } // find longest axis, by a margin of at least mjEPS, default to 0 int axis = 0; - double edges[3] = { AAMM[3]-AAMM[0], AAMM[4]-AAMM[1], AAMM[5]-AAMM[2] }; + double edges[3] = {AAMM[3] - AAMM[0], AAMM[4] - AAMM[1], AAMM[5] - AAMM[2]}; if (edges[1] >= edges[0] + mjEPS) axis = 1; if (edges[2] >= edges[axis] + mjEPS) axis = 2; // find median along the axis + auto compare = [&](const BVElement& e1, const BVElement& e2) { + if (std::abs(e1.lpos[axis] - e2.lpos[axis]) > mjEPS) { + return e1.lpos[axis] < e2.lpos[axis]; + } + // comparing pointers gives a stable sort, because they both come from the same array + return e1.e < e2.e; + }; + // note: nth_element performs a partial sort of elements - BVElementCompare compare; - compare.axis = axis; int m = nelements / 2; std::nth_element(elements_begin, elements_begin + m, elements_end, compare); // 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); } - if (lev>mjMAXTREEDEPTH) { + if (lev > mjMAXTREEDEPTH) { mju_warning("max tree depth exceeded in body=%s", name_.c_str()); } @@ -1595,8 +1610,9 @@ void mjCBody::ComputeBVH() { tree.Set(ipos, iquat); tree.AllocateBoundingVolumes(geoms.size()); - for (int i=0; iSetBoundingVolume(tree.GetBoundingVolume(i)); + for (const mjCGeom* geom : geoms) { + tree.AddBoundingVolume(&geom->id, geom->contype, geom->conaffinity, + geom->pos, geom->quat, geom->aabb); } tree.CreateBVH(); } @@ -2463,17 +2479,6 @@ double mjCGeom::GetVolume() const { -void mjCGeom::SetBoundingVolume(mjCBoundingVolume* bv) const { - bv->SetId(&id); - bv->contype = contype; - bv->conaffinity = conaffinity; - bv->aabb = aabb; - bv->pos = pos; - bv->quat = quat; -} - - - // set geom diagonal inertia given density void mjCGeom::SetInertia(void) { // get from mesh diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 79e259ed..9770bb27 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -16,9 +16,9 @@ #define MUJOCO_SRC_USER_USER_OBJECTS_H_ #include +#include #include #include -#include #include #include #include @@ -27,6 +27,7 @@ #include #include +#include #include "user/user_cache.h" #include "user/user_util.h" #include @@ -96,21 +97,59 @@ const char* ResolveOrientation(double* quat, // set frame quat // bounding volume class mjCBoundingVolume { public: - mjCBoundingVolume() { id_ = nullptr; }; + mjCBoundingVolume(int id, int contype, int conaffinity, const double pos[3], + const double quat[4], const double aabb[6]) : contype_(contype), + conaffinity_(conaffinity), idval_(id) { + std::copy(pos, pos + 3, pos_.begin()); + std::copy(aabb, aabb + 6, aabb_.begin()); + quat_set_ = quat != nullptr; + if (quat_set_) { + std::copy(quat, quat + 4, quat_.begin()); + } + } - int contype; // contact type - int conaffinity; // contact affinity - const double* aabb; // axis-aligned bounding box (center, size) - const double* pos; // position (set by user or Compile1) - const double* quat; // orientation (set by user or Compile1) + mjCBoundingVolume(const int* id, int contype, int conaffinity, const double pos[3], + const double quat[4], const double aabb[6]) : contype_(contype), + conaffinity_(conaffinity), id_(id) { + std::copy(pos, pos + 3, pos_.begin()); + std::copy(aabb, aabb + 6, aabb_.begin()); + quat_set_ = quat != nullptr; + if (quat_set_) { + std::copy(quat, quat + 4, quat_.begin()); + } + } - const int* GetId() const { if (id_) return id_; else return &idval_; } + int Contype() const { return contype_; } + int Conaffinity() const { return conaffinity_; } + const double* AABB() const { return aabb_.data(); } + double AABB(int i) const { return aabb_[i]; } + const double* Pos() const { return pos_.data(); } + double Pos(int i) const { return pos_[i]; } + const double* Quat() const { return quat_set_ ? quat_.data() : nullptr; } + const int* Id() const { return id_ ? id_ : &idval_; } + + void SetContype(int val) { contype_ = val; } + void SetConaffinity(int val) { conaffinity_ = val; } + void SetAABB(const double* aabb) { std::copy(aabb, aabb + 6, aabb_.begin()); } + void SetPos(const double* pos) { std::copy(pos, pos + 3, pos_.begin()); } + void SetQuat(const double* quat) { + quat_set_ = true; + std::copy(quat, quat + 4, quat_.begin()); + } 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 + int contype_; // contact type + int conaffinity_; // contact affinity + std::array aabb_; // axis-aligned bounding box (center, size) + std::array pos_; // position (set by user or Compiler) + std::array quat_; // orientation (set by user or Compiler) + bool quat_set_; // boolean flag is quat_ has been set + int idval_; // local id copy for nodes not storing their id's (e.g. faces) + + // pointer to object id + const int* id_ = nullptr; }; @@ -118,33 +157,38 @@ class mjCBoundingVolume { struct mjCBoundingVolumeHierarchy_ { 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 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 nodeidptr_; // ptr to 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]; - double iquat_[4]; + double ipos_[3] = {0, 0, 0}; + double iquat_[4] = {1, 0, 0, 0}; }; class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ { public: - mjCBoundingVolumeHierarchy(); - // make bounding volume hierarchy void CreateBVH(); void Set(double ipos_element[3], double iquat_element[4]); void AllocateBoundingVolumes(int nleaf); void RemoveInactiveVolumes(int nmax); - mjCBoundingVolume* GetBoundingVolume(int id); + const mjCBoundingVolume* + AddBoundingVolume(int id, int contype, int conaffinity, const double pos[3], + const double quat[4], const double aabb[6]); + const mjCBoundingVolume* + AddBoundingVolume(const int* id, int contype, int conaffinity, const double pos[3], + const double quat[4], const double aabb[6]); // 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& Nodeid() const { return nodeid_; } + int Nodeid(int id) const { return nodeid_[id]; } + const int* Nodeidptr(int id) const { return nodeidptr_[id]; } const std::vector& Level() const { return level_; } private: @@ -155,18 +199,6 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ { double lpos[3]; }; - struct BVElementCompare { - int axis = 0; - - bool operator()(const BVElement& e1, const BVElement& e2) const { - if (std::abs(e1.lpos[axis] - e2.lpos[axis]) > mjEPS) { - return e1.lpos[axis] < e2.lpos[axis]; - } - // comparing pointers gives a stable sort, because they both come from the same array - return e1.e < e2.e; - } - }; - int MakeBVH(std::vector::iterator elements_begin, std::vector::iterator elements_end, int lev = 0); }; @@ -578,9 +610,6 @@ class mjCGeom : public mjCGeom_, private mjsGeom { // Compute the kappa coefs of the added inertia due to the surrounding fluid. double GetAddedMassKappa(double dx, double dy, double dz); - // sets properties of a bounding volume - void SetBoundingVolume(mjCBoundingVolume* bv) const; - // used by mjXWriter and mjCModel const std::vector& get_userdata() const { return userdata_; } const std::string& get_hfieldname() const { return spec_hfieldname_; }