diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index a16b53c7..a2d7bc40 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -705,6 +705,12 @@ from its default. function. If a new contact is detected it is added, allowing for up to 4 additional contact points. This feature is currently considered experimental, and both the behavior and the way it is activated may change in the future. +.. _option-flag-midphase: + +:at:`midphase`: :at-val:`[disable, enable], "enable"` + This flag disables the mid-phase collision filtering using a static AABB bounding volume hierarchy (a BVH binary + tree). If disabled, all geoms pairs that are allowed to collide are checked for collisions. + .. _size: diff --git a/doc/changelog.rst b/doc/changelog.rst index a09dc375..2909f2be 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -30,6 +30,16 @@ General one step (ms), 0.5, 0.53, 0.77, 5.0 steps/second, 2000, 1900, 1300, 200 +- Added a collision mid-phase for pruning geoms in body pairs, see :ref:`documentation` for more details. + This is based on static AABB bounding volume hierarchy (a BVH binary tree) in the body inertial frame. + +.. image:: images/computation/midphase.gif + :width: 450px + +.. youtube:: e0babIM8hBo + :align: right + :height: 150px + - The ``mjd_transitionFD`` function no longer triggers sensor calculation unless explicitly requested. - Corrected the spelling of the ``inteval`` attribute to ``interval`` in the :ref:`mjLROpt` struct. - Mesh texture and normal mappings are now 3-per-triangle rather than 1-per-vertex. Mesh vertices are no longer diff --git a/doc/computation.rst b/doc/computation.rst index 85457f91..fb24d889 100644 --- a/doc/computation.rst +++ b/doc/computation.rst @@ -1422,10 +1422,14 @@ Generation user full control, however it is a static mechanism (independent of the spatial arrangement of the geoms at runtime) and can be tedious for large models. It is normally used to supplement the output of the "dynamic" mechanism. Dynamic generation works with bodies rather than geoms; when a body pair is included this means that all geoms attached to - one body can collide with all geoms attached to the other body. The body pairs are generated via broad-phase - collision detection based on a modified sweep-and-prune algorithm. The modification is that the axis for sorting is - chosen as the principal eigenvector of the covariance matrix of all geom centers - which maximizes the spread. If - broad-phase collision detection is disabled by the user, all body pairs are included in this step. + one body can collide with all geoms attached to the other body. + + The body pairs are generated via broad-phase collision detection based on a modified sweep-and-prune algorithm. The + modification is that the axis for sorting is chosen as the principal eigenvector of the covariance matrix of all geom + centers - which maximizes the spread. Then, for each body pair, a mid-phase collision detection using a static + bounding volume hierarchy (a BVH binary tree) of axis-aligned bounding boxes (AABB) is performed. Each body is + equipped with an AABB tree of its geoms, aligned with the body inertial or geom frames for all inner or leaf nodes, + respectively. Finally, the user can explicitly exclude certain body pairs using the :ref:`exclude ` element in MJCF. Exclusion is applied when "dynamic" or "all" are selected, but not when "pair" is selected. At the end of diff --git a/doc/images/computation/midphase.gif b/doc/images/computation/midphase.gif new file mode 100644 index 00000000..fe50cc2d Binary files /dev/null and b/doc/images/computation/midphase.gif differ diff --git a/doc/includes/references.h b/doc/includes/references.h index 3a12960e..6c7c5c7e 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -221,6 +221,9 @@ struct mjData_ { mjtNum* qLDiagInv; // 1/diag(D) (nv x 1) mjtNum* qLDiagSqrtInv; // 1/sqrt(diag(D)) (nv x 1) + // computed by mj_collisionTree + mjtByte* bvh_active; // volume has been added to collisions (nbvh x 1) + //-------------------------------- POSITION, VELOCITY dependent // computed by mj_fwdVelocity @@ -337,8 +340,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags mjDSBL_ACTUATION = 1<<10, // apply actuation forces mjDSBL_REFSAFE = 1<<11, // integrator safety: make ref[0]>=2*timestep mjDSBL_SENSOR = 1<<12, // sensors + mjDSBL_MIDPHASE = 1<<13, // mid-phase collision filtering - mjNDISABLE = 13 // number of disable flags + mjNDISABLE = 14 // number of disable flags } mjtDisableBit; typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_OVERRIDE = 1<<0, // override contact parameters @@ -649,6 +653,7 @@ struct mjVisual_ { // visualization options float realtime; // initial real-time factor (1: real time) int offwidth; // width of offscreen buffer int offheight; // height of offscreen buffer + int treedepth; // depth of the bounding volume hierarchy } global; struct { // rendering quality @@ -744,6 +749,7 @@ struct mjModel_ { int nu; // number of actuators/controls = dim(ctrl) int na; // number of activation states = dim(act) int nbody; // number of bodies + int nbvh; // number of total bounding volumes in all bodies int njnt; // number of joints int ngeom; // number of geoms int nsite; // number of sites @@ -846,6 +852,14 @@ struct mjModel_ { mjtNum* body_gravcomp; // antigravity force, units of body weight (nbody x 1) mjtNum* body_user; // user data (nbody x nuser_body) int* body_plugin; // plugin instance id (-1 if not in use) (nbody x 1) + int* body_bvhadr; // address of bvh root (nbody x 1) + int* body_bvhnum; // number of bounding volumes (nbody x 1) + + // bounding volume hierarchy + int* bvh_depth; // depth in the bounding volume hierarchy (nbvh x 1) + int* bvh_child; // left and right children in tree (nbvh x 2) + int* bvh_geomid; // geom id of the node (non-leaf: -1) (nbvh x 1) + mjtNum* bvh_aabb; // bounding box of node (center, size) (nbvh x 6) // joints int* jnt_type; // type of joint (mjtJoint) (njnt x 1) @@ -892,6 +906,7 @@ struct mjModel_ { mjtNum* geom_solref; // constraint solver reference: contact (ngeom x mjNREF) mjtNum* geom_solimp; // constraint solver impedance: contact (ngeom x mjNIMP) mjtNum* geom_size; // geom-specific size parameters (ngeom x 3) + mjtNum* geom_aabb; // bounding box, (center, size) (ngeom x 6) mjtNum* geom_rbound; // radius of bounding sphere (ngeom x 1) mjtNum* geom_pos; // local position offset rel. to body (ngeom x 3) mjtNum* geom_quat; // local orientation offset rel. to body (ngeom x 4) @@ -1618,6 +1633,7 @@ typedef enum mjtVisFlag_ { // flags enabling model element visualization mjVIS_SELECT, // selection point mjVIS_STATIC, // static bodies mjVIS_SKIN, // skin + mjVIS_MIDPHASE, // mid-phase bounding volume hierarchy mjNVISFLAG // number of visualization flags } mjtVisFlag; diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index e7b5e691..df68035f 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -246,6 +246,9 @@ struct mjData_ { mjtNum* qLDiagInv; // 1/diag(D) (nv x 1) mjtNum* qLDiagSqrtInv; // 1/sqrt(diag(D)) (nv x 1) + // computed by mj_collisionTree + mjtByte* bvh_active; // volume has been added to collisions (nbvh x 1) + //-------------------------------- POSITION, VELOCITY dependent // computed by mj_fwdVelocity diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 90c60fc0..5a19b35a 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -56,8 +56,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags mjDSBL_ACTUATION = 1<<10, // apply actuation forces mjDSBL_REFSAFE = 1<<11, // integrator safety: make ref[0]>=2*timestep mjDSBL_SENSOR = 1<<12, // sensors + mjDSBL_MIDPHASE = 1<<13, // mid-phase collision filtering - mjNDISABLE = 13 // number of disable flags + mjNDISABLE = 14 // number of disable flags } mjtDisableBit; @@ -430,6 +431,7 @@ struct mjVisual_ { // visualization options float realtime; // initial real-time factor (1: real time) int offwidth; // width of offscreen buffer int offheight; // height of offscreen buffer + int treedepth; // depth of the bounding volume hierarchy } global; struct { // rendering quality @@ -533,6 +535,7 @@ struct mjModel_ { int nu; // number of actuators/controls = dim(ctrl) int na; // number of activation states = dim(act) int nbody; // number of bodies + int nbvh; // number of total bounding volumes in all bodies int njnt; // number of joints int ngeom; // number of geoms int nsite; // number of sites @@ -635,6 +638,14 @@ struct mjModel_ { mjtNum* body_gravcomp; // antigravity force, units of body weight (nbody x 1) mjtNum* body_user; // user data (nbody x nuser_body) int* body_plugin; // plugin instance id (-1 if not in use) (nbody x 1) + int* body_bvhadr; // address of bvh root (nbody x 1) + int* body_bvhnum; // number of bounding volumes (nbody x 1) + + // bounding volume hierarchy + int* bvh_depth; // depth in the bounding volume hierarchy (nbvh x 1) + int* bvh_child; // left and right children in tree (nbvh x 2) + int* bvh_geomid; // geom id of the node (non-leaf: -1) (nbvh x 1) + mjtNum* bvh_aabb; // bounding box of node (center, size) (nbvh x 6) // joints int* jnt_type; // type of joint (mjtJoint) (njnt x 1) @@ -681,6 +692,7 @@ struct mjModel_ { mjtNum* geom_solref; // constraint solver reference: contact (ngeom x mjNREF) mjtNum* geom_solimp; // constraint solver impedance: contact (ngeom x mjNIMP) mjtNum* geom_size; // geom-specific size parameters (ngeom x 3) + mjtNum* geom_aabb; // bounding box, (center, size) (ngeom x 6) mjtNum* geom_rbound; // radius of bounding sphere (ngeom x 1) mjtNum* geom_pos; // local position offset rel. to body (ngeom x 3) mjtNum* geom_quat; // local orientation offset rel. to body (ngeom x 4) diff --git a/include/mujoco/mjvisualize.h b/include/mujoco/mjvisualize.h index d8f761ca..787c4fee 100644 --- a/include/mujoco/mjvisualize.h +++ b/include/mujoco/mjvisualize.h @@ -120,6 +120,7 @@ typedef enum mjtVisFlag_ { // flags enabling model element visualization mjVIS_SELECT, // selection point mjVIS_STATIC, // static bodies mjVIS_SKIN, // skin + mjVIS_MIDPHASE, // mid-phase bounding volume hierarchy mjNVISFLAG // number of visualization flags } mjtVisFlag; diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 82dfe873..39a81830 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -67,6 +67,7 @@ X( nu ) \ X( na ) \ X( nbody ) \ + X( nbvh ) \ X( njnt ) \ X( ngeom ) \ X( nsite ) \ @@ -176,6 +177,12 @@ X( mjtNum, body_gravcomp, nbody, 1 ) \ X( mjtNum, body_user, nbody, MJ_M(nuser_body) ) \ X( int, body_plugin, nbody, 1 ) \ + X( int, body_bvhadr, nbody, 1 ) \ + X( int, body_bvhnum, nbody, 1 ) \ + X( int, bvh_depth, nbvh, 1 ) \ + X( int, bvh_child, nbvh, 2 ) \ + X( int, bvh_geomid, nbvh, 1 ) \ + X( mjtNum, bvh_aabb, nbvh, 6 ) \ X( int, jnt_type, njnt, 1 ) \ X( int, jnt_qposadr, njnt, 1 ) \ X( int, jnt_dofadr, njnt, 1 ) \ @@ -216,6 +223,7 @@ X( mjtNum, geom_solref, ngeom, mjNREF ) \ X( mjtNum, geom_solimp, ngeom, mjNIMP ) \ X( mjtNum, geom_size, ngeom, 3 ) \ + X( mjtNum, geom_aabb, ngeom, 6 ) \ X( mjtNum, geom_rbound, ngeom, 1 ) \ X( mjtNum, geom_pos, ngeom, 3 ) \ X( mjtNum, geom_quat, ngeom, 4 ) \ @@ -502,6 +510,7 @@ X( mjtNum, qLD, nM, 1 ) \ X( mjtNum, qLDiagInv, nv, 1 ) \ X( mjtNum, qLDiagSqrtInv, nv, 1 ) \ + X( mjtByte, bvh_active, nbvh, 1 ) \ X( mjtNum, ten_velocity, ntendon, 1 ) \ X( mjtNum, actuator_velocity, nu, 1 ) \ X( mjtNum, cvel, nbody, 6 ) \ diff --git a/introspect/enums.py b/introspect/enums.py index eaaf8d65..22f04147 100755 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -40,7 +40,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjDSBL_ACTUATION', 1024), ('mjDSBL_REFSAFE', 2048), ('mjDSBL_SENSOR', 4096), - ('mjNDISABLE', 13), + ('mjDSBL_MIDPHASE', 8192), + ('mjNDISABLE', 14), ]), )), ('mjtEnableBit', @@ -521,7 +522,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjVIS_SELECT', 20), ('mjVIS_STATIC', 21), ('mjVIS_SKIN', 22), - ('mjNVISFLAG', 23), + ('mjVIS_MIDPHASE', 23), + ('mjNVISFLAG', 24), ]), )), ('mjtRndFlag', diff --git a/python/mujoco/structs.cc b/python/mujoco/structs.cc index 5bdc6e16..907d01e8 100644 --- a/python/mujoco/structs.cc +++ b/python/mujoco/structs.cc @@ -1307,6 +1307,7 @@ PYBIND11_MODULE(_structs, m) { X(realtime); X(offwidth); X(offheight); + X(treedepth); #undef X py::class_ mjVisualQuality(mjVisual, "Quality"); diff --git a/simulate/simulate.cc b/simulate/simulate.cc index cfb20801..73bcb40b 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -710,6 +710,12 @@ void makerendering(mj::Simulate* sim, int oldstate) { defFlag[0].pdata = sim->vopt.flags + i; mjui_add(&sim->ui0, defFlag); } + + // create tree slider + sim->m->vis.global.treedepth = 0; + mjuiDef defTree[] = {{mjITEM_SLIDERINT, "Tree depth", 2, &sim->m->vis.global.treedepth, "-1 15"}}; + mjui_add(&sim->ui0, defTree); + mjui_add(&sim->ui0, defOpenGL); for (i=0; i #include -#include #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; kpair_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]; g1body_geomadr[b1]+m->body_geomnum[b1]; g1++) { - for (g2=m->body_geomadr[b2]; g2body_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; kpair_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]; g1body_geomadr[b1]+m->body_geomnum[b1]; g1++) { + for (g2=m->body_geomadr[b2]; g2body_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 diff --git a/src/engine/engine_collision_driver.h b/src/engine/engine_collision_driver.h index 987ee0bf..f6152863 100644 --- a/src/engine/engine_collision_driver.h +++ b/src/engine/engine_collision_driver.h @@ -21,14 +21,28 @@ #ifdef __cplusplus extern "C" { +#else +#include #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); diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index f4f8c79d..11450a5e 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -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); diff --git a/src/engine/engine_io.h b/src/engine/engine_io.h index 0d83e7ab..3fe66c0f 100644 --- a/src/engine/engine_io.h +++ b/src/engine/engine_io.h @@ -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, diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 774d6835..bcc89781 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -53,7 +53,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = { "Filterparent", "Actuation", "Refsafe", - "Sensor" + "Sensor", + "Midphase" }; diff --git a/src/engine/engine_vis_init.c b/src/engine/engine_vis_init.c index e05cd96e..8028cd81 100644 --- a/src/engine/engine_vis_init.c +++ b/src/engine/engine_vis_init.c @@ -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", "`"} }; diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index fe18487e..d3b6df9d 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -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]) { diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 37abc90a..6bcb2e0d 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -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; inbvh; + } + // nmeshvert, nmeshface, nmeshtexcoord, nmeshgraph for (i=0; invert; @@ -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; ibody_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, diff --git a/src/user/user_model.h b/src/user/user_model.h index 5bc1a86f..afe3a08e 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -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 diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 99049f30..c0d041bf 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -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& 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; iconaffinity==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 pos(nelements); + + for (int i=0; ipos[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 left; + std::vector right; + int skipped = 0; + + for (int i=0; ipos[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; imeshes[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)) { diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 573a3324..fdc362a3 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -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&, int lev); // make bounding volume hierarchy + + int nbvh; + std::vector bvh; // bounding volume hierarchy + std::vector child; // children of bvh nodes + std::vector nodeid; // id of the geom contained by the node + std::vector level; // levels of bvh + + // objects allocated by Add functions std::vector bodies; // child bodies std::vector 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 }; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 6f476ecb..5bd0f324 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -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) \ diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 2161d0c2..15d7c0af 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -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) \ diff --git a/test/engine/engine_collision_driver_test.cc b/test/engine/engine_collision_driver_test.cc index 4bba297e..024aa0f0 100644 --- a/test/engine/engine_collision_driver_test.cc +++ b/test/engine/engine_collision_driver_test.cc @@ -23,6 +23,8 @@ #include #include #include "test/fixture.h" +#include "src/engine/engine_collision_driver.h" + namespace mujoco { namespace { @@ -216,5 +218,24 @@ TEST_F(MjCollisionTest, FilterParentDoesntAffectWorldBody) { mj_deleteModel(m); } +TEST_F(MjCollisionTest, TestOBB) { + mjtNum bvh1[6] = {-1, -1, -1, 1, 1, 1}; + mjtNum bvh2[6] = {-1, -1, -1, 1, 1, 1}; + mjtNum pos1[3] = {0, 0, 0}; + mjtNum mat1[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; + mjtNum pos2[3] = {1.71, 1.71, 0}; // just a little more than 1+sqrt(2)/2 + mjtNum mat2[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; + + EXPECT_THAT( + mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, NULL, NULL, 0), true); + + // rotate by 45 degrees + mat2[0] = 1./mju_sqrt(2.); mat2[1] = -1./mju_sqrt(2.); + mat2[3] = 1./mju_sqrt(2.); mat2[4] = 1./mju_sqrt(2.); + + EXPECT_THAT( + mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, NULL, NULL, 0), false); +} + } // namespace } // namespace mujoco diff --git a/test/engine/testdata/collision_driver/humanoid.xml b/test/engine/testdata/collision_driver/humanoid.xml new file mode 100644 index 00000000..324d3fd2 --- /dev/null +++ b/test/engine/testdata/collision_driver/humanoid.xml @@ -0,0 +1,248 @@ + + + + diff --git a/test/engine/testdata/collision_driver/midphase.xml b/test/engine/testdata/collision_driver/midphase.xml new file mode 100644 index 00000000..3939de92 --- /dev/null +++ b/test/engine/testdata/collision_driver/midphase.xml @@ -0,0 +1,1016 @@ + + + + diff --git a/test/engine/testdata/collision_driver/planks.xml b/test/engine/testdata/collision_driver/planks.xml new file mode 100644 index 00000000..fe61f75a --- /dev/null +++ b/test/engine/testdata/collision_driver/planks.xml @@ -0,0 +1,177 @@ + + + + + + + + + + + + +​ + +​ + + + + + +​ + + + + + +​ + + + +​ + + + + + +​ + + + + + + + + + + + + + + + + + + + + + + +​ + + + + + + + + + + + + + + + + + + + + + + +​ + + + + + + + + + + + + + + + + + + + + + + +​ + + + + + + + + + + + + + + + + + + + + + + +​ + + + + + + + + + + + + + + + + + + + + + + +​ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 021bd781..31894b5d 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -146,7 +146,8 @@ public enum mjtDisableBit : int{ mjDSBL_ACTUATION = 1024, mjDSBL_REFSAFE = 2048, mjDSBL_SENSOR = 4096, - mjNDISABLE = 13, + mjDSBL_MIDPHASE = 8192, + mjNDISABLE = 14, } public enum mjtEnableBit : int{ mjENBL_OVERRIDE = 1, @@ -485,7 +486,8 @@ public enum mjtVisFlag : int{ mjVIS_SELECT = 20, mjVIS_STATIC = 21, mjVIS_SKIN = 22, - mjNVISFLAG = 23, + mjVIS_MIDPHASE = 23, + mjNVISFLAG = 24, } public enum mjtRndFlag : int{ mjRND_SHADOW = 0, @@ -1644,6 +1646,7 @@ public unsafe struct mjData_ { public double* qLD; public double* qLDiagInv; public double* qLDiagSqrtInv; + public byte* bvh_active; public double* ten_velocity; public double* actuator_velocity; public double* cvel; @@ -1764,6 +1767,7 @@ public unsafe struct global { public float realtime; public int offwidth; public int offheight; + public int treedepth; } [StructLayout(LayoutKind.Sequential)] @@ -1872,6 +1876,7 @@ public unsafe struct mjModel_ { public int nu; public int na; public int nbody; + public int nbvh; public int njnt; public int ngeom; public int nsite; @@ -1960,6 +1965,12 @@ public unsafe struct mjModel_ { public double* body_gravcomp; public double* body_user; public int* body_plugin; + public int* body_bvhadr; + public int* body_bvhnum; + public int* bvh_depth; + public int* bvh_child; + public int* bvh_geomid; + public double* bvh_aabb; public int* jnt_type; public int* jnt_qposadr; public int* jnt_dofadr; @@ -2000,6 +2011,7 @@ public unsafe struct mjModel_ { public double* geom_solref; public double* geom_solimp; public double* geom_size; + public double* geom_aabb; public double* geom_rbound; public double* geom_pos; public double* geom_quat; @@ -2558,7 +2570,7 @@ public unsafe struct mjvOption_ { public fixed byte tendongroup[6]; public fixed byte actuatorgroup[6]; public fixed byte skingroup[6]; - public fixed byte flags[23]; + public fixed byte flags[24]; } [StructLayout(LayoutKind.Sequential)]