Add mjCBoundingVolume class to represent an element of mjCBoundingVolumeHierarchy (for example an mjCGeom).
PiperOrigin-RevId: 527906004 Change-Id: Ibc3bdac0ec6d85c7fd56fd2057f5a63970e8b1d4
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
c50177d301
commit
15e77b13dd
+52
-24
@@ -262,7 +262,7 @@ const char* mjCAlternative::Set(double* quat, double* inertia,
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCTree implementation -------------------------------------------
|
||||
//------------------------- class mjCBoundingVolumeHierarchy implementation ------------------------
|
||||
|
||||
// constructor
|
||||
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
|
||||
int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCGeom *>& elements, int lev) {
|
||||
int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements, int lev) {
|
||||
int nelements = elements.size();
|
||||
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++) {
|
||||
// skip visual objects
|
||||
if (elements[i]->conaffinity==0 && elements[i]->contype==0) {
|
||||
if (elements[i].conaffinity==0 && elements[i].contype==0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// transform aabb representation
|
||||
mjtNum aabb[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 aabb[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 AABB
|
||||
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]);
|
||||
|
||||
// rotate to the body inertial frame
|
||||
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(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);
|
||||
AABB[0] = mjMIN(AABB[0], vert[0]);
|
||||
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++) {
|
||||
child[2*index+i] = -1;
|
||||
}
|
||||
nodeid[index] = elements[0]->id;
|
||||
nodeid[index] = elements[0].id;
|
||||
return index;
|
||||
}
|
||||
|
||||
@@ -363,9 +375,9 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCGeom *>& elements, int le
|
||||
|
||||
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];
|
||||
@@ -376,20 +388,20 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCGeom *>& elements, int le
|
||||
mjtNum threshold = pos[m];
|
||||
|
||||
// split using median
|
||||
std::vector<mjCGeom *> left;
|
||||
std::vector<mjCGeom *> right;
|
||||
std::vector<mjCBoundingVolume> left;
|
||||
std::vector<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;
|
||||
}
|
||||
@@ -949,7 +961,10 @@ void mjCBody::Compile(void) {
|
||||
// compute bounding volume hierarchy
|
||||
if (!geoms.empty()) {
|
||||
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
|
||||
@@ -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
|
||||
void mjCGeom::SetInertia(void) {
|
||||
double height;
|
||||
|
||||
+29
-1
@@ -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
|
||||
class mjCBoundingVolumeHierarchy {
|
||||
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> 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 AddBundingVolume(const mjCBoundingVolume& bv);
|
||||
|
||||
private:
|
||||
int MakeBVH(std::vector<mjCBoundingVolume>& elements, int lev = 0);
|
||||
|
||||
std::vector<mjCBoundingVolume> bvh_;
|
||||
std::string name_;
|
||||
double ipos_[3];
|
||||
double iquat_[4];
|
||||
@@ -320,6 +345,9 @@ 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;
|
||||
|
||||
// variables set by user and copied into mjModel
|
||||
mjtGeom type; // geom type
|
||||
int contype; // contact type
|
||||
|
||||
Reference in New Issue
Block a user