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/engine/engine_collision_driver.c
+327 -55
View File
@@ -19,7 +19,6 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjxmacro.h>
#include "engine/engine_callback.h"
#include "engine/engine_collision_convex.h"
#include "engine/engine_collision_primitive.h"
@@ -48,8 +47,319 @@ mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
//------------------------------------ static functions --------------------------------------------
// plane to geom_center squared distance, g1 is a plane
static mjtNum plane_geom(const mjModel* m, mjData* d, int g1, int g2) {
mjtNum* mat1 = d->geom_xmat + 9*g1;
mjtNum norm[3] = {mat1[2], mat1[5], mat1[8]};
mjtNum dif[3];
mju_sub3(dif, d->geom_xpos + 3*g2, d->geom_xpos + 3*g1);
return mju_dot3(dif, norm);
}
// squared Euclidean distance between 3D vectors
static inline mjtNum squaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]};
return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2];
}
// bounding-sphere collision
static int mj_collideSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum margin) {
// neither geom is a plane
if (m->geom_rbound[g1]>0 && m->geom_rbound[g2]>0) {
mjtNum bound = m->geom_rbound[g1] + m->geom_rbound[g2] + margin;
if (squaredDist3(d->geom_xpos+3*g1, d->geom_xpos+3*g2) > bound*bound) {
return 0;
}
}
// one geom is a plane
if (m->geom_type[g1]==mjGEOM_PLANE && m->geom_rbound[g2]>0
&& plane_geom(m, d, g1, g2) > margin+m->geom_rbound[g2]) {
return 0;
}
if (m->geom_type[g2]==mjGEOM_PLANE && m->geom_rbound[g1]>0
&& plane_geom(m, d, g2, g1) > margin+m->geom_rbound[g1]) {
return 0;
}
return 1;
}
//------------------------------------ binary tree search ------------------------------------------
// checks if the proposed collision pair is already present in pair_geom and calls narrow phase
void mj_collidePair(const mjModel* m, mjData* d, int g1, int g2, int merged,
int startadr, int pairadr) {
// merged: make sure geom pair is not repeated
if (merged) {
// find matching pair
int found = 0;
for (int k=startadr; k<pairadr; k++) {
if ((m->pair_geom1[k]==g1 && m->pair_geom2[k]==g2) ||
(m->pair_geom1[k]==g2 && m->pair_geom2[k]==g1)) {
found = 1;
break;
}
}
// not found: test
if (!found) {
mj_collideGeoms(m, d, g1, g2, 0, 0);
}
}
// not merged: always test
else {
mj_collideGeoms(m, d, g1, g2, 0, 0);
}
}
// oriented bounding boxes collision (see Gottschalk et al.)
int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6],
const mjtNum xpos1[3], const mjtNum xmat1[9],
const mjtNum xpos2[3], const mjtNum xmat2[9],
mjtNum product[36], mjtNum offset[12], mjtByte* initialize) {
// get infinite dimensions (planes only)
mjtByte inf1[3] = {aabb1[3] >= mjMAXVAL, aabb1[4] >= mjMAXVAL, aabb1[5] >= mjMAXVAL};
mjtByte inf2[3] = {aabb2[3] >= mjMAXVAL, aabb2[4] >= mjMAXVAL, aabb2[5] >= mjMAXVAL};
// if a bounding box is infinite, there must be a collision
if ((inf1[0] && inf1[1] && inf1[2]) || (inf2[0] && inf2[1] && inf2[2])) {
return 1;
}
const mjtNum* aabb[2] = {aabb1, aabb2};
const mjtNum *xmat[2] = {xmat1, xmat2};
const mjtNum *xpos[2] = {xpos1, xpos2};
mjtNum xcenter[2][3], normal[2][3][3];
mjtNum proj[2], radius[2];
mjtByte infinite[2] = {inf1[0] || inf1[1] || inf1[2], inf2[0] || inf2[1] || inf2[2]};
// compute centers in local coordinates
if (product==NULL) {
for (int i=0; i<2; i++) { // bounding boxes
for (int j=0; j<3; j++) { // axes
mju_rotVecMat(xcenter[i], aabb[i], xmat[i]);
mju_addTo3(xcenter[i], xpos[i]);
}
}
}
// compute normals in global coordinates
for (int i=0; i<2; i++) { // bounding boxes
for (int j=0; j<3; j++) { // faces
for (int k=0; k<3; k++) { // world axes
normal[i][j][k] = xmat[i][3*k+j];
}
}
}
// precompute dot products
if (product && offset && *initialize) {
for (int i=0; i<2; i++) { // bodies
for (int j=0; j<2; j++) { // bodies
for (int k=0; k<3; k++) { // axes
for (int l=0; l<3; l++) { // axes
product[18*i + 9*j + 3*k + l] = mju_dot3(normal[i][l], normal[j][k]);
}
offset[6*i + 3*j + k] = mju_dot3(xpos[i], normal[j][k]);
}
}
}
*initialize = 0;
}
// check intersections
for (int j=0; j<2; j++) { // bounding boxes
if (infinite[1-j]) {
continue; // skip test against an infinite body
}
for (int k=0; k<3; k++) { // face
for (int i=0; i<2; i++) { // bounding boxes
if (product==NULL) {
proj[i] = mju_dot3(xcenter[i], normal[j][k]);
radius[i] = fabs(aabb[i][3]*mju_dot3(normal[i][0], normal[j][k])) +
fabs(aabb[i][4]*mju_dot3(normal[i][1], normal[j][k])) +
fabs(aabb[i][5]*mju_dot3(normal[i][2], normal[j][k]));
} else {
int adr = 18*i + 9*j + 3*k;
proj[i] = aabb[i][0] * product[adr + 0] +
aabb[i][1] * product[adr + 1] +
aabb[i][2] * product[adr + 2] +
offset[6*i + 3*j + k];
radius[i] = fabs(aabb[i][3]*product[adr + 0]) +
fabs(aabb[i][4]*product[adr + 1]) +
fabs(aabb[i][5]*product[adr + 2]);
}
}
if (radius[0]+radius[1] < fabs(proj[1]-proj[0])) {
return 0;
}
}
}
return 1;
}
static mjCollisionTree* mj_stackAllocTree(mjData* d, int max_stack) {
// check that the quotient is an integer
_Static_assert(sizeof(mjCollisionTree*) % sizeof(mjtNum) == 0,
"mjCollisionTree has a different size from mjtNum");
return (mjCollisionTree*)mj_stackAlloc(
d, max_stack * sizeof(mjCollisionTree*) / sizeof(mjtNum));
}
// binary search between two body trees
void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2,
int merged, int startadr, int pairadr) {
const int bvhadr1 = m->body_bvhadr[b1];
const int bvhadr2 = m->body_bvhadr[b2];
const mjtNum* bvh1 = m->bvh_aabb + 6 * bvhadr1;
const mjtNum* bvh2 = m->bvh_aabb + 6 * bvhadr2;
const int* child1 = m->bvh_child + 2 * bvhadr1;
const int* child2 = m->bvh_child + 2 * bvhadr2;
mjtNum product[36] = {}; // 2 bb x 2 bb x 3 axes (body) x 3 axes (world)
mjtNum offset[12] = {}; // 2 bb x 2 bb x 3 axes (world)
mjtByte initialize = 1;
mjMARKSTACK;
// TODO(b/273737633): Store bvh max depths to make this bound tighter.
const int max_stack = m->body_bvhnum[b1] + m->body_bvhnum[b2];
mjCollisionTree* stack = mj_stackAllocTree(d, max_stack);
int nstack = 1;
stack[0].node1 = stack[0].node2 = 0;
while (nstack) {
// pop from stack
nstack--;
int node1 = stack[nstack].node1;
int node2 = stack[nstack].node2;
mjtByte isleaf1 = (child1[2*node1] == -1) && (child1[2*node1+1] == -1);
mjtByte isleaf2 = (child2[2*node2] == -1) && (child2[2*node2+1] == -1);
int nodeid1 = m->bvh_geomid[bvhadr1 + node1];
int nodeid2 = m->bvh_geomid[bvhadr2 + node2];
// both are leaves
if (isleaf1 && isleaf2 && nodeid1!=-1 && nodeid2!=-1) {
if (mj_collideSphere(m, d, nodeid1, nodeid2, /*margin=*/ 0)) {
if (mj_collideOBB(m->geom_aabb + 6*nodeid1, m->geom_aabb + 6*nodeid2,
d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1,
d->geom_xpos + 3*nodeid2, d->geom_xmat + 9*nodeid2,
NULL, NULL, &initialize)) {
mj_collidePair(m, d, nodeid1, nodeid2, merged, startadr, pairadr);
d->bvh_active[node1 + bvhadr1] = 1;
d->bvh_active[node2 + bvhadr2] = 1;
}
}
continue;
}
// if no intersection at intermediate levels, stop
if (!mj_collideOBB(bvh1 + 6*node1, bvh2 + 6*node2,
d->xipos + 3*b1, d->ximat + 9*b1,
d->xipos + 3*b2, d->ximat + 9*b2,
product, offset, &initialize)) {
continue;
}
d->bvh_active[node1 + bvhadr1] = 1;
d->bvh_active[node2 + bvhadr2] = 1;
// keep traversing the tree
if (!isleaf1 && isleaf2) {
for (int i=0; i<2; i++) {
if (child1[2*node1+i] != -1) {
if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = child1[2*node1+i];
stack[nstack].node2 = node2;
nstack++;
}
}
} else if (isleaf1 && !isleaf2) {
for (int i=0; i<2; i++) {
if (child2[2*node2+i] != -1) {
if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = node1;
stack[nstack].node2 = child2[2*node2+i];
nstack++;
}
}
} else {
// compute surface areas of bounding boxes
mjtNum x1 = bvh1[6*node1+3]-bvh1[6*node1+0];
mjtNum y1 = bvh1[6*node1+4]-bvh1[6*node1+1];
mjtNum z1 = bvh1[6*node1+5]-bvh1[6*node1+2];
mjtNum x2 = bvh2[6*node2+3]-bvh2[6*node2+0];
mjtNum y2 = bvh2[6*node2+4]-bvh2[6*node2+1];
mjtNum z2 = bvh2[6*node2+5]-bvh2[6*node2+2];
mjtNum surface1 = x1*y1 + y1*z1 + z1*x1;
mjtNum surface2 = x2*y2 + y2*z2 + z2*x2;
// traverse the hierarchy whose bounding box has the larger surface area
if (surface1 > surface2) {
for (int i = 0; i < 2; i++) {
if (child1[2 * node1 + i] != -1) {
if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = child1[2 * node1 + i];
stack[nstack].node2 = node2;
nstack++;
}
}
} else {
for (int i = 0; i < 2; i++) {
if (child2[2 * node2 + i] != -1) {
if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = node1;
stack[nstack].node2 = child2[2*node2+i];
nstack++;
}
}
}
}
}
mjFREESTACK;
}
//----------------------------- collision detection entry point ------------------------------------
// compare contact pairs by their geom IDs
quicksortfunc(contactcompare, context, el1, el2) {
const mjModel* m = (const mjModel*) context;
mjContact* con1 = (mjContact*)el1;
mjContact* con2 = (mjContact*)el2;
// reproduce the order contacts without mj_collideTree
// normally sorted by (g1, g2), but in mj_collideGeoms, g1 and g2 are swapped based on geom_type.
// here we undo this swapping for the purpose of sorting - needs to be done for each mjContact
int con1_g1 = con1->geom1;
int con1_g2 = con1->geom2;
if (m->geom_type[con1_g1] > m->geom_type[con1_g2]) {
int tmp = con1_g1;
con1_g1 = con1_g2;
con1_g2 = tmp;
}
int con2_g1 = con2->geom1;
int con2_g2 = con2->geom2;
if (m->geom_type[con2_g1] > m->geom_type[con2_g2]) {
int tmp = con2_g1;
con2_g1 = con2_g2;
con2_g2 = tmp;
}
if (con1_g1 < con2_g1) return -1;
if (con1_g1 > con2_g1) return 1;
if (con1_g2 < con2_g2) return -1;
if (con1_g2 > con2_g2) return 1;
return 0;
}
void mj_collision(const mjModel* m, mjData* d) {
int g1, g2, merged, b1 = 0, b2 = 0, exadr = 0, pairadr = 0, startadr;
int nexclude = m->nexclude, npair = m->npair, nbodypair = ((m->nbody-1)*m->nbody)/2;
@@ -59,6 +369,9 @@ void mj_collision(const mjModel* m, mjData* d) {
// reset the size of the contact array
d->ncon = 0;
// reset the visualization flags
memset(d->bvh_active, 0, m->nbvh);
// return if disabled
if (mjDISABLED(mjDSBL_CONSTRAINT) || mjDISABLED(mjDSBL_CONTACT)
|| m->nconmax==0 || m->nbody < 2) {
@@ -122,29 +435,17 @@ void mj_collision(const mjModel* m, mjData* d) {
// test all geom pairs within this body pair
if (m->body_geomnum[b1] && m->body_geomnum[b2]) {
for (g1=m->body_geomadr[b1]; g1<m->body_geomadr[b1]+m->body_geomnum[b1]; g1++) {
for (g2=m->body_geomadr[b2]; g2<m->body_geomadr[b2]+m->body_geomnum[b2]; g2++) {
// merged: make sure geom pair is not repeated
if (merged) {
// find matching pair
int found = 0;
for (int k=startadr; k<pairadr; k++) {
if ((m->pair_geom1[k]==g1 && m->pair_geom2[k]==g2) ||
(m->pair_geom1[k]==g2 && m->pair_geom2[k]==g1)) {
found = 1;
break;
}
}
// not found: test
if (!found) {
mj_collideGeoms(m, d, g1, g2, 0, 0);
}
}
// not merged: always test
else {
mj_collideGeoms(m, d, g1, g2, 0, 0);
if (!mjDISABLED(mjDSBL_MIDPHASE) && m->body_geomnum[b1]*m->body_geomnum[b2]>1) {
int ncon_before = d->ncon;
mj_collideTree(m, d, b1, b2, merged, startadr, pairadr);
int ncon_after = d->ncon;
void* context = (void*) m;
mjQUICKSORT(d->contact + ncon_before, ncon_after - ncon_before,
sizeof(mjContact), contactcompare, context);
} else {
for (g1=m->body_geomadr[b1]; g1<m->body_geomadr[b1]+m->body_geomnum[b1]; g1++) {
for (g2=m->body_geomadr[b2]; g2<m->body_geomadr[b2]+m->body_geomnum[b2]; g2++) {
mj_collidePair(m, d, g1, g2, merged, startadr, pairadr);
}
}
}
@@ -522,22 +823,6 @@ endbroad:
//----------------------------- narrow-phase collision detection -----------------------------------
// plane : geom_center distance, assuming g1 is plane
static mjtNum plane_geom(const mjModel* m, mjData* d, int g1, int g2) {
mjtNum* mat1 = d->geom_xmat + 9*g1;
mjtNum norm[3] = {mat1[2], mat1[5], mat1[8]};
mjtNum dif[3];
mju_sub3(dif, d->geom_xpos + 3*g2, d->geom_xpos + 3*g1);
return mju_dot3(dif, norm);
}
// squared Euclidean distance between 3D vectors
static inline mjtNum squaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]};
return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2];
}
// test two geoms for collision, apply filters, add to contact list
// flg_user disables filters and uses usermargin
@@ -615,21 +900,8 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
}
// bounding sphere filter
if (m->geom_rbound[g1]>0 && m->geom_rbound[g2]>0) {
mjtNum bound = m->geom_rbound[g1] + m->geom_rbound[g2] + margin;
if (squaredDist3(d->geom_xpos+3*g1, d->geom_xpos+3*g2) > bound*bound) {
return;
}
}
// plane : bounding sphere filter
if (m->geom_type[g1]==mjGEOM_PLANE && m->geom_rbound[g2]>0
&& plane_geom(m, d, g1, g2) > margin+m->geom_rbound[g2]) {
return;
}
if (m->geom_type[g2]==mjGEOM_PLANE && m->geom_rbound[g1]>0
&& plane_geom(m, d, g2, g1) > margin+m->geom_rbound[g1]) {
return;
if (!mj_collideSphere(m, d, g1, g2, margin)) {
return;
}
// call collision detector to generate contacts
src/engine/engine_collision_driver.h
+14
View File
@@ -21,14 +21,28 @@
#ifdef __cplusplus
extern "C" {
#else
#include <stdalign.h>
#endif
struct mjCollisionTree_ {
alignas(mjtNum) int node1;
int node2;
};
typedef struct mjCollisionTree_ mjCollisionTree;
// collision function pointers and max contact pairs
MJAPI extern mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES];
// collision detection entry point
MJAPI void mj_collision(const mjModel* m, mjData* d);
// applies Separating Axis Theorem for rotated AABBs
MJAPI int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6],
const mjtNum xpos1[3], const mjtNum xmat1[9],
const mjtNum xpos2[3], const mjtNum xmat2[9],
mjtNum product[36], mjtNum offset[12], mjtByte* initialize);
// broad phase collistion detection; return list of body pairs for narrow phase
int mj_broadphase(const mjModel* m, mjData* d, int* bodypair, int maxpair);
src/engine/engine_io.c
+5 -3
View File
@@ -144,6 +144,7 @@ void mj_defaultVisual(mjVisual* vis) {
vis->global.offwidth = 640;
vis->global.offheight = 480;
vis->global.realtime = 1.0;
vis->global.treedepth = 1;
// rendering quality
vis->quality.shadowsize = 4096;
@@ -375,7 +376,7 @@ static int safeAddToBufferSize(intptr_t* offset, int* nbuffer, size_t type_size,
// allocate and initialize mjModel structure
mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int njnt,
mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int njnt,
int ngeom, int nsite, int ncam, int nlight,
int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
@@ -402,6 +403,7 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int njnt,
m->nu = nu;
m->na = na;
m->nbody = nbody;
m->nbvh = nbvh;
m->njnt = njnt;
m->ngeom = ngeom;
m->nsite = nsite;
@@ -521,7 +523,7 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
// allocate new model if needed
if (!dest) {
dest = mj_makeModel(src->nq, src->nv, src->nu, src->na, src->nbody, src->njnt,
dest = mj_makeModel(src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh, src->njnt,
src->ngeom, src->nsite, src->ncam, src->nlight, src->nmesh, src->nmeshvert,
src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, src->nmeshgraph,
src->nskin, src->nskinvert, src->nskintexvert, src->nskinface,
@@ -702,7 +704,7 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
info[28], info[29], info[30], info[31], info[32], info[33], info[34],
info[35], info[36], info[37], info[38], info[39], info[40], info[41],
info[42], info[43], info[44], info[45], info[46], info[47], info[48],
info[49], info[50], info[51]);
info[49], info[50], info[51], info[52]);
if (!m || m->nbuffer!=info[getnint()-1]) {
if (fp) {
fclose(fp);
src/engine/engine_io.h
+1 -1
View File
@@ -47,7 +47,7 @@ void mj_defaultStatistic(mjStatistic* stat);
//------------------------------- mjModel ----------------------------------------------------------
// allocate mjModel
mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int njnt,
mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int njnt,
int ngeom, int nsite, int ncam, int nlight,
int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
src/engine/engine_support.c
+2 -1
View File
@@ -53,7 +53,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = {
"Filterparent",
"Actuation",
"Refsafe",
"Sensor"
"Sensor",
"Midphase"
};
src/engine/engine_vis_init.c
+2 -1
View File
@@ -88,7 +88,8 @@ const char* mjVISSTRING[mjNVISFLAG][3] = {
{"Center of &Mass", "0", "M"},
{"S&elect Point", "0", "E"},
{"Static Bo&dy", "1", "D"},
{"Skin", "1", ";"}
{"Skin", "1", ";"},
{"Body Tree", "0", "`"}
};
src/engine/engine_vis_visualize.c
+65
View File
@@ -520,6 +520,71 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
}
// bounding volume hierarchy
if (vopt->flags[mjVIS_MIDPHASE]) {
int bodyid = 0;
float rgba[] = {1, 0, 0, 1};
for (int i = 0; i < m->nbvh; i++) {
int isleaf = m->bvh_child[2*i]==-1 && m->bvh_child[2*i+1]==-1;
if (scn->ngeom >= scn->maxgeom) break;
if (m->bvh_depth[i] != m->vis.global.treedepth) {
if (!isleaf || m->bvh_depth[i] > m->vis.global.treedepth) {
continue;
}
}
// find geom number
int geomid = m->bvh_geomid[i];
while (i >= m->body_bvhadr[bodyid] + m->body_bvhnum[bodyid]) {
bodyid++;
if (bodyid >= m->nbody) {
mju_error("nbvh outside body range.");
}
}
// compute transformation
mjtNum *aabb = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i;
mjtNum x[3];
const mjtNum* xpos = isleaf ? d->geom_xpos + 3 * geomid : d->xipos + 3 * bodyid;
const mjtNum* xmat = isleaf ? d->geom_xmat + 9 * geomid : d->ximat + 9 * bodyid;
mju_rotVecMat(x, aabb, xmat);
mju_addTo3(x, xpos);
rgba[0] = d->bvh_active[i] ? 1 : 0;
rgba[1] = d->bvh_active[i] ? 0 : 1;
mjtNum dist[3][3];
for (int j=0; j<3; j++) {
for (int k=0; k<3; k++) {
dist[k][j] = aabb[k+3] * xmat[3*j+k];
}
}
int split[3] = {1, 2, 4};
for (int v=0; v<8; v++) {
mjtNum from[3] = {x[0], x[1], x[2]};
for (int k=0; k<3; k++) {
mju_addToScl3(from, dist[k], v&split[k] ? 1 : -1);
}
mjtNum to[3];
for (int k=0; k<3; k++) {
mju_addScl3(to, from, dist[k], 2);
if (!(v&split[k])) {
START
mjv_makeConnector(thisgeom, mjGEOM_LINE, 2,
from[0], from[1], from[2],
to[0], to[1], to[2]);
f2f(thisgeom->rgba, rgba, 4);
FINISH
}
}
}
}
}
// inertia
objtype = mjOBJ_BODY;
if (vopt->flags[mjVIS_INERTIA]) {
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
};
src/xml/xml_native_reader.cc
+1
View File
@@ -983,6 +983,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
READDSBL("actuation", mjDSBL_ACTUATION)
READDSBL("refsafe", mjDSBL_REFSAFE)
READDSBL("sensor", mjDSBL_SENSOR)
READDSBL("midphase", mjDSBL_MIDPHASE)
#undef READDSBL
#define READENBL(NAME, MASK) \
src/xml/xml_native_writer.cc
+1
View File
@@ -800,6 +800,7 @@ void mjXWriter::Option(XMLElement* root) {
WRITEDSBL("actuation", mjDSBL_ACTUATION)
WRITEDSBL("refsafe", mjDSBL_REFSAFE)
WRITEDSBL("sensor", mjDSBL_SENSOR)
WRITEDSBL("midphase", mjDSBL_MIDPHASE)
#undef WRITEDSBL
#define WRITEENBL(NAME, MASK) \