diff --git a/src/user/user_model.cc b/src/user/user_model.cc
index 588a514e..32cd7027 100644
--- a/src/user/user_model.cc
+++ b/src/user/user_model.cc
@@ -4138,6 +4138,8 @@ void mjCModel::FuseReindex(mjCBody* body) {
makelistid(joints_, body->joints);
makelistid(geoms_, body->geoms);
makelistid(sites_, body->sites);
+ makelistid(cameras_, body->cameras);
+ makelistid(lights_, body->lights);
// process children recursively
for (int i=0; i < body->bodies.size(); i++) {
@@ -4265,10 +4267,25 @@ void mjCModel::FuseStatic(void) {
mju_error("Internal error: FuseStatic: body not found");
}
- //------------- assign geoms and sites to parent, change frames
+ //------------- assign geoms, sites, cameras, lights to parent, change frames
ReassignChild(par->geoms, body->geoms, par, body);
ReassignChild(par->sites, body->sites, par, body);
+ ReassignChild(par->cameras, body->cameras, par, body);
+
+ // lights have dir instead of quat, so handle separately
+ for (int j=0; j < body->lights.size(); j++) {
+ body->lights[j]->body = par;
+ par->lights.push_back(body->lights[j]);
+
+ // transform pos into parent frame
+ double qunit[4] = {1, 0, 0, 0};
+ changeframe(body->lights[j]->pos, qunit, body->pos, body->quat);
+
+ // rotate dir into parent frame
+ mjuu_rotVecQuat(body->lights[j]->dir, body->lights[j]->dir, body->quat);
+ }
+ body->lights.clear();
//------------- remove from global body list, reduce global counts
@@ -4300,6 +4317,8 @@ void mjCModel::FuseStatic(void) {
joints_.clear();
geoms_.clear();
sites_.clear();
+ cameras_.clear();
+ lights_.clear();
FuseReindex(bodies_[0]);
// recompute parent contype, conaffinity, and margin
diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc
index 7deb8894..02d3598e 100644
--- a/test/user/user_model_test.cc
+++ b/test/user/user_model_test.cc
@@ -668,6 +668,53 @@ TEST_F(FuseStaticTest, FuseStaticForceSensorReferencedBody) {
mj_deleteModel(m);
}
+TEST_F(FuseStaticTest, FuseStaticCameraInBody) {
+ static constexpr char xml[] = R"(
+
+
+
+
+
+
+
+
+
+
+
+
+
+ )";
+ std::array error;
+ mjModel* m = LoadModelFromString(xml, error.data(), error.size());
+ ASSERT_THAT(m, NotNull()) << error.data();
+ EXPECT_EQ(m->nbody, 2) << "Static body should be fused";
+ EXPECT_EQ(m->ncam, 1);
+ mj_deleteModel(m);
+}
+
+TEST_F(FuseStaticTest, FuseStaticLightInBody) {
+ static constexpr char xml[] = R"(
+
+
+
+
+
+
+
+
+
+
+
+
+ )";
+ std::array error;
+ mjModel* m = LoadModelFromString(xml, error.data(), error.size());
+ ASSERT_THAT(m, NotNull()) << error.data();
+ EXPECT_EQ(m->nbody, 2) << "Static body should be fused";
+ EXPECT_EQ(m->nlight, 1);
+ mj_deleteModel(m);
+}
+
// ------------- test discardvisual --------------------------------------------
using DiscardVisualTest = MujocoTest;