Improve mesh processing readability

PiperOrigin-RevId: 721524528
Change-Id: I3af4e25815cbc7517b772826af7d929ea93f8164

src/user/user_mesh.cc

@@ -139,7 +139,7 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
   invalidorientation_.first = -1;
   invalidorientation_.second = -1;
   validarea_ = true;
-  validvolume_ = 1;
+  validvolume_ = MeshVolumeOK;
   valideigenvalue_ = true;
   validinequality_ = true;
   processed_ = false;
@@ -1401,10 +1401,7 @@ void mjCMesh::ComputeFaceCentroid(double facecen[3]) {
 
 
 void mjCMesh::Process() {
-  double facecen[3] = {0, 0, 0};
+  double facecen[3] = {0, 0, 0};;
-  double nrm[3];
-  double cen[3];
-
   // user offset, rotation, scaling
   ApplyTransformations();
 
@@ -1413,7 +1410,13 @@ void mjCMesh::Process() {
 
   double density = model->def_map[classname]->Geom().density;
 
+  bool centered = false;                      // true if the mesh is centered at the CoM
+  bool aligned_with_inertial_frame = false;   // true if mesh is aligned with inertial frame
+
   // compute inertial properties for both inertia types
+  // the mesh is transformed such that it is centered at the CoM and the axes
+  // are the principle axes of inertia. If the volume is valid, we use the volume
+  // inertia for this transformation, otherwise we use the shell inertia.
   for ( const auto type : { mjtGeomInertia::mjINERTIA_VOLUME, mjtGeomInertia::mjINERTIA_SHELL } ) {
     double CoM[3] = {0, 0, 0};
     double inert[6] = {0, 0, 0, 0, 0, 0};
@@ -1426,7 +1429,7 @@ void mjCMesh::Process() {
       if (type == mjINERTIA_SHELL) {
         validarea_ = 0;
       } else {
-        validvolume_ = GetVolumeRef(type) < 0 ? -1 : 0;
+        validvolume_ = GetVolumeRef(type) < 0 ? MeshNegativeVolume : MeshZeroVolume;
       }
       continue;
     }
@@ -1438,14 +1441,68 @@ void mjCMesh::Process() {
     mjuu_copyvec(GetPosPtr(type), CoM, 3);
 
     // re-center mesh at CoM
-    if (type==mjINERTIA_VOLUME || validvolume_<=0) {
+    // we only want to do this if the mesh is not already centered at the CoM
+    if (!centered) {
       for (int i=0; i < nvert(); i++) {
         for (int j=0; j<3; j++) {
           vert_[3*i+j] -= CoM[j];
         }
       }
+      centered = true;
     }
 
+    // compute inertia
+    ComputeInertia(type, inert);
+
+    // get quaternion and diagonal inertia
+    double eigval[3], eigvec[9], quattmp[4];
+    double full[9] = {
+      inert[0], inert[3], inert[4],
+      inert[3], inert[1], inert[5],
+      inert[4], inert[5], inert[2]
+    };
+    mjuu_eig3(eigval, eigvec, quattmp, full);
+
+    // check eigval - SHOULD NOT OCCUR
+    if (eigval[2]<=0) {
+      valideigenvalue_ = false;
+      return;
+    }
+    if (eigval[0] + eigval[1] < eigval[2] ||
+        eigval[0] + eigval[2] < eigval[1] ||
+        eigval[1] + eigval[2] < eigval[0]) {
+      validinequality_ = false;
+      return;
+    }
+
+    // compute sizes of equivalent inertia box
+    double mass = GetVolumeRef(type) * density;
+    double* boxsz = GetInertiaBoxPtr(type);
+    boxsz[0] = sqrt(6*(eigval[1]+eigval[2]-eigval[0])/mass)/2;
+    boxsz[1] = sqrt(6*(eigval[0]+eigval[2]-eigval[1])/mass)/2;
+    boxsz[2] = sqrt(6*(eigval[0]+eigval[1]-eigval[2])/mass)/2;
+
+    // if mesh is aligned with inertial frame, we already successfully
+    // computed the volume inertia, so we can copy volume quat to shell,
+    // otherwise use shell quat for coordinate transformations
+    if (aligned_with_inertial_frame) {
+      mjuu_copyvec(GetQuatPtr(type), GetQuatPtr(mjINERTIA_VOLUME), 4);
+    }
+    // rotate vertices and normals to axes of inertia
+    // we only want to do this if the mesh is not already rotated
+    else {
+      mjuu_copyvec(GetQuatPtr(type), quattmp, 4);
+      Rotate(quattmp);
+      aligned_with_inertial_frame = true;
+    }
+  }
+}
+
+void mjCMesh::ComputeInertia(mjtGeomInertia type, double inert[6]) {
+  double nrm[3];
+  double cen[3];
+  double density = model->def_map[classname]->Geom().density;
+
   // accumulate products of inertia, recompute volume
   const int k[6][2] = {{0, 0}, {1, 1}, {2, 2}, {0, 1}, {0, 2}, {1, 2}};
   double P[6] = {0, 0, 0, 0, 0, 0};
@@ -1487,45 +1544,12 @@ void mjCMesh::Process() {
   inert[3] = -P[3];
   inert[4] = -P[4];
   inert[5] = -P[5];
+}
 
-    // get quaternion and diagonal inertia
-    double eigval[3], eigvec[9], quattmp[4];
-    double full[9] = {
-      inert[0], inert[3], inert[4],
-      inert[3], inert[1], inert[5],
-      inert[4], inert[5], inert[2]
-    };
-    mjuu_eig3(eigval, eigvec, quattmp, full);
 
-    // check eigval - SHOULD NOT OCCUR
+void mjCMesh::Rotate(double quat[4]) {
-    if (eigval[2]<=0) {
+  // Rotates vertices and normals of mesh by quaternion.
-      valideigenvalue_ = false;
+  double neg[4] = {quat[0], -quat[1], -quat[2], -quat[3]};
-      return;
-    }
-    if (eigval[0] + eigval[1] < eigval[2] ||
-        eigval[0] + eigval[2] < eigval[1] ||
-        eigval[1] + eigval[2] < eigval[0]) {
-      validinequality_ = false;
-      return;
-    }
-
-    // compute sizes of equivalent inertia box
-    double mass = GetVolumeRef(type) * density;
-    double* boxsz = GetInertiaBoxPtr(type);
-    boxsz[0] = sqrt(6*(eigval[1]+eigval[2]-eigval[0])/mass)/2;
-    boxsz[1] = sqrt(6*(eigval[0]+eigval[2]-eigval[1])/mass)/2;
-    boxsz[2] = sqrt(6*(eigval[0]+eigval[1]-eigval[2])/mass)/2;
-
-    // if volume was valid, copy volume quat to shell and stop,
-    // otherwise use shell quat for coordinate transformations
-    if (type==mjINERTIA_SHELL && validvolume_>0) {
-      mjuu_copyvec(GetQuatPtr(type), GetQuatPtr(mjINERTIA_VOLUME), 4);
-      continue;
-    }
-
-    // rotate vertices and normals into axis-aligned frame
-    mjuu_copyvec(GetQuatPtr(type), quattmp, 4);
-    double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]};
   double mat[9];
   mjuu_quat2mat(mat, neg);
   for (int i=0; i < nvert(); i++) {
@@ -1550,10 +1574,8 @@ void mjCMesh::Process() {
       normal_[3*i+j] = (float) res[j];
     }
   }
-  }
 }
 
-
 // check that the mesh is valid
 void mjCMesh::CheckMesh(mjtGeomInertia type) {
   if (!processed_) {
@@ -1566,9 +1588,9 @@ void mjCMesh::CheckMesh(mjtGeomInertia type) {
                    name.c_str(), invalidorientation_.first, invalidorientation_.second);
   if (!validarea_ && type==mjINERTIA_SHELL)
     throw mjCError(this, "mesh surface area is too small: %s", name.c_str());
-  if (validvolume_<0 && type==mjINERTIA_VOLUME)
+  if (validvolume_==MeshNegativeVolume && type==mjINERTIA_VOLUME)
     throw mjCError(this, "mesh volume is negative (misoriented triangles): %s", name.c_str());
-  if (!validvolume_ && type==mjINERTIA_VOLUME)
+  if (validvolume_==MeshZeroVolume && type==mjINERTIA_VOLUME)
     throw mjCError(this, "mesh volume is too small: %s", name.c_str());
   if (!valideigenvalue_)
     throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str());

src/user/user_objects.cc

@@ -3014,12 +3014,14 @@ void mjCGeom::Compile(void) {
       meshname_.clear();
       mesh = nullptr;
     } else {
+      // Retrieve the mesh position for the relevant inertia type.
       mjuu_copyvec(meshpos, mesh->GetPosPtr(typeinertia), 3);
     }
 
     // apply geom pos/quat as offset
     mjuu_frameaccum(pos, quat, meshpos, pmesh->GetQuatPtr(typeinertia));
     mjuu_copyvec(pmesh->GetOffsetPosPtr(), meshpos, 3);
+    // Retrieve the mesh quaternion for the relevant inertia type.
     mjuu_copyvec(pmesh->GetOffsetQuatPtr(), pmesh->GetQuatPtr(typeinertia), 4);
   }
 

src/user/user_objects.h

@@ -863,7 +863,11 @@ class mjCMesh_ : public mjCBase {
   // mesh properties that indicate a well-formed mesh
   std::pair<int, int> invalidorientation_;    // indices of invalid edge; -1 if none
   bool validarea_;                            // false if the area is too small
-  int validvolume_;                           // 0: volume is too small, -1: volume is negative
+  enum ValidVolume {
+    MeshNegativeVolume = -1,
+    MeshZeroVolume = 0,
+    MeshVolumeOK = 1
+  } validvolume_;                             // indicates if volume is valid
   bool valideigenvalue_;                      // false if inertia eigenvalue is too small
   bool validinequality_;                      // false if inertia inequality is not satisfied
   bool processed_;                            // false if the mesh has not been processed yet
@@ -993,6 +997,10 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
   void ComputeFaceCentroid(double[3]);        // compute centroid of all faces
   void CheckMesh(mjtGeomInertia type);        // check if the mesh is valid
   void CopyPlugin();
+  void Rotate(double quat[4]);                // rotate mesh by quaternion
+
+  // computes the inertia matrix of the mesh given the type of inertia
+  void ComputeInertia(mjtGeomInertia type, double inert[6]);
 
   // mesh data to be copied into mjModel
   double* center_;                    // face circumcenter data (3*nface)