diff --git a/doc/changelog.rst b/doc/changelog.rst index bf37db43..c6473e62 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,6 +2,15 @@ Changelog ========= +Upcoming version (not yet released) +----------------------------------- + +Bug fixes +^^^^^^^^^ + +1. Fixed a bug the could cause collisions to be missed when :ref:`fusestatic` is enabled, as is + often the case for URDF imports. + Version 3.1.5 (May 7, 2024) --------------------------- diff --git a/src/user/user_model.cc b/src/user/user_model.cc index ec58d4c8..cf525376 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2969,12 +2969,18 @@ void mjCModel::FuseStatic(void) { // recompute parent contype, conaffinity, and margin par->contype = par->conaffinity = 0; par->margin = 0; - for (const auto& geom : geoms) { + for (const auto& geom : par->geoms) { par->contype |= geom->contype; par->conaffinity |= geom->conaffinity; par->margin = mju_max(par->margin, geom->margin); } + // recompute BVH + int nbvhfuse = body->tree.nbvh + par->tree.nbvh; + par->ComputeBVH(); + nbvhstatic += par->tree.nbvh - nbvhfuse; + nbvh += par->tree.nbvh - nbvhfuse; + //------------- delete body (without deleting children) // delete allocation diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index db747ab3..438b5e7b 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -399,6 +399,7 @@ void mjCBoundingVolumeHierarchy::Set(mjtNum ipos_element[3], mjtNum iquat_elemen void mjCBoundingVolumeHierarchy::AllocateBoundingVolumes(int nleaf) { nbvh = 0; + bvh.clear(); child.clear(); nodeid.clear(); level.clear(); @@ -1333,6 +1334,23 @@ void mjCBody::MakeInertialExplicit() { } + +// compute bounding volume hierarchy +void mjCBody::ComputeBVH() { + if (geoms.empty()) { + return; + } + + tree.Set(ipos, iquat); + tree.AllocateBoundingVolumes(geoms.size()); + for (int i=0; iSetBoundingVolume(tree.GetBoundingVolume(i)); + } + tree.CreateBVH(); +} + + + // compiler void mjCBody::Compile(void) { CopyFromSpec(); @@ -1447,14 +1465,7 @@ void mjCBody::Compile(void) { } // compute bounding volume hierarchy - if (!geoms.empty()) { - tree.Set(ipos, iquat); - tree.AllocateBoundingVolumes(geoms.size()); - for (int i=0; iSetBoundingVolume(tree.GetBoundingVolume(i)); - } - tree.CreateBVH(); - } + ComputeBVH(); // compile all joints, count dofs dofnum = 0; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 068b1502..644f8898 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -293,6 +293,9 @@ class mjCBody : public mjCBody_, private mjsBody { // set explicitinertial to true void MakeInertialExplicit(); + // compute the bounding volume hierarchy of the body. + void ComputeBVH(); + // variables set by user mjsBody spec; diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index d50b123e..22b46ec0 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -251,6 +251,7 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) { + @@ -273,6 +274,9 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) { EXPECT_EQ(m_fuse->body_contype[1], 1); EXPECT_EQ(m_fuse->body_conaffinity[1], 1); + EXPECT_EQ(m_no_fuse->body_bvhnum[2], 3); + EXPECT_EQ(m_fuse->body_bvhnum[1], 3); + mjData* d_fuse = mj_makeData(m_fuse); mjData* d_no_fuse = mj_makeData(m_no_fuse);