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

Add mjCBoundingVolume class to represent an element of mjCBoundingVolumeHierarchy (for example an mjCGeom).

Browse Source 
PiperOrigin-RevId: 527906004
Change-Id: Ibc3bdac0ec6d85c7fd56fd2057f5a63970e8b1d4
This commit is contained in:
Alessio Quaglino
2023-04-28 09:29:15 -07:00
committed by Copybara-Service
parent c50177d301
commit 15e77b13dd
2 changed files with 81 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_objects.cc
+52 -24
View File
@@ -262,7 +262,7 @@ const char* mjCAlternative::Set(double* quat, double* inertia,
//------------------------- class mjCTree implementation ------------------------------------------- //------------------------- class mjCBoundingVolumeHierarchy implementation ------------------------
// constructor // constructor
mjCBoundingVolumeHierarchy::mjCBoundingVolumeHierarchy() { mjCBoundingVolumeHierarchy::mjCBoundingVolumeHierarchy() {
@@ -279,8 +279,20 @@ void mjCBoundingVolumeHierarchy::Set(mjtNum ipos_element[3], mjtNum iquat_elemen
} }
// add geom to bvh
void mjCBoundingVolumeHierarchy::AddBundingVolume(const mjCBoundingVolume& bv) {
bvh_.push_back(bv);
}
// create bounding volume hierarchy
void mjCBoundingVolumeHierarchy::CreateBVH() {
MakeBVH(bvh_);
}
// compute bounding volume hierarchy // compute bounding volume hierarchy
int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCGeom *>& elements, int lev) { int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements, int lev) {
int nelements = elements.size(); int nelements = elements.size();
mjtNum AABB[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL}; mjtNum AABB[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
@@ -289,17 +301,17 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCGeom *>& elements, int le
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 aabb representation // transform aabb representation
mjtNum aabb[6] = {elements[i]->aabb[0] - elements[i]->aabb[3], mjtNum aabb[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 AABB // update node AABB
for (int v=0; v<8; v++) { for (int v=0; v<8; v++) {
@@ -309,10 +321,10 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCGeom *>& elements, int le
vert[2] = (v&4 ? aabb[5] : aabb[2]); vert[2] = (v&4 ? aabb[5] : aabb[2]);
// rotate to the body inertial frame // rotate to the body inertial frame
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);
AABB[0] = mjMIN(AABB[0], vert[0]); AABB[0] = mjMIN(AABB[0], vert[0]);
AABB[1] = mjMIN(AABB[1], vert[1]); AABB[1] = mjMIN(AABB[1], vert[1]);
@@ -349,7 +361,7 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCGeom *>& elements, int le
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;
} }
@@ -363,9 +375,9 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCGeom *>& elements, int le
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];
@@ -376,20 +388,20 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCGeom *>& elements, int le
mjtNum threshold = pos[m]; mjtNum threshold = pos[m];
// split using median // split using median
std::vector<mjCGeom *> left; std::vector<mjCBoundingVolume> left;
std::vector<mjCGeom *> right; std::vector<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;
} }
@@ -949,7 +961,10 @@ 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.MakeBVH(geoms); for (int i=0; i<geoms.size(); i++) {
tree.AddBundingVolume(geoms[i]->GetBoundingVolume());
}
tree.CreateBVH();
} }
// compile all joints, count dofs // compile all joints, count dofs
@@ -1268,6 +1283,19 @@ 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;
}
// set geom diagonal inertia given density // set geom diagonal inertia given density
void mjCGeom::SetInertia(void) { void mjCGeom::SetInertia(void) {
double height; double height;
src/user/user_objects.h
+29 -1
View File
@@ -114,6 +114,26 @@ class mjCAlternative {
}; };
//------------------------- class mjCBoundingVolumeHierarchy ---------------------------------------
// bounding volume
class mjCBoundingVolume {
public:
mjCBoundingVolume() = default;
int id; // object id
int contype; // contact type
int conaffinity; // contact affinity
const mjtNum* aabb; // half-sizes of axis-aligned bounding box
const mjtNum* pos; // position (set by user or Compile1)
const mjtNum* quat; // orientation (set by user or Compile1)
};
// bounding volume hierarchy // bounding volume hierarchy
class mjCBoundingVolumeHierarchy { class mjCBoundingVolumeHierarchy {
public: public:
@@ -125,10 +145,15 @@ class mjCBoundingVolumeHierarchy {
std::vector<int> nodeid; // id of the geom contained by the node (nbvh x 1) std::vector<int> nodeid; // id of the geom contained by the node (nbvh x 1)
std::vector<int> level; // levels of each node (nbvh x 1) std::vector<int> level; // levels of each node (nbvh x 1)
int MakeBVH(std::vector<mjCGeom *>&, int lev = 0); // make bounding volume hierarchy // make bounding volume hierarchy
void CreateBVH();
void Set(mjtNum ipos_element[3], mjtNum iquat_element[4]); void Set(mjtNum ipos_element[3], mjtNum iquat_element[4]);
void AddBundingVolume(const mjCBoundingVolume& bv);
private: private:
int MakeBVH(std::vector<mjCBoundingVolume>& elements, int lev = 0);
std::vector<mjCBoundingVolume> bvh_;
std::string name_; std::string name_;
double ipos_[3]; double ipos_[3];
double iquat_[4]; double iquat_[4];
@@ -320,6 +345,9 @@ 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
mjCBoundingVolume GetBoundingVolume() 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
int contype; // contact type int contype; // contact type