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

PiperOrigin-RevId: 517357695
Change-Id: I068fee714d9638be2fb6d042d5d86f8ff970635b

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:
 

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<coSelection>` 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

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 <contact-exclude>` element
    in MJCF. Exclusion is applied when "dynamic" or "all" are selected, but not when "pair" is selected. At the end of

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;