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

Added a collision midphase on geom pairs using rotated AABBs static trees.

Browse Source 
PiperOrigin-RevId: 517357695
Change-Id: I068fee714d9638be2fb6d042d5d86f8ff970635b
This commit is contained in:
Alessio Quaglino
2023-03-17 02:36:38 -07:00
committed by Copybara-Service
parent 47b58f5ccb
commit 70959c1a2c
30 changed files with 2228 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_model.cc
+20 -1
View File
@@ -234,6 +234,7 @@ mjCModel::~mjCModel() {
void mjCModel::Clear(void) {
// sizes set from list lengths
nbody = 0;
nbvh = 0;
njnt = 0;
ngeom = 0;
nsite = 0;
@@ -935,6 +936,11 @@ void mjCModel::SetSizes(void) {
}
}
// nbvh
for (i=0; i<nbody; i++) {
nbvh += bodies[i]->nbvh;
}
// nmeshvert, nmeshface, nmeshtexcoord, nmeshgraph
for (i=0; i<nmesh; i++) {
nmeshvert += meshes[i]->nvert;
@@ -1324,6 +1330,7 @@ void mjCModel::CopyTree(mjModel* m) {
int jntadr = 0; // addresses in global arrays
int dofadr = 0;
int qposadr = 0;
int bvh_adr = 0;
// main loop over bodies
for (i=0; i<nbody; i++) {
@@ -1350,6 +1357,17 @@ void mjCModel::CopyTree(mjModel* m) {
m->body_gravcomp[i] = pb->gravcomp;
copyvec(m->body_user+nuser_body*i, pb->userdata.data(), nuser_body);
// bounding volume hierarchy
m->body_bvhadr[i] = (!pb->geoms.empty() ? bvh_adr : -1);
m->body_bvhnum[i] = pb->nbvh;
if (pb->nbvh) {
memcpy(m->bvh_aabb + 6*bvh_adr, pb->bvh.data(), 6*pb->nbvh*sizeof(mjtNum));
memcpy(m->bvh_child + 2*bvh_adr, pb->child.data(), 2*pb->nbvh*sizeof(int));
memcpy(m->bvh_geomid + bvh_adr, pb->nodeid.data(), pb->nbvh*sizeof(int));
memcpy(m->bvh_depth + bvh_adr, pb->level.data(), pb->nbvh*sizeof(int));
}
bvh_adr += pb->nbvh;
// count free joints
int cntfree = 0;
for (j=0; j<(int)pb->joints.size(); j++) {
@@ -1498,6 +1516,7 @@ void mjCModel::CopyTree(mjModel* m) {
m->geom_group[gid] = pg->group;
m->geom_priority[gid] = pg->priority;
copyvec(m->geom_size+3*gid, pg->size, 3);
copyvec(m->geom_aabb+6*gid, pg->aabb, 6);
copyvec(m->geom_pos+3*gid, pg->locpos, 3);
copyvec(m->geom_quat+4*gid, pg->locquat, 4);
copyvec(m->geom_friction+3*gid, pg->friction, 3);
@@ -2659,7 +2678,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
}
// create low-level model
m = mj_makeModel(nq, nv, nu, na, nbody, njnt, ngeom, nsite, ncam, nlight,
m = mj_makeModel(nq, nv, nu, na, nbody, nbvh, njnt, ngeom, nsite, ncam, nlight,
nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph,
nskin, nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert,
nhfield, nhfielddata, ntex, ntexdata, nmat, npair, nexclude,
src/user/user_model.h
+1
View File
@@ -215,6 +215,7 @@ class mjCModel {
int nv; // number of degrees of freedom = dim(qvel)
int nu; // number of actuators/controls
int na; // number of activation variables
int nbvh; // number of boundary volume hierarchies
int nmeshvert; // number of vertices in all meshes
int nmeshnormal; // number of normals in all meshes
int nmeshtexcoord; // number of texture coordinates in all meshes
src/user/user_objects.cc
+223
View File
@@ -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)) {
src/user/user_objects.h
+11
View File
@@ -213,6 +213,15 @@ class mjCBody : public mjCBase {
int lastdof; // id of last dof
int subtreedofs; // number of dofs in subtree, including self
int MakeBVH(std::vector<mjCGeom *>&, int lev); // make bounding volume hierarchy
int nbvh;
std::vector<mjtNum> bvh; // bounding volume hierarchy
std::vector<int> child; // children of bvh nodes
std::vector<int> nodeid; // id of the geom contained by the node
std::vector<int> level; // levels of bvh
// objects allocated by Add functions
std::vector<mjCBody*> bodies; // child bodies
std::vector<mjCGeom*> geoms; // geoms attached to this body
@@ -333,6 +342,7 @@ class mjCGeom : public mjCBase {
mjCGeom(mjCModel* = 0, mjCDef* = 0);// constructor
void Compile(void); // compiler
double GetRBound(void); // compute bounding sphere radius
void ComputeAABB(void); // compute axis-aligned bounding box
int matid; // id of geom's material
int meshid; // id of geom's mesh (-1: none)
@@ -341,6 +351,7 @@ class mjCGeom : public mjCBase {
double inertia[3]; // local diagonal inertia
double locpos[3]; // local position
double locquat[4]; // local orientation
double aabb[6]; // half-sizes of axis-aligned bounding box
mjCBody* body; // geom's body
};