Add timing of collision phases.

PiperOrigin-RevId: 572273621
Change-Id: Ib9440a996a05e42e00dcf4f1ce487a12f65e999b

doc/includes/references.h

@@ -73,6 +73,11 @@ typedef enum mjtTimer_ {     // internal timers
   mjTIMER_POS_MAKE,          // make constraints
   mjTIMER_POS_PROJECT,       // project constraints
 
+  // breakdown of mj_collision
+  mjTIMER_COL_BROAD,         // broadphase
+  mjTIMER_COL_MID,           // midphase
+  mjTIMER_COL_NARROW,        // narrowphase
+
   mjNTIMER                   // number of timers
 } mjtTimer;
 struct mjContact_ {                // result of collision detection functions

include/mujoco/mjdata.h

@@ -85,6 +85,11 @@ typedef enum mjtTimer_ {     // internal timers
   mjTIMER_POS_MAKE,          // make constraints
   mjTIMER_POS_PROJECT,       // project constraints
 
+  // breakdown of mj_collision
+  mjTIMER_COL_BROAD,         // broadphase
+  mjTIMER_COL_MID,           // midphase
+  mjTIMER_COL_NARROW,        // narrowphase
+
   mjNTIMER                   // number of timers
 } mjtTimer;
 

introspect/enums.py

@@ -440,7 +440,10 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjTIMER_POS_COLLISION', 10),
              ('mjTIMER_POS_MAKE', 11),
              ('mjTIMER_POS_PROJECT', 12),
-             ('mjNTIMER', 13),
+             ('mjTIMER_COL_BROAD', 13),
+             ('mjTIMER_COL_MID', 14),
+             ('mjTIMER_COL_NARROW', 15),
+             ('mjNTIMER', 16),
          ]),
      )),
     ('mjtCatBit',

introspect/structs.py

@@ -3647,7 +3647,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  name='timer',
                  type=ArrayType(
                      inner_type=ValueType(name='mjTimerStat'),
-                     extents=(13,),
+                     extents=(16,),
                  ),
                  doc='timer statistics',
              ),

src/engine/engine_collision_driver.c

@@ -388,7 +388,7 @@ quicksortfunc(contactcompare, context, el1, el2) {
 }
 
 void mj_collision(const mjModel* m, mjData* d) {
-  TM_START;
+  TM_START1;
   int g1, g2, merged, b1 = 0, b2 = 0, exadr = 0, pairadr = 0, startadr;
   int nexclude = m->nexclude, npair = m->npair;
   int *broadphasepair = 0;
@@ -415,11 +415,19 @@ void mj_collision(const mjModel* m, mjData* d) {
 
   mj_markStack(d);
 
-  // call broadphase collision detector
+  // broadphase collision detector
+  TM_START;
   int nbodypair = (m->nbody*(m->nbody - 1))/2;
   broadphasepair = mj_stackAllocInt(d, nbodypair);
   nbodypair = mj_broadphase(m, d, broadphasepair, nbodypair);
   unsigned int last_signature = -1;
+  TM_END(mjTIMER_COL_BROAD);
+
+  // midphase collision detector
+  TM_RESTART;
+
+  // save current narrowphase duration
+  mjtNum tmNarrow = d->timer[mjTIMER_COL_NARROW].duration;
 
   // loop over body pairs
   for (int i=0; i < nbodypair; i++) {
@@ -493,8 +501,17 @@ void mj_collision(const mjModel* m, mjData* d) {
     }
   }
 
+  // end midphase timer
+  TM_END(mjTIMER_COL_MID);
+
+  // subtract nested narrowphase timing from midphase timer
+  d->timer[mjTIMER_COL_MID].duration -= (d->timer[mjTIMER_COL_NARROW].duration - tmNarrow);
+
+  // increment narrowphase counter
+  d->timer[mjTIMER_COL_NARROW].number++;
+
   mj_freeStack(d);
-  TM_END(mjTIMER_POS_COLLISION);
+  TM_END1(mjTIMER_POS_COLLISION);
 }
 
 
@@ -863,6 +880,7 @@ endbroad:
 //  flg_user disables filters and uses usermargin
 static void collideGeoms(const mjModel* m, mjData* d,
                          int g1, int g2, int flg_user, mjtNum usermargin) {
+  TM_START;
   int num, type1, type2, condim;
   mjtNum margin, gap, mix, friction[5], solref[mjNREF], solimp[mjNIMP];
   mjtNum solreffriction[mjNREF] = {0};
@@ -1127,6 +1145,9 @@ static void collideGeoms(const mjModel* m, mjData* d,
 
   // move arena pointer back to the end of the contact array
   resetArena(d);
+
+  // add duration without incrementing counter
+  TM_ADD(mjTIMER_COL_NARROW);
 }
 
 

src/engine/engine_support.c

@@ -87,7 +87,10 @@ const char* mjTIMERSTRING[mjNTIMER]= {
   "pos_inertia",
   "pos_collision",
   "pos_make",
-  "pos_project"
+  "pos_project",
+  "col_broadphase",
+  "col_midphase",
+  "col_narrowphase"
 };
 
 

unity/Runtime/Bindings/MjBindings.cs

@@ -137,7 +137,10 @@ public enum mjtTimer : int{
   mjTIMER_POS_COLLISION = 10,
   mjTIMER_POS_MAKE = 11,
   mjTIMER_POS_PROJECT = 12,
-  mjNTIMER = 13,
+  mjTIMER_COL_BROAD = 13,
+  mjTIMER_COL_MID = 14,
+  mjTIMER_COL_NARROW = 15,
+  mjNTIMER = 16,
 }
 public enum mjtDisableBit : int{
   mjDSBL_CONSTRAINT = 1,
@@ -738,6 +741,9 @@ public unsafe struct mjData_ {
   public mjTimerStat_ timer10;
   public mjTimerStat_ timer11;
   public mjTimerStat_ timer12;
+  public mjTimerStat_ timer13;
+  public mjTimerStat_ timer14;
+  public mjTimerStat_ timer15;
   public mjSolverStat_ solver0;
   public mjSolverStat_ solver1;
   public mjSolverStat_ solver2;