Auto-generate 3D mesh from convex OBJ if dim=3 in flex.

This requires the user to explicitly specify an origin.

PiperOrigin-RevId: 712483526
Change-Id: Ie316a5eccb676bb918d4bb746795019145c10f5b

src/user/user_flexcomp.cc

@@ -940,8 +940,8 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) {
   }
 
   // check dim
-  if (def.spec.flex->dim != 2) {
-    return comperr(error, "Flex dim must be 2 in for mesh", error_sz);
+  if (def.spec.flex->dim < 2) {
+    return comperr(error, "Flex dim must be at least 2 for mesh", error_sz);
   }
 
   // load resource
@@ -990,15 +990,42 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) {
     mesh.RemoveRepeated();
   }
 
-  // copy faces
-  element = mesh.Face();
-
   // copy vertices, convert from float to double
   point = vector<double> (mesh.nvert()*3);
   for (int i=0; i < mesh.nvert()*3; i++) {
     point[i] = (double) mesh.Vert(i);
   }
 
+  // copy faces or create 3D mesh
+  if (def.spec.flex->dim == 2) {
+    element = mesh.Face();
+  } else {
+    point.insert(point.begin() + 0, origin[0]);
+    point.insert(point.begin() + 1, origin[1]);
+    point.insert(point.begin() + 2, origin[2]);
+    for (int i=0; i < mesh.Face().size(); i+=3) {
+      // only add tetrahedra with positive volume
+      int tet[3] = {mesh.Face()[i+0]+1,
+                    mesh.Face()[i+1]+1,
+                    mesh.Face()[i+2]+1};
+      double edge1[3], edge2[3], edge3[3];
+      for (int i=0; i < 3; i++) {
+        edge1[i] = point[3*tet[0]+i] - origin[i];
+        edge2[i] = point[3*tet[1]+i] - origin[i];
+        edge3[i] = point[3*tet[2]+i] - origin[i];
+      }
+      double normal[3];
+      mjuu_crossvec(normal, edge1, edge2);
+      if (mjuu_dot3(normal, edge3) < mjMINVAL) {
+        continue;
+      }
+      element.push_back(0);
+      element.push_back(tet[0]);
+      element.push_back(tet[1]);
+      element.push_back(tet[2]);
+    }
+  }
+
   return true;
 }
 

src/user/user_flexcomp.h

@@ -68,6 +68,7 @@ class mjCFlexcomp {
   int count[3];                   // grid count in each dimension
   double spacing[3];              // spacing between grid elements
   double scale[3];                // scaling for mesh and direct
+  double origin[3];               // origin for generating a 3D mesh from a convex 2D mesh
   double mass;                    // total mass of auto-generated bodies
   double inertiabox;              // size of inertia box for each body
   bool equality;                  // create edge equality constraint

src/xml/xml_native_reader.cc

@@ -311,10 +311,10 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
               {"config", "*", "2", "key", "value"},
             {">"},
         {">"},
-        {"flexcomp", "*", "24", "name", "type", "group", "dim",
+        {"flexcomp", "*", "25", "name", "type", "group", "dim",
             "count", "spacing", "radius", "rigid", "mass", "inertiabox",
             "scale", "file", "point", "element", "texcoord", "material", "rgba",
-            "flatskin", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler"},
+            "flatskin", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler", "origin"},
         {"<"},
             {"edge", "?", "5", "equality", "solref", "solimp", "stiffness", "damping"},
             {"elasticity", "?", "4", "young", "poisson", "damping", "thickness"},
@@ -2643,6 +2643,10 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) {
   ReadAttrInt(elem, "dim", &dflex.dim);
   ReadAttr(elem, "radius", 1, &dflex.radius, text);
   ReadAttrInt(elem, "group", &dflex.group);
+  if (!ReadAttr(elem, "origin", 3, fcomp.origin, text) &&
+      fcomp.type == mjFCOMPTYPE_MESH && dflex.dim == 3) {
+    throw mjXError(elem, "origin must be specified for mesh flexcomps if dim=3");
+  }
 
   // pose
   ReadAttr(elem, "pos", 3, fcomp.pos, text);