Save normalising transform applied to mesh assets in mjModel.mesh_{pos,quat}

Fixes #409

PiperOrigin-RevId: 564217402
Change-Id: I53ddeef4e343905021cd7852d548f6769c876474

src/user/user_mesh.cc

@@ -134,6 +134,9 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
   mjuu_setvec(pos_volume_, 0, 0, 0);
   mjuu_setvec(quat_surface_, 1, 0, 0, 0);
   mjuu_setvec(quat_volume_, 1, 0, 0, 0);
+  mjuu_setvec(pos_, 0, 0, 0);
+  mjuu_setvec(quat_, 1, 0, 0, 0);
+
   mjuu_setvec(boxsz_surface_, 0, 0, 0);
   mjuu_setvec(boxsz_volume_, 0, 0, 0);
   mjuu_setvec(aabb_, 1e10, 1e10, 1e10);
@@ -630,6 +633,18 @@ double* mjCMesh::GetQuatPtr(mjtMeshType type) {
 
 
 
+double* mjCMesh::GetOffsetPosPtr() {
+  return pos_;
+}
+
+
+
+double* mjCMesh::GetOffsetQuatPtr() {
+  return quat_;
+}
+
+
+
 bool mjCMesh::HasTexcoord() const {
   return texcoord_ != nullptr;
 }

src/user/user_model.cc

@@ -1737,6 +1737,8 @@ void mjCModel::CopyObjects(mjModel* m) {
     m->mesh_graphadr[i] = (pme->szgraph() ? graph_adr : -1);
     m->mesh_bvhadr[i] = bvh_adr;
     m->mesh_bvhnum[i] = pme->tree().nbvh;
+    copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3);
+    copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4);
 
     // copy vertices, normals, faces, texcoords, aux data
     pme->CopyVert(m->mesh_vert + 3*vert_adr);
@@ -3015,6 +3017,15 @@ bool mjCModel::CopyBack(const mjModel* m) {
     }
   }
 
+  // mesh
+  mjCMesh* pm;
+  for (int i=0; i<nmesh; i++) {
+    pm = meshes[i];
+
+    copyvec(pm->GetOffsetPosPtr(), m->mesh_pos+3*i, 3);
+    copyvec(pm->GetOffsetQuatPtr(), m->mesh_quat+4*i, 4);
+  }
+
   // sites
   for (int i=0; i<nsite; i++) {
     copyvec(sites[i]->size, m->site_size + 3 * i, 3);

src/user/user_objects.cc

@@ -1747,6 +1747,8 @@ void mjCGeom::Compile(void) {
 
     // apply geom pos/quat as offset
     mjuu_frameaccum(pos, quat, meshpos, pmesh->GetQuatPtr(typeinertia));
+    mjuu_copyvec(pmesh->GetOffsetPosPtr(), meshpos, 3);
+    mjuu_copyvec(pmesh->GetOffsetQuatPtr(), pmesh->GetQuatPtr(typeinertia), 4);
   }
 
   // check size parameters

src/user/user_objects.h

@@ -576,6 +576,8 @@ class mjCMesh: public mjCBase {
   void Compile(const mjVFS* vfs);                   // compiler
   double* GetPosPtr(mjtMeshType type);              // get position
   double* GetQuatPtr(mjtMeshType type);             // get orientation
+  double* GetOffsetPosPtr();                        // get position offset for geom
+  double* GetOffsetQuatPtr();                       // get orientation offset for geom
   double* GetInertiaBoxPtr(mjtMeshType type);       // get inertia box
   double& GetVolumeRef(mjtMeshType type);           // get volume
   void FitGeom(mjCGeom* geom, double* meshpos);     // approximate mesh with simple geom
@@ -639,6 +641,8 @@ class mjCMesh: public mjCBase {
   double pos_surface_[3];             // CoM position
   double quat_volume_[4];             // inertia orientation
   double quat_surface_[4];            // inertia orientation
+  double pos_[3];                     // translation applied to asset vertices
+  double quat_[4];                    // rotation applied to asset vertices
   double boxsz_volume_[3];            // half-sizes of equivalent inertia box (volume)
   double boxsz_surface_[3];           // half-sizes of equivalent inertia box (surface)
   double aabb_[6];                    // axis-aligned bounding box