diff --git a/doc/includes/references.h b/doc/includes/references.h index 8133a71e..dd9a261f 100644 --- a/doc/includes/references.h +++ b/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 diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 73ad99fa..82400cf5 100644 --- a/include/mujoco/mjdata.h +++ b/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; diff --git a/introspect/enums.py b/introspect/enums.py index abbe45d0..890bcdf1 100644 --- a/introspect/enums.py +++ b/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', diff --git a/introspect/structs.py b/introspect/structs.py index e85ee1f3..de0edd9b 100644 --- a/introspect/structs.py +++ b/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', ), diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index f656efe2..7e42f406 100644 --- a/src/engine/engine_collision_driver.c +++ b/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); } diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index b89aebc9..ce48d6e8 100644 --- a/src/engine/engine_support.c +++ b/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" }; diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 718c5167..2debba82 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/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;