Added a collision midphase on geom pairs using rotated AABBs static trees.
PiperOrigin-RevId: 517357695 Change-Id: I068fee714d9638be2fb6d042d5d86f8ff970635b
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
47b58f5ccb
commit
70959c1a2c
@@ -37,6 +37,7 @@
|
||||
#include "engine/engine_util_errmem.h"
|
||||
#include "engine/engine_util_misc.h"
|
||||
#include "engine/engine_util_solve.h"
|
||||
#include "engine/engine_util_spatial.h"
|
||||
#include "engine/engine_vfs.h"
|
||||
#include "user/user_model.h"
|
||||
#include "user/user_util.h"
|
||||
@@ -330,6 +331,7 @@ mjCBody::mjCBody(mjCModel* _model) {
|
||||
subtreedofs = 0;
|
||||
gravcomp = 0;
|
||||
userdata.clear();
|
||||
nbvh = 0;
|
||||
|
||||
// plugin variables
|
||||
is_plugin = false;
|
||||
@@ -344,6 +346,10 @@ mjCBody::mjCBody(mjCModel* _model) {
|
||||
sites.clear();
|
||||
cameras.clear();
|
||||
lights.clear();
|
||||
bvh.clear();
|
||||
child.clear();
|
||||
nodeid.clear();
|
||||
level.clear();
|
||||
}
|
||||
|
||||
|
||||
@@ -664,6 +670,154 @@ void mjCBody::MakeInertialExplicit() {
|
||||
}
|
||||
|
||||
|
||||
// compute bounding volume hierarchy
|
||||
int mjCBody::MakeBVH(std::vector<mjCGeom *>& elements, int lev) {
|
||||
int nelements = elements.size();
|
||||
mjtNum AABB[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
|
||||
|
||||
// inverse transformation
|
||||
mjtNum qinv[4] = {iquat[0], -iquat[1], -iquat[2], -iquat[3]};
|
||||
|
||||
for (int i=0; i<nelements; i++) {
|
||||
// skip visual objects
|
||||
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]};
|
||||
|
||||
// update node AABB
|
||||
for (int v=0; v<8; v++) {
|
||||
mjtNum vert[3], box[3];
|
||||
vert[0] = (v&1 ? aabb[3] : aabb[0]);
|
||||
vert[1] = (v&2 ? aabb[4] : aabb[1]);
|
||||
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(vert, box, qinv);
|
||||
AABB[0] = mjMIN(AABB[0], vert[0]);
|
||||
AABB[1] = mjMIN(AABB[1], vert[1]);
|
||||
AABB[2] = mjMIN(AABB[2], vert[2]);
|
||||
AABB[3] = mjMAX(AABB[3], vert[0]);
|
||||
AABB[4] = mjMAX(AABB[4], vert[1]);
|
||||
AABB[5] = mjMAX(AABB[5], vert[2]);
|
||||
}
|
||||
}
|
||||
|
||||
// store current index
|
||||
int index = nbvh++;
|
||||
child.push_back(-1);
|
||||
child.push_back(-1);
|
||||
nodeid.push_back(-1);
|
||||
level.push_back(lev);
|
||||
|
||||
// transform representation
|
||||
mjtNum center[] = {(AABB[3] + AABB[0]) / 2, (AABB[4] + AABB[1]) / 2,
|
||||
(AABB[5] + AABB[2]) / 2};
|
||||
mjtNum size[] = {(AABB[3] - AABB[0]) / 2, (AABB[4] - AABB[1]) / 2,
|
||||
(AABB[5] - AABB[2]) / 2};
|
||||
|
||||
// store bounding box of the current node
|
||||
for (int i=0; i<3; i++) {
|
||||
bvh.push_back(center[i]);
|
||||
}
|
||||
for (int i=0; i<3; i++) {
|
||||
bvh.push_back(size[i]);
|
||||
}
|
||||
|
||||
// leaf node, return
|
||||
if (nelements==1) {
|
||||
for (int i=0; i<2; i++) {
|
||||
child[2*index+i] = -1;
|
||||
}
|
||||
nodeid[index] = elements[0]->id;
|
||||
return index;
|
||||
}
|
||||
|
||||
// find longest axis for splitting the bounding box
|
||||
mjtNum edges[3] = { AABB[3]-AABB[0], AABB[4]-AABB[1], AABB[5]-AABB[2] };
|
||||
int axis = edges[0] > edges[1] ? 0 : 1;
|
||||
axis = edges[axis] > edges[2] ? axis : 2;
|
||||
|
||||
// find median along the axis
|
||||
std::vector<mjtNum> pos(nelements);
|
||||
|
||||
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 lpos[3] = {};
|
||||
mju_rotVecQuat(lpos, vert, qinv);
|
||||
pos[i] = lpos[axis];
|
||||
}
|
||||
|
||||
auto m = pos.size()/2;
|
||||
std::nth_element(pos.begin(), pos.begin() + m, pos.end());
|
||||
mjtNum threshold = pos[m];
|
||||
|
||||
// split using median
|
||||
std::vector<mjCGeom *> left;
|
||||
std::vector<mjCGeom *> 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 lpos[3] = {};
|
||||
mju_rotVecQuat(lpos, vert, qinv);
|
||||
|
||||
// skip visual objects
|
||||
if (elements[i]->conaffinity==0 && elements[i]->contype==0) {
|
||||
skipped++;
|
||||
continue;
|
||||
}
|
||||
if (lpos[axis] < threshold) {
|
||||
left.push_back(elements[i]);
|
||||
} else if (lpos[axis] > threshold) {
|
||||
right.push_back(elements[i]);
|
||||
} else {
|
||||
if (left.size() < right.size()) left.push_back(elements[i]);
|
||||
else right.push_back(elements[i]);
|
||||
}
|
||||
}
|
||||
|
||||
// recursive calls
|
||||
if (!left.empty()) {
|
||||
child[2*index+0] = MakeBVH(left, lev+1);
|
||||
}
|
||||
|
||||
if (!right.empty()) {
|
||||
child[2*index+1] = MakeBVH(right, lev+1);
|
||||
}
|
||||
|
||||
// SHOULD NOT OCCUR
|
||||
if (left.size()+right.size()+skipped != nelements) {
|
||||
throw mjCError(this, "some elements were lost, body=%s parent=%d children=%d",
|
||||
name.c_str(), nelements, left.size()+right.size()+skipped);
|
||||
}
|
||||
|
||||
if (child[2*index+0]==-1 && child[2*index+1]==-1 && !skipped) {
|
||||
throw mjCError(this, "this should have been a leaf, body=%s nelements=%d",
|
||||
name.c_str(), nelements);
|
||||
}
|
||||
|
||||
return index;
|
||||
}
|
||||
|
||||
|
||||
// compiler
|
||||
void mjCBody::Compile(void) {
|
||||
unsigned int i;
|
||||
@@ -774,6 +928,11 @@ void mjCBody::Compile(void) {
|
||||
MakeLocal(geoms[i]->locpos, geoms[i]->locquat, geoms[i]->pos, geoms[i]->quat);
|
||||
}
|
||||
|
||||
// compute bounding volume hierarchy
|
||||
if (!geoms.empty()) {
|
||||
MakeBVH(geoms, 0);
|
||||
}
|
||||
|
||||
// compile all joints, count dofs
|
||||
dofnum = 0;
|
||||
for (i=0; i<joints.size(); i++) {
|
||||
@@ -1311,6 +1470,67 @@ void mjCGeom::SetFluidCoefs(void) {
|
||||
}
|
||||
|
||||
|
||||
// compute bounding box
|
||||
void mjCGeom::ComputeAABB() {
|
||||
switch (type) {
|
||||
case mjGEOM_SPHERE:
|
||||
aabb[3] = aabb[4] = aabb[5] = size[0];
|
||||
mjuu_setvec(aabb, -aabb[3], -aabb[4], -aabb[5]);
|
||||
break;
|
||||
|
||||
case mjGEOM_CAPSULE:
|
||||
aabb[3] = aabb[4] = size[0];
|
||||
aabb[5] = size[0] + size[1];
|
||||
mjuu_setvec(aabb, -aabb[3], -aabb[4], -aabb[5]);
|
||||
break;
|
||||
|
||||
case mjGEOM_CYLINDER:
|
||||
aabb[3] = aabb[4] = size[0];
|
||||
aabb[5] = size[1];
|
||||
mjuu_setvec(aabb, -aabb[3], -aabb[4], -aabb[5]);
|
||||
break;
|
||||
|
||||
case mjGEOM_MESH:
|
||||
mjuu_copyvec(aabb, model->meshes[meshid]->aabb, 6);
|
||||
break;
|
||||
|
||||
case mjGEOM_PLANE:
|
||||
aabb[0] = aabb[1] = aabb[2] = -mjMAXVAL;
|
||||
aabb[3] = aabb[4] = mjMAXVAL;
|
||||
aabb[5] = 0;
|
||||
break;
|
||||
|
||||
case mjGEOM_HFIELD:
|
||||
aabb[0] = -size[0];
|
||||
aabb[1] = -size[1];
|
||||
aabb[2] = -model->hfields[hfieldid]->size[3];
|
||||
aabb[3] = size[0];
|
||||
aabb[4] = size[1];
|
||||
aabb[5] = model->hfields[hfieldid]->size[2];
|
||||
break;
|
||||
|
||||
default:
|
||||
mjuu_copyvec(aabb+3, size, 3);
|
||||
mjuu_setvec(aabb, -size[0], -size[1], -size[2]);
|
||||
break;
|
||||
}
|
||||
|
||||
aabb[0] -= margin;
|
||||
aabb[1] -= margin;
|
||||
aabb[2] -= margin;
|
||||
aabb[3] += margin;
|
||||
aabb[4] += margin;
|
||||
aabb[5] += margin;
|
||||
|
||||
mjtNum pos[] = {(aabb[3] + aabb[0]) / 2, (aabb[4] + aabb[1]) / 2,
|
||||
(aabb[5] + aabb[2]) / 2};
|
||||
mjtNum size[] = {(aabb[3] - aabb[0]) / 2, (aabb[4] - aabb[1]) / 2,
|
||||
(aabb[5] - aabb[2]) / 2};
|
||||
|
||||
mjuu_copyvec(aabb, pos, 3);
|
||||
mjuu_copyvec(aabb+3, size, 3);
|
||||
}
|
||||
|
||||
|
||||
// compiler
|
||||
void mjCGeom::Compile(void) {
|
||||
@@ -1444,6 +1664,9 @@ void mjCGeom::Compile(void) {
|
||||
size[2] = mjMAX(fabs(aabb[2]), fabs(aabb[5]));
|
||||
}
|
||||
|
||||
// compute aabb
|
||||
ComputeAABB();
|
||||
|
||||
// compute geom mass and inertia
|
||||
if (inferinertia) {
|
||||
if (mjuu_defined(_mass)) {
|
||||
|
||||
Reference in New Issue
Block a user