Update BVH during fusestatic. Fixes #1069 and #1577.

PiperOrigin-RevId: 631748750
Change-Id: I4e1ae01207568c90fc49bcd7d9f760f5187f1afa

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<compiler-fusestatic>` is enabled, as is
+   often the case for URDF imports.
+
 Version 3.1.5 (May 7, 2024)
 ---------------------------
 

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

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; i<geoms.size(); i++) {
+    geoms[i]->SetBoundingVolume(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()) {
+  ComputeBVH();
-    tree.Set(ipos, iquat);
-    tree.AllocateBoundingVolumes(geoms.size());
-    for (int i=0; i<geoms.size(); i++) {
-      geoms[i]->SetBoundingVolume(tree.GetBoundingVolume(i));
-    }
-    tree.CreateBVH();
-  }
 
   // compile all joints, count dofs
   dofnum = 0;

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;
 

test/user/user_model_test.cc

@@ -251,6 +251,7 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) {
         <geom size="0.5" pos="1 0 0" contype="0" conaffinity="0"/>
         <body>
           <geom size="0.5" pos="0 1 0" contype="1" conaffinity="1"/>
+          <geom size="0.5" pos="0 -2 0" contype="1" conaffinity="1"/>
         </body>
       </body>
     </worldbody>
@@ -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);