chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 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();
}
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) {
bvh_.push_back(bv);
void mjCBoundingVolumeHierarchy::AllocateBoundingVolumes(int nbvh) {
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
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
int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements, int lev) {
int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<const mjCBoundingVolume*>& elements, int lev) {
if (elements.empty()) {
return -1;
}
@@ -309,17 +323,17 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
// accumulate AAMM over elements
for (int i=0; i<nelements; i++) {
// skip visual objects
if (elements[i].conaffinity==0 && elements[i].contype==0) {
if (elements[i]->conaffinity==0 && elements[i]->contype==0) {
continue;
}
// transform element aabb to aamm format
mjtNum aamm[6] = {elements[i].aabb[0] - elements[i].aabb[3],
elements[i].aabb[1] - elements[i].aabb[4],
elements[i].aabb[2] - elements[i].aabb[5],
elements[i].aabb[0] + elements[i].aabb[3],
elements[i].aabb[1] + elements[i].aabb[4],
elements[i].aabb[2] + elements[i].aabb[5]};
mjtNum aamm[6] = {elements[i]->aabb[0] - elements[i]->aabb[3],
elements[i]->aabb[1] - elements[i]->aabb[4],
elements[i]->aabb[2] - elements[i]->aabb[5],
elements[i]->aabb[0] + elements[i]->aabb[3],
elements[i]->aabb[1] + elements[i]->aabb[4],
elements[i]->aabb[2] + elements[i]->aabb[5]};
// update node AAMM
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]);
// rotate to the body inertial frame if specified
if (elements[i].quat) {
mju_rotVecQuat(box, vert, elements[i].quat);
box[0] += elements[i].pos[0] - ipos_[0];
box[1] += elements[i].pos[1] - ipos_[1];
box[2] += elements[i].pos[2] - ipos_[2];
if (elements[i]->quat) {
mju_rotVecQuat(box, vert, elements[i]->quat);
box[0] += elements[i]->pos[0] - ipos_[0];
box[1] += elements[i]->pos[1] - ipos_[1];
box[2] += elements[i]->pos[2] - ipos_[2];
mju_rotVecQuat(vert, box, qinv);
}
@@ -374,7 +388,7 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
for (int i=0; i<2; i++) {
child[2*index+i] = -1;
}
nodeid[index] = elements[0].id;
nodeid[index] = elements[0]->id;
return index;
}
@@ -388,9 +402,9 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
for (int i=0; i<nelements; i++) {
// get position in the body inertial frame
mjtNum vert[3] = {elements[i].pos[0] - ipos_[0],
elements[i].pos[1] - ipos_[1],
elements[i].pos[2] - ipos_[2]};
mjtNum vert[3] = {elements[i]->pos[0] - ipos_[0],
elements[i]->pos[1] - ipos_[1],
elements[i]->pos[2] - ipos_[2]};
mjtNum lpos[3];
mju_rotVecQuat(lpos, vert, qinv);
pos[i] = lpos[axis];
@@ -401,20 +415,20 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
mjtNum threshold = pos[m];
// split using median
std::vector<mjCBoundingVolume> left;
std::vector<mjCBoundingVolume> right;
std::vector<const mjCBoundingVolume*> left;
std::vector<const mjCBoundingVolume*> right;
int skipped = 0;
for (int i=0; i<nelements; i++) {
// get position in the body inertial frame
mjtNum vert[3] = {elements[i].pos[0] - ipos_[0],
elements[i].pos[1] - ipos_[1],
elements[i].pos[2] - ipos_[2]};
mjtNum vert[3] = {elements[i]->pos[0] - ipos_[0],
elements[i]->pos[1] - ipos_[1],
elements[i]->pos[2] - ipos_[2]};
mjtNum lpos[3];
mju_rotVecQuat(lpos, vert, qinv);
// skip visual objects
if (elements[i].conaffinity==0 && elements[i].contype==0) {
if (elements[i]->conaffinity==0 && elements[i]->contype==0) {
skipped++;
continue;
}
@@ -1008,8 +1022,9 @@ void mjCBody::Compile(void) {
// compute bounding volume hierarchy
if (!geoms.empty()) {
tree.Set(ipos, iquat);
tree.AllocateBoundingVolumes(geoms.size());
for (int i=0; i<geoms.size(); i++) {
tree.AddBoundingVolume(geoms[i]->GetBoundingVolume());
geoms[i]->SetBoundingVolume(tree.GetBoundingVolume(i));
}
tree.CreateBVH();
}
@@ -1396,15 +1411,13 @@ double mjCGeom::GetVolume(void) {
mjCBoundingVolume mjCGeom::GetBoundingVolume() const {
mjCBoundingVolume bv;
bv.id = id;
bv.contype = contype;
bv.conaffinity = conaffinity;
bv.aabb = aabb;
bv.pos = pos;
bv.quat = quat;
return bv;
void mjCGeom::SetBoundingVolume(mjCBoundingVolume* bv) const {
bv->id = id;
bv->contype = contype;
bv->conaffinity = conaffinity;
bv->aabb = aabb;
bv->pos = pos;
bv->quat = quat;
}
src/user/user_objects.h
+9 -6
View File
@@ -150,10 +150,12 @@ class mjCBoundingVolumeHierarchy {
// make bounding volume hierarchy
void CreateBVH(void);
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:
int MakeBVH(std::vector<mjCBoundingVolume>& elements, int lev = 0);
int MakeBVH(std::vector<const mjCBoundingVolume*>& elements, int lev = 0);
std::vector<mjCBoundingVolume> bvh_;
std::string name_;
@@ -378,8 +380,8 @@ class mjCGeom : public mjCBase {
// Compute the kappa coefs of the added inertia due to the surrounding fluid.
double GetAddedMassKappa(double dx, double dy, double dz);
// returns a bounding volume
mjCBoundingVolume GetBoundingVolume() const;
// sets properties of a bounding volume
void SetBoundingVolume(mjCBoundingVolume* bv) const;
// variables set by user and copied into mjModel
mjtGeom type; // geom type
@@ -578,6 +580,7 @@ class mjCFlex: public mjCBase {
std::vector<std::string> vertbody; // vertex body names
std::vector<mjtNum> vert; // vertex positions
std::vector<mjtNum> elemaabb; // element bounding volume
std::vector<int> elem; // element vertex ids
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 CopyGraph(int* arr) const; // copy graph data into array
// returns a bounding volume given a face
mjCBoundingVolume GetBoundingVolume(int faceid);
// sets properties of a bounding volume given a face id
void SetBoundingVolume(int faceid);
private:
std::string content_type_; // content type of file