From dc3a900809f0cc0e1650f765d4e3ba8d2048dd9a Mon Sep 17 00:00:00 2001 From: Nimrod Gileadi Date: Mon, 6 Feb 2023 03:45:15 -0800 Subject: [PATCH] Apply body pair filtering at broadphase. mj_contactFilter removed geom pairs where their bodies share the same weldid, but this check can be applied at broadphase instead of at geom-geom level. PiperOrigin-RevId: 507435848 Change-Id: Ia37c741071dc5e1c26686dd319f42c9bd334d305 --- src/engine/engine_collision_driver.c | 74 ++++++++++++--------- src/engine/engine_collision_driver.h | 5 +- test/engine/engine_collision_driver_test.cc | 71 ++++++++++++++++++++ 3 files changed, 114 insertions(+), 36 deletions(-) diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 2c368696..3ed98526 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -227,7 +227,23 @@ static int has_plane_or_hfield(const mjModel* m, int body) { return 0; } +// filter body pair: 1- discard, 0- proceed +static int body_pair_filter(int weldbody1, int weldparent1, int weldbody2, + int weldparent2, int dsbl_filterparent) { + // same weldbody check + if (weldbody1==weldbody2) { + return 1; + } + // weldparent check + if ((!dsbl_filterparent && weldbody1 != 0 && weldbody2 != 0) && + (weldbody1 == weldparent2 || weldbody2 == weldparent1)) { + return 1; + } + + // all tests passed + return 0; +} // add body pair in buffer static void add_pair(const mjModel* m, int b1, int b2, int* npair, int* pair, int maxpair) { @@ -320,6 +336,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) { mjtNum *aabb; mjMARKSTACK; + int dsbl_filterparent = mjDISABLED(mjDSBL_FILTERPARENT); // world with geoms, and body with plane or hfield, can collide all bodies for (b1=0; b1body_geomnum[b1]>0) || (m->body_weldid[b1]==0 && has_plane_or_hfield(m, b1))) { + int weld1 = 0; + int parent_weld1 = 0; for (b2=0; b2body_weldid[b2]; + int parent_weld2 = m->body_weldid[m->body_parentid[weld2]]; + if (!body_pair_filter(weld1, parent_weld1, weld2, parent_weld2, + dsbl_filterparent)) { add_pair(NULL, b1, b2, &npair, pair, maxpair); } } @@ -438,10 +460,21 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) { // min value: collide with all in list, add if (!(sortbuf[i].body_ismax & 0x10000)) { for (j=0; jbody_weldid[b1]; + int weld2 = m->body_weldid[b2]; + int parent_weld1 = m->body_weldid[m->body_parentid[weld1]]; + int parent_weld2 = m->body_weldid[m->body_parentid[weld2]]; + + if (body_pair_filter(weld1, parent_weld1, weld2, parent_weld2, + dsbl_filterparent)) { + continue; + } + // use the other two axes to prune if possible if (aabb[6*b1+2] > aabb[6*b2+3] || aabb[6*b1+3] < aabb[6*b2+2] || @@ -509,7 +542,7 @@ static inline mjtNum squaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) { // test two geoms for collision, apply filters, add to contact list // flg_user disables filters and uses usermargin void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, mjtNum usermargin) { - int i, num, type1, type2, b1, b2, weld1, weld2, condim; + int i, num, type1, type2, condim; mjtNum margin, gap, mix, friction[5], solref[mjNREF], solimp[mjNIMP]; mjContact con[mjMAXCONPAIR]; int ipair = (g2<0 ? g1 : -1); @@ -530,10 +563,6 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, // copy types and bodies type1 = m->geom_type[g1]; type2 = m->geom_type[g2]; - b1 = m->geom_bodyid[g1]; - b2 = m->geom_bodyid[g2]; - weld1 = m->body_weldid[b1]; - weld2 = m->body_weldid[b2]; // return if no collision function if (!mjCOLLISIONFUNC[type1][type2]) { @@ -550,12 +579,8 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, } // otherwise built-in filter - else if (mj_contactFilter( - type1, m->geom_contype[g1], m->geom_conaffinity[g1], - weld1, m->body_weldid[m->body_parentid[weld1]], - type2, m->geom_contype[g2], m->geom_conaffinity[g2], - weld2, m->body_weldid[m->body_parentid[weld2]], - !mjDISABLED(mjDSBL_FILTERPARENT) && weld1 && weld2)) { + else if (mj_contactFilter(m->geom_contype[g1], m->geom_conaffinity[g1], + m->geom_contype[g2], m->geom_conaffinity[g2])) { return; } } @@ -775,24 +800,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, // filter contacts: 1- discard, 0- proceed -int mj_contactFilter(int type1, int contype1, int conaffinity1, int weldbody1, int weldparent1, - int type2, int contype2, int conaffinity2, int weldbody2, int weldparent2, - int filterparent) { - // compatibility check - if (!(contype1 & conaffinity2) && !(contype2 & conaffinity1)) { - return 1; - } - - // same weldbody check - if (weldbody1==weldbody2) { - return 1; - } - - // weldparent check - if (filterparent && (weldbody1==weldparent2 || weldbody2==weldparent1)) { - return 1; - } - - // all tests passed - return 0; +int mj_contactFilter(int contype1, int conaffinity1, + int contype2, int conaffinity2) { + return !(contype1 & conaffinity2) && !(contype2 & conaffinity1); } diff --git a/src/engine/engine_collision_driver.h b/src/engine/engine_collision_driver.h index dc2f410e..987ee0bf 100644 --- a/src/engine/engine_collision_driver.h +++ b/src/engine/engine_collision_driver.h @@ -38,9 +38,8 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, mjtNum usermargin); // number of possible collisions based on fitlers and geom types -int mj_contactFilter(int type1, int contype1, int conaffinity1, int weldbody1, int weldparent1, - int type2, int contype2, int conaffinity2, int weldbody2, int weldparent2, - int filterparent); +int mj_contactFilter(int contype1, int conaffinity1, + int contype2, int conaffinity2); #ifdef __cplusplus } diff --git a/test/engine/engine_collision_driver_test.cc b/test/engine/engine_collision_driver_test.cc index 5b5c920e..4bba297e 100644 --- a/test/engine/engine_collision_driver_test.cc +++ b/test/engine/engine_collision_driver_test.cc @@ -145,5 +145,76 @@ TEST_F(MjCollisionTest, ContactCount) { mj_deleteModel(m); } +TEST_F(MjCollisionTest, FilterParent) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + )"; + mjModel* m = LoadModelFromString(xml); + ASSERT_THAT(m, NotNull()); + mjData* d = mj_makeData(m); + ASSERT_THAT(d, NotNull()); + + mj_fwdPosition(m, d); + + // there should be zero contacts, because colliding1 and colliding2 are in + // bodies that have a parent-child relationship, through welds + EXPECT_EQ(d->ncon, 0); + + // when this filtering is disabled, the geoms should collide + m->opt.disableflags |= mjDSBL_FILTERPARENT; + mj_fwdPosition(m, d); + + EXPECT_THAT(colliding_pairs(m, d), + ElementsAre(GeomPair("colliding1", "colliding2"))); + + mj_deleteData(d); + mj_deleteModel(m); +} + +TEST_F(MjCollisionTest, FilterParentDoesntAffectWorldBody) { + constexpr char xml[] = R"( + + + + + + + + + + )"; + mjModel* m = LoadModelFromString(xml); + ASSERT_THAT(m, NotNull()); + mjData* d = mj_makeData(m); + ASSERT_THAT(d, NotNull()); + + mj_fwdPosition(m, d); + + // even though colliding1 and colliding2 are have a parent-child relationship, + // they collide because colliding1 is in + EXPECT_THAT(colliding_pairs(m, d), + ElementsAre(GeomPair("colliding1", "colliding2"))); + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco