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

Preallocate mjCBoundingVolumeHierarchy and avoid copies during MakeBVH.

Browse Source 
PiperOrigin-RevId: 598579741
Change-Id: Iac154affa6ee22994c571f2ec00ac602bfe7f4d6
This commit is contained in:
Alessio Quaglino
2024-01-15 04:47:59 -08:00
committed by Copybara-Service
parent 5d46c39529
commit cab1c6bb39
3 changed files with 80 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_mesh.cc
+21 -20
View File
@@ -587,8 +587,9 @@ 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_);
for (int i=0; i<nface_; i++) { for (int i=0; i<nface_; i++) {
tree_.AddBoundingVolume(GetBoundingVolume(i)); SetBoundingVolume(i);
} }
tree_.CreateBVH(); tree_.CreateBVH();
} }
@@ -597,13 +598,13 @@ void mjCMesh::Compile(const mjVFS* vfs) {
// get bounding volume // get bounding volume
mjCBoundingVolume mjCMesh::GetBoundingVolume(int faceid) { void mjCMesh::SetBoundingVolume(int faceid) {
mjCBoundingVolume node; mjCBoundingVolume* node = tree_.GetBoundingVolume(faceid);
node.id = faceid; node->id = faceid;
node.conaffinity = 1; node->conaffinity = 1;
node.contype = 1; node->contype = 1;
node.pos = center_ + 3*faceid; node->pos = center_ + 3*faceid;
node.quat = NULL; node->quat = NULL;
mjtNum face_aamm[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10}; mjtNum face_aamm[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10};
for (int j=0; j<3; j++) { for (int j=0; j<3; j++) {
int vertid = face_[3*faceid+j]; int vertid = face_[3*faceid+j];
@@ -620,8 +621,7 @@ mjCBoundingVolume mjCMesh::GetBoundingVolume(int faceid) {
face_aabb_[6*faceid+3] = .5 * (face_aamm[3] - face_aamm[0]); 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+4] = .5 * (face_aamm[4] - face_aamm[1]);
face_aabb_[6*faceid+5] = .5 * (face_aamm[5] - face_aamm[2]); face_aabb_[6*faceid+5] = .5 * (face_aamm[5] - face_aamm[2]);
node.aabb = face_aabb_.data() + 6*faceid; node->aabb = face_aabb_.data() + 6*faceid;
return node;
} }
@@ -2428,14 +2428,11 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// create flex BVH // create flex BVH
void mjCFlex::CreateBVH(void) { void mjCFlex::CreateBVH(void) {
// init bounding volume object int nbvh = 0;
mjCBoundingVolume bv;
bv.contype = contype;
bv.conaffinity = conaffinity;
bv.quat = NULL;
// allocate element bounding boxes // allocate element bounding boxes
vector<mjtNum> elemaabb(6*nelem); elemaabb.resize(6*nelem);
tree.AllocateBoundingVolumes(nelem);
// construct element bounding boxes, add to hierarchy // construct element bounding boxes, add to hierarchy
for (int e=0; e<nelem; e++) { for (int e=0; e<nelem; e++) {
@@ -2466,13 +2463,17 @@ void mjCFlex::CreateBVH(void) {
elemaabb[6*e+5] = 0.5*(xmax[2]-xmin[2]) + radius; elemaabb[6*e+5] = 0.5*(xmax[2]-xmin[2]) + radius;
// add bounding volume for this element // add bounding volume for this element
bv.id = e; mjCBoundingVolume* bv = tree.GetBoundingVolume(nbvh++);
bv.aabb = elemaabb.data() + 6*e; bv->contype = contype;
bv.pos = bv.aabb; bv->conaffinity = conaffinity;
tree.AddBoundingVolume(bv); bv->quat = NULL;
bv->id = e;
bv->aabb = elemaabb.data() + 6*e;
bv->pos = bv->aabb;
} }
// create hierarchy // create hierarchy
tree.RemoveInactiveVolumes(nbvh);
tree.CreateBVH(); tree.CreateBVH();
} }
src/user/user_objects.cc
+50 -37
View File
@@ -282,20 +282,34 @@ void mjCBoundingVolumeHierarchy::Set(mjtNum ipos_element[3], mjtNum iquat_elemen
} }
// add geom to bvh
void mjCBoundingVolumeHierarchy::AddBoundingVolume(const mjCBoundingVolume& bv) { void mjCBoundingVolumeHierarchy::AllocateBoundingVolumes(int nbvh) {
bvh_.push_back(bv); bvh_.resize(nbvh);
}
void mjCBoundingVolumeHierarchy::RemoveInactiveVolumes(int nmax) {
bvh_.erase(bvh_.begin() + nmax, bvh_.end());
}
mjCBoundingVolume* mjCBoundingVolumeHierarchy::GetBoundingVolume(int id) {
return bvh_.data() + id;
} }
// create bounding volume hierarchy // create bounding volume hierarchy
void mjCBoundingVolumeHierarchy::CreateBVH() { void mjCBoundingVolumeHierarchy::CreateBVH() {
MakeBVH(bvh_); std::vector<const mjCBoundingVolume*> elements(bvh_.size());
for (int i=0; i<bvh_.size(); i++) {
elements[i] = bvh_.data() + i;
}
MakeBVH(elements);
} }
// compute bounding volume hierarchy // compute bounding volume hierarchy
int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements, int lev) { int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<const mjCBoundingVolume*>& elements, int lev) {
if (elements.empty()) { if (elements.empty()) {
return -1; return -1;
} }
@@ -309,17 +323,17 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
// accumulate AAMM over elements // accumulate AAMM over elements
for (int i=0; i<nelements; i++) { for (int i=0; i<nelements; i++) {
// skip visual objects // skip visual objects
if (elements[i].conaffinity==0 && elements[i].contype==0) { if (elements[i]->conaffinity==0 && elements[i]->contype==0) {
continue; continue;
} }
// transform element aabb to aamm format // transform element aabb to aamm format
mjtNum aamm[6] = {elements[i].aabb[0] - elements[i].aabb[3], mjtNum aamm[6] = {elements[i]->aabb[0] - elements[i]->aabb[3],
elements[i].aabb[1] - elements[i].aabb[4], elements[i]->aabb[1] - elements[i]->aabb[4],
elements[i].aabb[2] - elements[i].aabb[5], elements[i]->aabb[2] - elements[i]->aabb[5],
elements[i].aabb[0] + elements[i].aabb[3], elements[i]->aabb[0] + elements[i]->aabb[3],
elements[i].aabb[1] + elements[i].aabb[4], elements[i]->aabb[1] + elements[i]->aabb[4],
elements[i].aabb[2] + elements[i].aabb[5]}; elements[i]->aabb[2] + elements[i]->aabb[5]};
// update node AAMM // update node AAMM
for (int v=0; v<8; v++) { for (int v=0; v<8; v++) {
@@ -329,11 +343,11 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
vert[2] = (v&4 ? aamm[5] : aamm[2]); vert[2] = (v&4 ? aamm[5] : aamm[2]);
// rotate to the body inertial frame if specified // rotate to the body inertial frame if specified
if (elements[i].quat) { if (elements[i]->quat) {
mju_rotVecQuat(box, vert, elements[i].quat); mju_rotVecQuat(box, vert, elements[i]->quat);
box[0] += elements[i].pos[0] - ipos_[0]; box[0] += elements[i]->pos[0] - ipos_[0];
box[1] += elements[i].pos[1] - ipos_[1]; box[1] += elements[i]->pos[1] - ipos_[1];
box[2] += elements[i].pos[2] - ipos_[2]; box[2] += elements[i]->pos[2] - ipos_[2];
mju_rotVecQuat(vert, box, qinv); mju_rotVecQuat(vert, box, qinv);
} }
@@ -374,7 +388,7 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
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] = elements[0].id; nodeid[index] = elements[0]->id;
return index; return index;
} }
@@ -388,9 +402,9 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
for (int i=0; i<nelements; i++) { for (int i=0; i<nelements; i++) {
// get position in the body inertial frame // get position in the body inertial frame
mjtNum vert[3] = {elements[i].pos[0] - ipos_[0], mjtNum vert[3] = {elements[i]->pos[0] - ipos_[0],
elements[i].pos[1] - ipos_[1], elements[i]->pos[1] - ipos_[1],
elements[i].pos[2] - ipos_[2]}; elements[i]->pos[2] - ipos_[2]};
mjtNum lpos[3]; mjtNum lpos[3];
mju_rotVecQuat(lpos, vert, qinv); mju_rotVecQuat(lpos, vert, qinv);
pos[i] = lpos[axis]; pos[i] = lpos[axis];
@@ -401,20 +415,20 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
mjtNum threshold = pos[m]; mjtNum threshold = pos[m];
// split using median // split using median
std::vector<mjCBoundingVolume> left; std::vector<const mjCBoundingVolume*> left;
std::vector<mjCBoundingVolume> right; std::vector<const mjCBoundingVolume*> right;
int skipped = 0; int skipped = 0;
for (int i=0; i<nelements; i++) { for (int i=0; i<nelements; i++) {
// get position in the body inertial frame // get position in the body inertial frame
mjtNum vert[3] = {elements[i].pos[0] - ipos_[0], mjtNum vert[3] = {elements[i]->pos[0] - ipos_[0],
elements[i].pos[1] - ipos_[1], elements[i]->pos[1] - ipos_[1],
elements[i].pos[2] - ipos_[2]}; elements[i]->pos[2] - ipos_[2]};
mjtNum lpos[3]; mjtNum lpos[3];
mju_rotVecQuat(lpos, vert, qinv); mju_rotVecQuat(lpos, vert, qinv);
// skip visual objects // skip visual objects
if (elements[i].conaffinity==0 && elements[i].contype==0) { if (elements[i]->conaffinity==0 && elements[i]->contype==0) {
skipped++; skipped++;
continue; continue;
} }
@@ -1008,8 +1022,9 @@ void mjCBody::Compile(void) {
// compute bounding volume hierarchy // compute bounding volume hierarchy
if (!geoms.empty()) { if (!geoms.empty()) {
tree.Set(ipos, iquat); tree.Set(ipos, iquat);
tree.AllocateBoundingVolumes(geoms.size());
for (int i=0; i<geoms.size(); i++) { for (int i=0; i<geoms.size(); i++) {
tree.AddBoundingVolume(geoms[i]->GetBoundingVolume()); geoms[i]->SetBoundingVolume(tree.GetBoundingVolume(i));
} }
tree.CreateBVH(); tree.CreateBVH();
} }
@@ -1396,15 +1411,13 @@ double mjCGeom::GetVolume(void) {
mjCBoundingVolume mjCGeom::GetBoundingVolume() const { void mjCGeom::SetBoundingVolume(mjCBoundingVolume* bv) const {
mjCBoundingVolume bv; bv->id = id;
bv.id = id; bv->contype = contype;
bv.contype = contype; bv->conaffinity = conaffinity;
bv.conaffinity = conaffinity; bv->aabb = aabb;
bv.aabb = aabb; bv->pos = pos;
bv.pos = pos; bv->quat = quat;
bv.quat = quat;
return bv;
} }
src/user/user_objects.h
+9 -6
View File
@@ -150,10 +150,12 @@ class mjCBoundingVolumeHierarchy {
// make bounding volume hierarchy // make bounding volume hierarchy
void CreateBVH(void); void CreateBVH(void);
void Set(mjtNum ipos_element[3], mjtNum iquat_element[4]); void Set(mjtNum ipos_element[3], mjtNum iquat_element[4]);
void AddBoundingVolume(const mjCBoundingVolume& bv); void AllocateBoundingVolumes(int nbvh);
void RemoveInactiveVolumes(int nmax);
mjCBoundingVolume* GetBoundingVolume(int id);
private: private:
int MakeBVH(std::vector<mjCBoundingVolume>& elements, int lev = 0); int MakeBVH(std::vector<const mjCBoundingVolume*>& elements, int lev = 0);
std::vector<mjCBoundingVolume> bvh_; std::vector<mjCBoundingVolume> bvh_;
std::string name_; std::string name_;
@@ -378,8 +380,8 @@ class mjCGeom : public mjCBase {
// Compute the kappa coefs of the added inertia due to the surrounding fluid. // Compute the kappa coefs of the added inertia due to the surrounding fluid.
double GetAddedMassKappa(double dx, double dy, double dz); double GetAddedMassKappa(double dx, double dy, double dz);
// returns a bounding volume // sets properties of a bounding volume
mjCBoundingVolume GetBoundingVolume() const; void SetBoundingVolume(mjCBoundingVolume* bv) const;
// variables set by user and copied into mjModel // variables set by user and copied into mjModel
mjtGeom type; // geom type mjtGeom type; // geom type
@@ -578,6 +580,7 @@ class mjCFlex: public mjCBase {
std::vector<std::string> vertbody; // vertex body names std::vector<std::string> vertbody; // vertex body names
std::vector<mjtNum> vert; // vertex positions std::vector<mjtNum> vert; // vertex positions
std::vector<mjtNum> elemaabb; // element bounding volume
std::vector<int> elem; // element vertex ids std::vector<int> elem; // element vertex ids
std::vector<float> texcoord; // vertex texture coordinates std::vector<float> texcoord; // vertex texture coordinates
@@ -679,8 +682,8 @@ class mjCMesh: public mjCBase {
void CopyTexcoord(float* arr) const; // copy texcoord data into array void CopyTexcoord(float* arr) const; // copy texcoord data into array
void CopyGraph(int* arr) const; // copy graph data into array void CopyGraph(int* arr) const; // copy graph data into array
// returns a bounding volume given a face // sets properties of a bounding volume given a face id
mjCBoundingVolume GetBoundingVolume(int faceid); void SetBoundingVolume(int faceid);
private: private:
std::string content_type_; // content type of file std::string content_type_; // content type of file