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
if (tree_.bvh.empty()) {
face_aabb_.assign(6*nface_, 0);
tree_.AllocateBoundingVolumes(nface_);
for (int i=0; i<nface_; i++) {
tree_.AddBoundingVolume(GetBoundingVolume(i));
SetBoundingVolume(i);
}
tree_.CreateBVH();
}
@@ -597,13 +598,13 @@ void mjCMesh::Compile(const mjVFS* vfs) {
// get bounding volume
mjCBoundingVolume mjCMesh::GetBoundingVolume(int faceid) {
mjCBoundingVolume node;
node.id = faceid;
node.conaffinity = 1;
node.contype = 1;
node.pos = center_ + 3*faceid;
node.quat = NULL;
void mjCMesh::SetBoundingVolume(int faceid) {
mjCBoundingVolume* node = tree_.GetBoundingVolume(faceid);
node->id = faceid;
node->conaffinity = 1;
node->contype = 1;
node->pos = center_ + 3*faceid;
node->quat = NULL;
mjtNum 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];
@@ -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+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;
return node;
node->aabb = face_aabb_.data() + 6*faceid;
}
@@ -2428,14 +2428,11 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// create flex BVH
void mjCFlex::CreateBVH(void) {
// init bounding volume object
mjCBoundingVolume bv;
bv.contype = contype;
bv.conaffinity = conaffinity;
bv.quat = NULL;
int nbvh = 0;
// allocate element bounding boxes
vector<mjtNum> elemaabb(6*nelem);
elemaabb.resize(6*nelem);
tree.AllocateBoundingVolumes(nelem);
// construct element bounding boxes, add to hierarchy
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;
// add bounding volume for this element
bv.id = e;
bv.aabb = elemaabb.data() + 6*e;
bv.pos = bv.aabb;
tree.AddBoundingVolume(bv);
mjCBoundingVolume* bv = tree.GetBoundingVolume(nbvh++);
bv->contype = contype;
bv->conaffinity = conaffinity;
bv->quat = NULL;
bv->id = e;
bv->aabb = elemaabb.data() + 6*e;
bv->pos = bv->aabb;
}
// create hierarchy
tree.RemoveInactiveVolumes(nbvh);
tree.CreateBVH();
}