Use the geom frame for SDF collisions.

Also disables mesh reorientation for meshes autogenerated with marching cubes from analytic SDFs.

PiperOrigin-RevId: 781487195
Change-Id: I36b66384f240a764823c489f78f22ba9fe32f15f

src/user/user_mesh.cc

@@ -141,6 +141,7 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
   processed_ = false;
   visual_ = true;
   needoct_ = false;
+  needreorient_ = true;
 
   // reset to default if given
   if (_def) {
@@ -360,6 +361,7 @@ void mjCMesh::LoadSDF() {
     userface.push_back(index);
   }
 
+  needreorient_ = false;
   normal_ = std::move(usernormal);
   face_ = std::move(userface);
   ProcessVertices(uservert);
@@ -1533,22 +1535,26 @@ void mjCMesh::Process() {
   boxsz_[1] = 0.5 * std::sqrt(6*(eigval[0] + eigval[2] - eigval[1])/volume);
   boxsz_[2] = 0.5 * std::sqrt(6*(eigval[0] + eigval[1] - eigval[2])/volume);
 
-  // make octree in the geom frame
-  // TODO: make octree in the mesh frame, update engine_collision_sdf
+  // prevent reorientation if the mesh was autogenerated using marching cubes
+  if (!needreorient_) {
+    mjuu_setvec(CoM, 0, 0, 0);
+    mjuu_setvec(quattmp, 1, 0, 0, 0);
+  }
+
+  // transform CoM to origin
+  for (int i=0; i < nvert(); i++) {
+    vert_[3*i + 0] -= CoM[0];
+    vert_[3*i + 1] -= CoM[1];
+    vert_[3*i + 2] -= CoM[2];
+  }
+  Rotate(quattmp);
+
+  // make octree in mesh frame
   if (!needoct_) {
     octree_.Clear();
   } else if (octree_.Nodes().empty()) {
-    double aamm[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
-    for (int i = 0; i < nvert(); i++) {
-      aamm[0] = std::min(aamm[0], vert_[3*i + 0]);
-      aamm[3] = std::max(aamm[3], vert_[3*i + 0]);
-      aamm[1] = std::min(aamm[1], vert_[3*i + 1]);
-      aamm[4] = std::max(aamm[4], vert_[3*i + 1]);
-      aamm[2] = std::min(aamm[2], vert_[3*i + 2]);
-      aamm[5] = std::max(aamm[5], vert_[3*i + 2]);
-    }
     octree_.SetFace(vert_, face_);
-    octree_.CreateOctree(aamm);
+    octree_.CreateOctree(aamm_);
 
     // compute sdf coefficients
     if (!plugin.active) {
@@ -1569,14 +1575,6 @@ void mjCMesh::Process() {
     }
   }
 
-  // transform CoM to origin
-  for (int i=0; i < nvert(); i++) {
-    vert_[3*i + 0] -= CoM[0];
-    vert_[3*i + 1] -= CoM[1];
-    vert_[3*i + 2] -= CoM[2];
-  }
-  Rotate(quattmp);
-
   // save the pos and quat that was used to transform the mesh
   mjuu_copyvec(pos_, CoM, 3);
   mjuu_copyvec(quat_, quattmp, 4);

src/user/user_objects.h

@@ -1003,6 +1003,7 @@ class mjCMesh_ : public mjCBase {
   std::vector<int> spec_facetexcoord_;
 
   // used by the compiler
+  bool needreorient_;                            // needs reorientation
   bool needoct_;                                 // needs octree
   bool visual_;                                  // true: the mesh is only visual
   std::vector< std::pair<int, int> > halfedge_;  // half-edge data