Support multi-contact with meshes in nativeccd. Contacts are pruned to up to 4 per geom.

PiperOrigin-RevId: 730457036
Change-Id: Ib2368f3a7def4fdb94673620df3603e372c35745

src/user/user_mesh.cc

@@ -22,6 +22,7 @@
 #include <cstring>
 #include <functional>
 #include <memory>
+#include <set>
 #include <string>
 #include <string_view>
 #include <unordered_map>
@@ -593,6 +594,8 @@ void mjCMesh::Compile(const mjVFS* vfs) {
     memcpy(facenormal_.data(), face_.data(), 3*nface()*sizeof(int));
   }
 
+  MakePolygons();
+
   // scale, center, orient, compute mass and inertia
   Process();
   processed_ = true;
@@ -602,6 +605,8 @@ void mjCMesh::Compile(const mjVFS* vfs) {
     MakeCenter();
   }
 
+  MakePolygonNormals();
+
   // make bounding volume hierarchy
   if (tree_.Bvh().empty()) {
     face_aabb_.assign(6*nface(), 0);
@@ -707,6 +712,44 @@ void mjCMesh::CopyGraph(int* arr) const {
 
 
 
+void mjCMesh::CopyPolygons(int* verts, int* adr, int* num, int poly_adr) const {
+  int n = polygons_.size(), count = 0;
+  for (int i = 0; i < n; ++i) {
+    int m = num[i] = polygons_[i].size();
+    adr[i] = poly_adr + count;
+    count += m;
+    for (int j = 0; j < m; ++j) {
+      verts[adr[i] + j - poly_adr] = polygons_[i][j];
+    }
+  }
+}
+
+
+
+void mjCMesh::CopyPolygonMap(int* faces, int* adr, int* num, int poly_adr) const {
+  int n = polygon_map_.size(), count = 0;
+  for (int i = 0; i < n; ++i) {
+    int m = num[i] = polygon_map_[i].size();
+    adr[i] = poly_adr + count;
+    count += m;
+    for (int j = 0; j < m; ++j) {
+      faces[adr[i] + j - poly_adr] = polygon_map_[i][j];
+    }
+  }
+}
+
+
+
+void mjCMesh::CopyPolygonNormals(mjtNum* arr) {
+  for (int i = 0; i < polygon_normals_.size(); i += 3) {
+    arr[i + 0] = (mjtNum)polygon_normals_[i + 0];
+    arr[i + 1] = (mjtNum)polygon_normals_[i + 1];
+    arr[i + 2] = (mjtNum)polygon_normals_[i + 2];
+  }
+}
+
+
+
 void mjCMesh::DelTexcoord() {
   texcoord_.clear();
 }
@@ -1997,6 +2040,295 @@ void mjCMesh::MakeCenter(void) {
 
 
 
+// compute the normals of the polygons
+void mjCMesh::MakePolygonNormals() {
+  for (int i = 0; i < polygons_.size(); ++i) {
+  double n[3];
+  mjuu_makenormal(n, &vert_[3*polygons_[i][0]], &vert_[3*polygons_[i][1]],
+                  &vert_[3*polygons_[i][2]]);
+  polygon_normals_[3*i + 0] = n[0];
+  polygon_normals_[3*i + 1] = n[1];
+  polygon_normals_[3*i + 2] = n[2];
+  }
+}
+
+
+
+// helper class to compute the polygons of a mesh
+class MeshPolygon {
+ public:
+  // constructors (need starting face)
+  MeshPolygon(const float v1[3], const float v2[3], const float v3[3],
+              int v1i, int v2i, int v3i);
+  MeshPolygon() = delete;
+  MeshPolygon(const MeshPolygon&) = delete;
+  MeshPolygon& operator=(const MeshPolygon&) = delete;
+
+  void InsertFace(int v1, int v2, int v3);          // insert a face into the polygon
+  std::vector<std::vector<int>> Paths() const;      // return trace of the polygons
+  const double* Normal() const { return normal_; }  // return the normal of the polygon
+
+  // return the ith component of the normal of the polygon
+  double Normal(int i) const { return normal_[i]; }
+
+ private:
+  std::vector<std::pair<int, int>> edges_;
+
+  // inserted faces do not necessarily share edges with the current polygon, so they're grouped as
+  // islands until they can be combined with later face insertions
+  std::vector<int> islands_;
+  int nisland_ = 0;
+  double normal_[3] = {0.0, 0.0, 0.0};
+  void CombineIslands(int& island1, int& island2);
+};
+
+
+
+MeshPolygon::MeshPolygon(const float v1[3], const float v2[3], const float v3[3],
+                         int v1i, int v2i, int v3i) {
+  mjuu_makenormal(normal_, v1, v2, v3);
+  edges_ = {{v1i, v2i}, {v2i, v3i}, {v3i, v1i}};
+  nisland_ = 1;
+  islands_ = {0, 0, 0};
+}
+
+
+
+// comparison operator for std::set
+bool PolygonCmp(const MeshPolygon& p1, const MeshPolygon& p2)  {
+  const double* n1 = p1.Normal();
+  const double* n2 = p2.Normal();
+  double dot3 = n1[0] * n2[0] + n1[1] * n2[1] + n1[2] * n2[2];
+
+  // TODO(kylebayes): The tolerance should be a parameter set the user, as it should be optimized
+  // from mesh to mesh.
+  if (dot3 > 0.99999872) {
+    return false;
+  }
+
+  if (std::abs(n1[0] - n2[0]) > mjMINVAL) {
+    return n1[0] > n2[0];
+  }
+  if (std::abs(n1[1] - n2[1]) > mjMINVAL) {
+    return n1[1] > n2[1];
+  }
+  if (std::abs(n1[2] - n2[2]) > mjMINVAL) {
+    return n1[2] > n2[2];
+  }
+  return false;
+}
+
+
+
+// combine two islands when a newly inserted face connects them
+void MeshPolygon::CombineIslands(int& island1, int& island2) {
+  // pick the smaller island
+  if (island2 < island1) {
+    int tmp = island1;
+    island1 = island2;
+    island2 = tmp;
+  }
+
+  // renumber the islands
+  for (int k = 0; k < islands_.size(); ++k) {
+    if (islands_[k] == island2) {
+      islands_[k] = island1;
+    } else if (islands_[k] > island2) {
+      islands_[k]--;
+    }
+  }
+}
+
+
+
+// insert a triangular face into the polygon
+void MeshPolygon::InsertFace(int v1, int v2, int v3) {
+  int add1 = 1, add2 = 1, add3 = 1;
+  int island = -1;
+
+  // check if face can be attached via edge v1v2
+  for (int i = 0; i < edges_.size(); ++i) {
+    if (edges_[i].first == v2 && edges_[i].second == v1) {
+      add1 = 0;
+      island = islands_[i];
+      edges_.erase(edges_.begin() + i);
+      islands_.erase(islands_.begin() + i);
+      break;
+    }
+  }
+
+  // check if face can be attached via edge v2v3
+  for (int i = 0; i < edges_.size(); ++i) {
+    if (edges_[i].first == v3 && edges_[i].second == v2) {
+      int island2 = islands_[i];
+      if (island == -1) {
+        island = island2;
+      } else if (island2 != island) {
+        nisland_--;
+        CombineIslands(island, island2);
+      }
+      add2 = 0;
+      edges_.erase(edges_.begin() + i);
+      islands_.erase(islands_.begin() + i);
+      break;
+    }
+  }
+
+  // check if face can be attached via edge v3v1
+  for (int i = 0; i < edges_.size(); ++i) {
+    if (edges_[i].first == v1 && edges_[i].second == v3) {
+      int island3 = islands_[i];
+      if (island == -1) {
+        island = island3;
+      } else if (island3 != island) {
+        nisland_--;
+        CombineIslands(island, island3);
+      }
+      add3 = 0;
+      edges_.erase(edges_.begin() + i);
+      islands_.erase(islands_.begin() + i);
+      break;
+    }
+  }
+
+  if (island == -1) {
+    island = nisland_++;
+  }
+
+  // add only new edges to the polygon
+
+  if (add1) {
+    edges_.push_back({v1, v2});
+    islands_.push_back(island);
+  }
+  if (add2) {
+    edges_.push_back({v2, v3});
+    islands_.push_back(island);
+  }
+  if (add3) {
+    edges_.push_back({v3, v1});
+    islands_.push_back(island);
+  }
+}
+
+
+
+// return the traverse vertices of the polygon; there may be multiple paths if the polygon is
+// not connected
+std::vector<std::vector<int>> MeshPolygon::Paths() const {
+  std::vector<std::vector<int>> paths;
+  // shortcut if polygon is just a triangular face
+  if (edges_.size() == 3) {
+    return {{edges_[0].first, edges_[1].first, edges_[2].first}};
+  }
+
+  // go through each connected component of the polygon
+  for (int i = 0; i < nisland_; ++i) {
+    std::vector<int> path;
+
+    // find starting vertex
+    for (int j = 0; j < edges_.size(); ++j) {
+      if (islands_[j] == i) {
+        path.push_back(edges_[j].first);
+        path.push_back(edges_[j].second);
+        break;
+      }
+    }
+
+    // SHOULD NOT OCCUR (See logic in MeshPolygon::CombineIslands)
+    if (path.empty()) {
+      continue;
+    }
+
+    // visit the next vertex given the current edge
+    int next = path.back();
+    for (int l = 0; l < edges_.size(); ++l) {
+      int finished = 0;
+      for (int k = 1; k < edges_.size(); ++k) {
+        if (islands_[k] == i && edges_[k].first == next) {
+          next = edges_[k].second;
+          if (next == path[0]) {
+            paths.push_back(path);
+            finished = 1;
+            break;
+          }
+          path.push_back(next);
+          break;
+       }
+      }
+
+      // back at start
+      if (finished) {
+        break;
+      }
+    }
+  }
+  return paths;
+}
+
+
+
+// merge coplanar mesh triangular faces into polygonal sides to represent the geometry of the mesh
+void mjCMesh::MakePolygons() {
+  std::set<MeshPolygon, decltype(PolygonCmp)*> polygons(PolygonCmp);
+  polygons_.clear();
+  polygon_normals_.clear();
+  polygon_map_.clear();
+
+  // initialize polygon map
+  for (int i = 0; i < nvert(); i++) {
+    polygon_map_.push_back(std::vector<int>());
+  }
+
+  // use graph data if available
+  int *faces, nfaces;
+  if (graph_) {
+    int nvert = graph_[0];
+    nfaces = graph_[1];
+    faces = graph_ + 2 + 3*nvert + 3*nfaces;
+  } else {
+    nfaces = nface();
+    faces = face_.data();
+  }
+
+  // process each face
+  for (int i = 0; i < nfaces; i++) {
+    float* v1 = &vert_[3*faces[3*i + 0]];
+    float* v2 = &vert_[3*faces[3*i + 1]];
+    float* v3 = &vert_[3*faces[3*i + 2]];
+
+    MeshPolygon face(v1, v2, v3, faces[3*i + 0], faces[3*i + 1], faces[3*i + 2]);
+    auto it = polygons.find(face);
+    if (it == polygons.end()) {
+      polygons.emplace(v1, v2, v3, faces[3*i + 0], faces[3*i + 1], faces[3*i + 2]);
+    } else {
+      MeshPolygon& p = const_cast<MeshPolygon&>(*it);
+      p.InsertFace(faces[3*i + 0], faces[3*i + 1], faces[3*i + 2]);
+    }
+  }
+
+  for (const auto& polygon : polygons) {
+    std::vector<std::vector<int>> paths = polygon.Paths();
+
+    // separate the polygons if they were grouped together
+    for (const auto& path : paths) {
+      if (path.size() < 3) continue;
+      polygons_.push_back(path);
+      polygon_normals_.push_back(polygon.Normal(0));
+      polygon_normals_.push_back(polygon.Normal(1));
+      polygon_normals_.push_back(polygon.Normal(2));
+    }
+  }
+
+  // populate the polygon map
+  for (int i = 0; i < polygons_.size(); i++) {
+    for (int j = 0; j < polygons_[i].size(); ++j) {
+      polygon_map_[polygons_[i][j]].push_back(i);
+    }
+  }
+}
+
+
 //------------------ class mjCSkin implementation --------------------------------------------------
 
 // constructor

src/user/user_model.cc

@@ -837,6 +837,9 @@ void mjCModel::Clear() {
   nmeshtexcoord = 0;
   nmeshface = 0;
   nmeshgraph = 0;
+  nmeshpoly = 0;
+  nmeshpolyvert = 0;
+  nmeshpolymap = 0;
   nskinvert = 0;
   nskintexvert = 0;
   nskinface = 0;
@@ -1752,6 +1755,9 @@ void mjCModel::SetSizes() {
     nmeshface += meshes_[i]->nface();
     nmeshtexcoord += (meshes_[i]->HasTexcoord() ? meshes_[i]->ntexcoord() : 0);
     nmeshgraph += meshes_[i]->szgraph();
+    nmeshpoly += meshes_[i]->npolygon();
+    nmeshpolyvert += meshes_[i]->npolygonvert();
+    nmeshpolymap += meshes_[i]->npolygonmap();
   }
 
   // skin counts
@@ -2755,6 +2761,7 @@ void mjCModel::CopyObjects(mjModel* m) {
   int adr, bone_adr, vert_adr, node_adr, normal_adr, face_adr, texcoord_adr;
   int edge_adr, elem_adr, elemdata_adr, elemedge_adr, shelldata_adr, evpair_adr;
   int bonevert_adr, graph_adr, data_adr, bvh_adr;
+  int poly_adr, polymap_adr, polyvert_adr;
 
   // sizes outside call to mj_makeModel
   m->nemax = nemax;
@@ -2776,19 +2783,16 @@ void mjCModel::CopyObjects(mjModel* m) {
   texcoord_adr = 0;
   face_adr = 0;
   graph_adr = 0;
+  poly_adr = 0;
+  polyvert_adr = 0;
+  polymap_adr = 0;
   for (int i=0; i<nmesh; i++) {
     // get pointer
     mjCMesh* pme = meshes_[i];
 
-    // zero out multi-ccd fields
-    for (int j = 0; j < pme->nvert(); j++) {
-      m->mesh_polymapadr[vert_adr + j] = 0;
-      m->mesh_polymapnum[vert_adr + j] = 0;
-    }
-    m->mesh_polynum[i] = 0;
-    m->mesh_polyadr[i] = 0;
-
     // set fields
+    m->mesh_polyadr[i] = poly_adr;
+    m->mesh_polynum[i] = pme->npolygon();
     m->mesh_vertadr[i] = vert_adr;
     m->mesh_vertnum[i] = pme->nvert();
     m->mesh_normaladr[i] = normal_adr;
@@ -2818,6 +2822,11 @@ void mjCModel::CopyObjects(mjModel* m) {
     if (pme->szgraph()) {
       pme->CopyGraph(m->mesh_graph + graph_adr);
     }
+    pme->CopyPolygonNormals(m->mesh_polynormal + 3*poly_adr);
+    pme->CopyPolygons(m->mesh_polyvert + polyvert_adr, m->mesh_polyvertadr + poly_adr,
+                      m->mesh_polyvertnum + poly_adr, polyvert_adr);
+    pme->CopyPolygonMap(m->mesh_polymap + polymap_adr, m->mesh_polymapadr + vert_adr,
+                        m->mesh_polymapnum + vert_adr, polymap_adr);
 
     // copy bvh data
     if (pme->tree().Nbvh()) {
@@ -2830,6 +2839,9 @@ void mjCModel::CopyObjects(mjModel* m) {
     }
 
     // advance counters
+    poly_adr += pme->npolygon();
+    polyvert_adr += pme->npolygonvert();
+    polymap_adr += pme->npolygonmap();
     vert_adr += pme->nvert();
     normal_adr += pme->nnormal();
     texcoord_adr += (pme->HasTexcoord() ? pme->ntexcoord() : 0);
@@ -4266,7 +4278,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
   SetNuser();
 
   // compile meshes (needed for geom compilation)
-  if (compiler.usethread && meshes_.size() > 1) {
+  if (!compiler.usethread && meshes_.size() > 1) {
     // multi-threaded mesh compile
     CompileMeshes(vfs);
   } else {
@@ -4340,9 +4352,9 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
                nq, nv, nu, na, nbody, nbvh, nbvhstatic, nbvhdynamic, njnt, ngeom, nsite,
                ncam, nlight, nflex, nflexnode, nflexvert, nflexedge, nflexelem,
                nflexelemdata, nflexelemedge, nflexshelldata, nflexevpair, nflextexcoord,
-               nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph, 0, 0,
-               0, nskin, nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert,
-               nhfield, nhfielddata, ntex, ntexdata, nmat, npair, nexclude,
+               nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph, nmeshpoly,
+               nmeshpolyvert, nmeshpolymap, nskin, nskinvert, nskintexvert, nskinface, nskinbone,
+               nskinbonevert, nhfield, nhfielddata, ntex, ntexdata, nmat, npair, nexclude,
                neq, ntendon, nwrap, nsensor, nnumeric, nnumericdata, ntext, ntextdata,
                ntuple, ntupledata, nkey, nmocap, nplugin, npluginattr,
                nuser_body, nuser_jnt, nuser_geom, nuser_site, nuser_cam,

src/user/user_model.h

@@ -99,7 +99,10 @@ class mjCModel_ : public mjsElement {
   int nmeshnormal;     // number of normals in all meshes
   int nmeshtexcoord;   // number of texture coordinates in all meshes
   int nmeshface;       // number of triangular faces in all meshes
-  int nmeshgraph;      // number of shorts in mesh auxiliary data
+  int nmeshpoly;       // number of polygon faces in all meshes
+  int nmeshgraph;      // number of ints in mesh auxiliary data
+  int nmeshpolyvert;   // number of vertices in all polygon faces
+  int nmeshpolymap;    // number of polygons in vertex map
   int nskinvert;       // number of vertices in all skins
   int nskintexvert;    // number of vertices with texcoord in all skins
   int nskinface;       // number of faces in all skins

src/user/user_objects.h

@@ -942,6 +942,21 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
   int nnormal() const { return normal_.size()/3; }
   int ntexcoord() const { return texcoord_.size()/2; }
   int nface() const { return face_.size()/3; }
+  int npolygon() const { return polygons_.size(); }
+  int npolygonvert() const {
+    int acc = 0;
+    for (const auto& polygon : polygons_) {
+      acc += polygon.size();
+    }
+    return acc;
+  }
+  int npolygonmap() const {
+    int acc = 0;
+    for (const auto& polygon : polygon_map_) {
+      acc += polygon.size();
+    }
+    return acc;
+  }
 
   // return size of graph data in ints
   int szgraph() const { return szgraph_; }
@@ -968,6 +983,15 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
   void CopyTexcoord(float* arr) const;              // copy texcoord data into array
   void CopyGraph(int* arr) const;                   // copy graph data into array
 
+  // copy polygon data into array
+  void CopyPolygons(int* verts, int* adr, int* num, int poly_adr) const;
+
+  // copy polygon map data into array
+  void CopyPolygonMap(int *faces, int* adr, int* num, int poly_adr) const;
+
+  // copy polygon normal data into array
+  void CopyPolygonNormals(mjtNum* arr);
+
   // sets properties of a bounding volume given a face id
   void SetBoundingVolume(int faceid);
 
@@ -996,6 +1020,8 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
   void CopyPlugin();
   void Rotate(double quat[4]);                      // rotate mesh by quaternion
   void Transform(double pos[3], double quat[4]);    // transform mesh by position and quaternion
+  void MakePolygons();                              // compute the polygon sides of the mesh
+  void MakePolygonNormals();                        // compute the normals of the polygons
 
   // computes the inertia matrix of the mesh given the type of inertia
   void ComputeInertia(double inert[6], double CoM[3]);
@@ -1004,6 +1030,11 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
   double* center_;                    // face circumcenter data (3*nface)
   int* graph_;                        // convex graph data
 
+  // mesh data for collision detection
+  std::vector<std::vector<int>> polygons_;      // polygons of the mesh
+  std::vector<double> polygon_normals_;         // normals of the polygons
+  std::vector<std::vector<int>> polygon_map_;   // map from vertex to polygon
+
   // for caching purposes
   std::vector<int> vertex_index_;
   std::vector<int> normal_index_;

src/user/user_util.cc

@@ -360,17 +360,23 @@ void mjuu_crossvec(double* a, const double* b, const double* c) {
 
 // compute normal vector to given triangle, return length
 double mjuu_makenormal(double* normal, const float* a, const float* b, const float* c) {
-  double v1[3] = {b[0]-a[0], b[1]-a[1], b[2]-a[2]};
-  double v2[3] = {c[0]-a[0], c[1]-a[1], c[2]-a[2]};
-  double res;
+  double v1[3] = {a[0], a[1], a[2]};
+  double v2[3] = {b[0], b[1], b[2]};
+  double v3[3] = {c[0], c[1], c[2]};
+  double diffAB[3] = {v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2]};
+  double diffAC[3] = {v3[0]-v1[0], v3[1]-v1[1], v3[2]-v1[2]};
 
-  mjuu_crossvec(normal, v1, v2);
-  if ((res=mjuu_normvec(normal, 3)) < mjEPS) {
-    normal[0] = normal[1] = 0;
-    normal[2] = 1;
+  mjuu_crossvec(normal, diffAB, diffAC);
+  double nrm = std::sqrt(mjuu_dot3(normal, normal));
+  if (nrm < mjEPS) {
+    normal[0] = 1;
+    normal[1] = 0;
+    normal[2] = 0;
   }
-
-  return res;
+  normal[0] /= nrm;
+  normal[1] /= nrm;
+  normal[2] /= nrm;
+  return nrm;
 }