Add copy constructor to mjCBody.

PiperOrigin-RevId: 615050891
Change-Id: I420e19a0444e93e5ec99da10f5cc20b429802d3e

src/user/user_objects.cc

@@ -614,6 +614,12 @@ mjCBase::mjCBase(const mjCBase& other) {
 mjCBase& mjCBase::operator=(const mjCBase& other) {
   if (this != &other) {
     *static_cast<mjCBase_*>(this) = static_cast<const mjCBase_&>(other);
+    if (other.def) {
+      def = new mjCDef(*other.def);
+    }
+    if (other.frame) {
+      frame = new mjCFrame(*other.frame);
+    }
   }
   return *this;
 }
@@ -700,6 +706,60 @@ mjCBody::mjCBody(mjCModel* _model) {
 }
 
 
+
+mjCBody::mjCBody(const mjCBody& other) {
+  *this = other;
+}
+
+
+
+mjCBody& mjCBody::operator=(const mjCBody& other) {
+  if (this != &other) {
+    this->spec = other.spec;
+    *static_cast<mjCBody_*>(this) = static_cast<const mjCBody_&>(other);
+    *static_cast<mjmBody*>(this) = static_cast<const mjmBody&>(other);
+    this->bodies.clear();
+    this->frames.clear();
+    this->geoms.clear();
+    this->joints.clear();
+    this->sites.clear();
+    this->cameras.clear();
+    this->lights.clear();
+
+    // copy all children
+    for (int i=0; i<other.bodies.size(); i++) {
+      this->bodies.push_back(new mjCBody(*other.bodies[i]));  // triggers recursive call
+    }
+    for (int i=0; i<other.frames.size(); i++) {
+      this->frames.push_back(new mjCFrame(*other.frames[i]));
+    }
+    for (int i=0; i<other.geoms.size(); i++) {
+      this->geoms.push_back(new mjCGeom(*other.geoms[i]));
+      this->geoms.back()->body = this;
+    }
+    for (int i=0; i<other.joints.size(); i++) {
+      this->joints.push_back(new mjCJoint(*other.joints[i]));
+      this->joints.back()->body = this;
+    }
+    for (int i=0; i<other.sites.size(); i++) {
+      this->sites.push_back(new mjCSite(*other.sites[i]));
+      this->sites.back()->body = this;
+    }
+    for (int i=0; i<other.cameras.size(); i++) {
+      this->cameras.push_back(new mjCCamera(*other.cameras[i]));
+      this->cameras.back()->body = this;
+    }
+    for (int i=0; i<other.lights.size(); i++) {
+      this->lights.push_back(new mjCLight(*other.lights[i]));
+      this->lights.back()->body = this;
+    }
+  }
+  PointToLocal();
+  return *this;
+}
+
+
+
 void mjCBody::PointToLocal() {
   spec.element = (mjElement)this;
   spec.name = (mjString)&name;
@@ -1293,6 +1353,24 @@ mjCFrame::mjCFrame(mjCModel* _model, mjCFrame* _frame) {
 
 
 
+mjCFrame::mjCFrame(const mjCFrame& other) {
+  *this = other;
+}
+
+
+
+mjCFrame& mjCFrame::operator=(const mjCFrame& other) {
+  if (this != &other) {
+    this->spec = other.spec;
+    *static_cast<mjCFrame_*>(this) = static_cast<const mjCFrame_&>(other);
+    *static_cast<mjmFrame*>(this) = static_cast<const mjmFrame&>(other);
+  }
+  PointToLocal();
+  return *this;
+}
+
+
+
 void mjCFrame::PointToLocal() {
   spec.element = (mjElement)this;
   spec.info = (mjString)&info;

src/user/user_objects.h

@@ -158,9 +158,7 @@ class mjCBase_ {
   std::string classname;          // defaults class name
   int id;                         // object id
   std::string info;               // error message info set by the user
-  mjCDef* def;                    // defaults class used to init this object
   mjCModel* model;                // pointer to model that created object
-  mjCFrame* frame;                // pointer to frame transformation
 };
 
 class mjCBase : public mjCBase_ {
@@ -183,6 +181,9 @@ class mjCBase : public mjCBase_ {
   // Copy assignment
   mjCBase& operator=(const mjCBase& other);
 
+  mjCDef* def;                    // defaults class used to init this object
+  mjCFrame* frame;                // pointer to frame transformation
+
  protected:
   mjCBase();                                 // constructor
   mjCBase(const mjCBase& other);             // copy constructor
@@ -276,8 +277,11 @@ class mjCBody : public mjCBody_, private mjmBody {
   const std::vector<double>& get_userdata() { return userdata_; }
 
  private:
-  mjCBody(mjCModel*);             // constructor
-  ~mjCBody();                     // destructor
+  mjCBody(mjCModel*);                        // constructor
+  mjCBody(const mjCBody& other);             // copy constructor
+  mjCBody& operator=(const mjCBody& other);  // copy assignment
+  ~mjCBody();                                // destructor
+
   void Compile(void);             // compiler
   void GeomFrame(void);           // get inertial info from geoms
 
@@ -323,8 +327,10 @@ class mjCFrame : public mjCFrame_, private mjmFrame {
   void PointToLocal(void);
 
  private:
-  mjCFrame(mjCModel* = 0, mjCFrame* = 0);  // constructor
-  void Compile(void);                      // compiler
+  mjCFrame(mjCModel* = 0, mjCFrame* = 0);      // constructor
+  mjCFrame(const mjCFrame& other);             // copy constructor
+  mjCFrame& operator=(const mjCFrame& other);  // copy assignment
+  void Compile(void);                          // compiler
 };