diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 3c5610d4..2c368696 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -51,7 +51,7 @@ mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = { //----------------------------- collision detection entry point ------------------------------------ void mj_collision(const mjModel* m, mjData* d) { - int g1, g2, signature, merged, b1 = 0, b2 = 0, exadr = 0, pairadr = 0, startadr; + 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; int *broadphasepair = 0; mjMARKSTACK; @@ -77,6 +77,7 @@ void mj_collision(const mjModel* m, mjData* d) { // call broadphase collision detector broadphasepair = (int*)mj_stackAlloc(d, (m->nbody*(m->nbody-1))/2); nbodypair = mj_broadphase(m, d, broadphasepair, (m->nbody*(m->nbody-1))/2); + unsigned int last_signature = -1; // loop over body pairs (broadphase or all) for (int i=0; i; using ::testing::IsEmpty; using ::testing::ElementsAre; +using ::testing::NotNull; // Returns a sorted list of pairs of colliding geom names, where each pair of // geom names is sorted. @@ -108,5 +109,41 @@ TEST_F(MjCollisionTest, ZeroedHessian) { mj_deleteData(data); mj_deleteModel(model); } + +TEST_F(MjCollisionTest, ContactCount) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + )"; + mjModel* m = LoadModelFromString(xml); + ASSERT_THAT(m, NotNull()); + mjData* d = mj_makeData(m); + ASSERT_THAT(d, NotNull()); + + mj_forward(m, d); + + // there are 8 spheres, all touching the floor + EXPECT_EQ(d->ncon, 8); + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco